diff options
| author | Denis Yeldandi <dyeldandi@gramant.ru> | 2014-10-17 21:07:34 +0400 |
|---|---|---|
| committer | Denis Yeldandi <dyeldandi@gramant.ru> | 2014-10-17 21:07:34 +0400 |
| commit | 22f1b784525622ac39c9db50675c3937c7aeecf0 (patch) | |
| tree | dcd11d546db12670426229497c7d5eccc9ff666f /src/modules/position_estimator_inav/position_estimator_inav_main.c | |
| parent | 111a8745b3d9d4ab99e4e0d34a9b9ddd3cd1ecdb (diff) | |
| download | px4-firmware-22f1b784525622ac39c9db50675c3937c7aeecf0.tar.gz px4-firmware-22f1b784525622ac39c9db50675c3937c7aeecf0.tar.bz2 px4-firmware-22f1b784525622ac39c9db50675c3937c7aeecf0.zip | |
Adding vertical (Z) velocity to inav estimator
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 81bbaa658..a1bfa2dfc 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -846,6 +846,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; @@ -876,6 +877,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 */ @@ -960,6 +962,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) { |