From aaa9af2293b8496363451595d03aebcc0f5e82c3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Jan 2013 00:58:35 +0100 Subject: [PATCH] Reverting EKF change, as it did not really help. --- .../attitude_estimator_ekf_main.c | 27 ++----------------- 1 file changed, 2 insertions(+), 25 deletions(-) diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 685ab078b2..6533390bc0 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -65,7 +65,6 @@ #include #include #include -#include #include "codegen/attitudeKalmanfilter_initialize.h" #include "codegen/attitudeKalmanfilter.h" @@ -255,7 +254,6 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) bool initialized = false; float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; - float accel_magnitude = CONSTANTS_ONE_G; unsigned offset_count = 0; /* register the perf counter */ @@ -331,34 +329,13 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) z_k[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; - /* - * Check if assumption of zero acceleration roughly holds. - * If not, do not update the accelerometer, predict only with gyroscopes. - * The violation of the zero acceleration assumption can only hold shortly, - * and predicting the angle with the gyros is safe for - */ - bool acceleration_stationary = true; - - /* lowpass accel magnitude to prevent threshold aliasing in presence of vibrations */ - accel_magnitude = accel_magnitude * 0.95f + 0.05f * sqrtf(raw.accelerometer_m_s2[0] * raw.accelerometer_m_s2[0] - + raw.accelerometer_m_s2[1] * raw.accelerometer_m_s2[1] - + raw.accelerometer_m_s2[2] * raw.accelerometer_m_s2[2]); - - /* check if length of gravity vector differs in more than 15 % from expected result */ - if (fabsf(accel_magnitude - CONSTANTS_ONE_G) > (CONSTANTS_ONE_G * 0.15f)) { - /* acceleration violates assumptions, disable updates */ - acceleration_stationary = false; - } - - /* update accelerometer measurements, reject if vector norm ends up being weird */ - if (sensor_last_count[1] != raw.accelerometer_counter && - acceleration_stationary) { + /* update accelerometer measurements */ + if (sensor_last_count[1] != raw.accelerometer_counter) { update_vect[1] = 1; sensor_last_count[1] = raw.accelerometer_counter; sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); 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];