diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-12-29 16:21:59 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-12-29 16:21:59 +0100 |
commit | 4976a3a47d9ed368734135df04ad67634c39c65d (patch) | |
tree | fbbc6b028ee1699a3938aaf859934ea415300996 /apps/attitude_estimator_ekf | |
parent | 1fc0a6546e30cfc913de8c0c03cfc625fba5d2ae (diff) | |
download | px4-firmware-4976a3a47d9ed368734135df04ad67634c39c65d.tar.gz px4-firmware-4976a3a47d9ed368734135df04ad67634c39c65d.tar.bz2 px4-firmware-4976a3a47d9ed368734135df04ad67634c39c65d.zip |
Added accel magnitude check, added conversion functions for various standard cases
Diffstat (limited to 'apps/attitude_estimator_ekf')
-rw-r--r-- | apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c | 27 | ||||
-rw-r--r-- | apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c | 6 |
2 files changed, 28 insertions, 5 deletions
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 6533390bc..685ab078b 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -65,6 +65,7 @@ #include <systemlib/systemlib.h> #include <systemlib/perf_counter.h> #include <systemlib/err.h> +#include <systemlib/conversions.h> #include "codegen/attitudeKalmanfilter_initialize.h" #include "codegen/attitudeKalmanfilter.h" @@ -254,6 +255,7 @@ 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 */ @@ -329,13 +331,34 @@ 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]; - /* update accelerometer measurements */ - if (sensor_last_count[1] != raw.accelerometer_counter) { + /* + * 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_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]; diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 4fc2e0452..6879f9dc8 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -65,9 +65,9 @@ PARAM_DEFINE_FLOAT(EKF_ATT_R0, 0.01f); PARAM_DEFINE_FLOAT(EKF_ATT_R1, 0.01f); PARAM_DEFINE_FLOAT(EKF_ATT_R2, 0.01f); /* accelerometer measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e1f); -PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e1f); -PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e1f); +PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e2f); +PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e2f); +PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e2f); /* magnetometer measurement noise */ PARAM_DEFINE_FLOAT(EKF_ATT_R6, 1e-1f); PARAM_DEFINE_FLOAT(EKF_ATT_R7, 1e-1f); |