diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-05-20 19:47:38 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-05-20 19:47:38 +0400 |
commit | 5842c2212334876979f329b9b6bceba9609d91af (patch) | |
tree | 9eec1b7539f17b6a4b02ae01ca8468b1af0a5df5 /src/modules/position_estimator_inav/position_estimator_inav_main.c | |
parent | e8ec2ed93a22089ea8e5190368a78e26aa80fc0b (diff) | |
download | px4-firmware-5842c2212334876979f329b9b6bceba9609d91af.tar.gz px4-firmware-5842c2212334876979f329b9b6bceba9609d91af.tar.bz2 px4-firmware-5842c2212334876979f329b9b6bceba9609d91af.zip |
Use GPS velocity in position 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 | 23 |
1 files changed, 13 insertions, 10 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 2b485f895..6ecdfe01d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -370,7 +370,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (use_z[0] || use_z[1]) { /* correction */ - kalman_filter_inertial_update(z_est, z_meas, params.k_alt, use_z); + kalman_filter_inertial2_update(z_est, z_meas, params.k_alt, use_z); } if (params.use_gps) { @@ -378,23 +378,26 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { kalman_filter_inertial_predict(dt, x_est); kalman_filter_inertial_predict(dt, y_est); /* prepare vectors for kalman filter correction */ - float x_meas[2]; // position, acceleration - float y_meas[2]; // position, acceleration - bool use_xy[2] = { false, false }; + float x_meas[3]; // position, velocity, acceleration + float y_meas[3]; // position, velocity, acceleration + bool use_xy[3] = { false, false, false }; if (gps_updated) { x_meas[0] = local_pos_gps[0]; y_meas[0] = local_pos_gps[1]; + x_meas[1] = gps.vel_n_m_s; + y_meas[1] = gps.vel_e_m_s; use_xy[0] = true; + use_xy[1] = true; } if (accelerometer_updated) { - x_meas[1] = accel_NED[0]; - y_meas[1] = accel_NED[1]; - use_xy[1] = true; + x_meas[2] = accel_NED[0]; + y_meas[2] = accel_NED[1]; + use_xy[2] = true; } - if (use_xy[0] || use_xy[1]) { + if (use_xy[0] || use_xy[2]) { /* correction */ - kalman_filter_inertial_update(x_est, x_meas, params.k_pos, use_xy); - kalman_filter_inertial_update(y_est, y_meas, params.k_pos, use_xy); + kalman_filter_inertial3_update(x_est, x_meas, params.k_pos, use_xy); + kalman_filter_inertial3_update(y_est, y_meas, params.k_pos, use_xy); } } } |