diff options
Diffstat (limited to 'src/modules/position_estimator_inav/position_estimator_inav_main.c')
| -rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_main.c | 3 |
1 files changed, 3 insertions, 0 deletions
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 6de283396..ec297e7f0 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -877,6 +877,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy; float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy; float w_z_gps_p = params.w_z_gps_p * w_gps_z; + float w_z_gps_v = params.w_z_gps_v * w_gps_z; float w_xy_vision_p = params.w_xy_vision_p; float w_xy_vision_v = params.w_xy_vision_v; @@ -907,6 +908,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (use_gps_z) { accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p; + accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v; } /* transform error vector from NED frame to body frame */ @@ -991,6 +993,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) epv = fminf(epv, gps.epv); inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); + inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v); } if (use_vision_z) { |