diff options
| author | Lorenz Meier <lorenz@px4.io> | 2015-01-23 20:26:50 +0100 |
|---|---|---|
| committer | Lorenz Meier <lorenz@px4.io> | 2015-01-23 20:26:50 +0100 |
| commit | ba54deefc929ca670ae725cb772a8fe574f74742 (patch) | |
| tree | d54bac920277192c9df8e65308b9a4c3a14e943c /src/modules/position_estimator_inav/position_estimator_inav_main.c | |
| parent | b14e849fc4bcf81bbffd47064e2850a929652f6d (diff) | |
| parent | 0632d882bb734ac52ff5f7f5d1dc7930046e4600 (diff) | |
| download | px4-firmware-ba54deefc929ca670ae725cb772a8fe574f74742.tar.gz px4-firmware-ba54deefc929ca670ae725cb772a8fe574f74742.tar.bz2 px4-firmware-ba54deefc929ca670ae725cb772a8fe574f74742.zip | |
Merge pull request #1397 from dyeldandi/vz
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 39294e3c0..86764d620 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -872,6 +872,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; @@ -902,6 +903,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 */ @@ -986,6 +988,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) { |