From e413ae7cbc1c5bbf9acafe6c23602e16cdc11121 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 29 Oct 2013 09:14:52 +0400 Subject: [PATCH 1/2] attitude_estimator_ekf: acceleration compensation based on GPS velocity --- .../attitude_estimator_ekf_main.cpp | 47 +++++++++++++++++-- 1 file changed, 44 insertions(+), 3 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index a70a14fe4b..c0f7a5bdb6 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -58,6 +58,7 @@ #include #include #include +#include #include #include @@ -214,6 +215,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); + struct vehicle_gps_position_s gps; + memset(&gps, 0, sizeof(gps)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_control_mode_s control_mode; @@ -222,11 +225,16 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds uint64_t last_data = 0; uint64_t last_measurement = 0; + float vel_prev[3] = { 0.0f, 0.0f, 0.0f }; + /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); /* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */ orb_set_interval(sub_raw, 3); + /* subscribe to GPS */ + int sub_gps = orb_subscribe(ORB_ID(vehicle_gps_position)); + /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); @@ -306,6 +314,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* get latest measurements */ orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); + orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps); if (!initialized) { // XXX disabling init for now @@ -352,9 +361,41 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds sensor_last_timestamp[1] = raw.timestamp; } - z_k[3] = raw.accelerometer_m_s2[0]; - z_k[4] = raw.accelerometer_m_s2[1]; - z_k[5] = raw.accelerometer_m_s2[2]; + float acc[3]; + if (gps.fix_type >= 3 && gps.eph_m < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) { + /* calculate acceleration in NED frame */ + float acc_NED[3]; + acc_NED[0] = (gps.vel_n_m_s - vel_prev[0]) / dt; + acc_NED[1] = (gps.vel_e_m_s - vel_prev[1]) / dt; + acc_NED[2] = (gps.vel_d_m_s - vel_prev[2]) / dt; + + /* project acceleration to body frame */ + for (int i = 0; i < 3; i++) { + acc[i] = 0.0f; + for (int j = 0; j < 3; j++) { + acc[i] += att.R[j][i] * acc_NED[j]; + } + } + + } else { + acc[0] = 0.0f; + acc[1] = 0.0f; + acc[2] = 0.0f; + } + + if (gps.vel_ned_valid) { + vel_prev[0] = gps.vel_n_m_s; + vel_prev[1] = gps.vel_e_m_s; + vel_prev[2] = gps.vel_d_m_s; + } else { + vel_prev[0] = 0.0f; + vel_prev[1] = 0.0f; + vel_prev[2] = 0.0f; + } + + z_k[3] = raw.accelerometer_m_s2[0] - acc[0]; + z_k[4] = raw.accelerometer_m_s2[1] - acc[1]; + z_k[5] = raw.accelerometer_m_s2[2] - acc[2]; /* update magnetometer measurements */ if (sensor_last_count[2] != raw.magnetometer_counter) { From a770bd5cf3d458baa85675815541512efce99b56 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 2 Nov 2013 18:59:55 +0400 Subject: [PATCH 2/2] attitude_estimator_ekf: correct acceleration continuously, not only on GPS updates --- .../attitude_estimator_ekf_main.cpp | 26 ++++++++++++------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index c0f7a5bdb6..efa6d97fdd 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -224,6 +224,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds uint64_t last_data = 0; uint64_t last_measurement = 0; + uint64_t last_gps = 0; float vel_prev[3] = { 0.0f, 0.0f, 0.0f }; @@ -363,17 +364,20 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds float acc[3]; if (gps.fix_type >= 3 && gps.eph_m < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) { - /* calculate acceleration in NED frame */ - float acc_NED[3]; - acc_NED[0] = (gps.vel_n_m_s - vel_prev[0]) / dt; - acc_NED[1] = (gps.vel_e_m_s - vel_prev[1]) / dt; - acc_NED[2] = (gps.vel_d_m_s - vel_prev[2]) / dt; + if (last_gps != 0 && gps.timestamp_velocity != last_gps) { + float gps_dt = (gps.timestamp_velocity - last_gps) / 1000000.0f; + /* calculate acceleration in NED frame */ + float acc_NED[3]; + acc_NED[0] = (gps.vel_n_m_s - vel_prev[0]) / gps_dt; + acc_NED[1] = (gps.vel_e_m_s - vel_prev[1]) / gps_dt; + acc_NED[2] = (gps.vel_d_m_s - vel_prev[2]) / gps_dt; - /* project acceleration to body frame */ - for (int i = 0; i < 3; i++) { - acc[i] = 0.0f; - for (int j = 0; j < 3; j++) { - acc[i] += att.R[j][i] * acc_NED[j]; + /* project acceleration to body frame */ + for (int i = 0; i < 3; i++) { + acc[i] = 0.0f; + for (int j = 0; j < 3; j++) { + acc[i] += att.R[j][i] * acc_NED[j]; + } } } @@ -387,10 +391,12 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds vel_prev[0] = gps.vel_n_m_s; vel_prev[1] = gps.vel_e_m_s; vel_prev[2] = gps.vel_d_m_s; + last_gps = gps.timestamp_velocity; } else { vel_prev[0] = 0.0f; vel_prev[1] = 0.0f; vel_prev[2] = 0.0f; + last_gps = 0; } z_k[3] = raw.accelerometer_m_s2[0] - acc[0];