From 4976a3a47d9ed368734135df04ad67634c39c65d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 29 Dec 2012 16:21:59 +0100 Subject: [PATCH] Added accel magnitude check, added conversion functions for various standard cases --- .../attitude_estimator_ekf_main.c | 27 +++++- .../attitude_estimator_ekf_params.c | 6 +- apps/systemlib/conversions.c | 89 +++++++++++++++++++ apps/systemlib/conversions.h | 22 +++++ 4 files changed, 139 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 6533390bc0..685ab078b2 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 #include #include +#include #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 4fc2e0452a..6879f9dc8e 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); diff --git a/apps/systemlib/conversions.c b/apps/systemlib/conversions.c index 9105d83cbe..32df446d99 100644 --- a/apps/systemlib/conversions.c +++ b/apps/systemlib/conversions.c @@ -55,3 +55,92 @@ int16_t_from_bytes(uint8_t bytes[]) return u.w; } + +void rot2quat(const float R[9], float Q[4]) +{ + float q0_2; + float q1_2; + float q2_2; + float q3_2; + int32_t idx; + + /* conversion of rotation matrix to quaternion + * choose the largest component to begin with */ + q0_2 = (((1.0F + R[0]) + R[4]) + R[8]) / 4.0F; + q1_2 = (((1.0F + R[0]) - R[4]) - R[8]) / 4.0F; + q2_2 = (((1.0F - R[0]) + R[4]) - R[8]) / 4.0F; + q3_2 = (((1.0F - R[0]) - R[4]) + R[8]) / 4.0F; + + idx = 0; + + if (q0_2 < q1_2) { + q0_2 = q1_2; + + idx = 1; + } + + if (q0_2 < q2_2) { + q0_2 = q2_2; + idx = 2; + } + + if (q0_2 < q3_2) { + q0_2 = q3_2; + idx = 3; + } + + q0_2 = sqrtf(q0_2); + + /* solve for the remaining three components */ + if (idx == 0) { + q1_2 = q0_2; + q2_2 = (R[5] - R[7]) / 4.0F / q0_2; + q3_2 = (R[6] - R[2]) / 4.0F / q0_2; + q0_2 = (R[1] - R[3]) / 4.0F / q0_2; + } else if (idx == 1) { + q2_2 = q0_2; + q1_2 = (R[5] - R[7]) / 4.0F / q0_2; + q3_2 = (R[3] + R[1]) / 4.0F / q0_2; + q0_2 = (R[6] + R[2]) / 4.0F / q0_2; + } else if (idx == 2) { + q3_2 = q0_2; + q1_2 = (R[6] - R[2]) / 4.0F / q0_2; + q2_2 = (R[3] + R[1]) / 4.0F / q0_2; + q0_2 = (R[7] + R[5]) / 4.0F / q0_2; + } else { + q1_2 = (R[1] - R[3]) / 4.0F / q0_2; + q2_2 = (R[6] + R[2]) / 4.0F / q0_2; + q3_2 = (R[7] + R[5]) / 4.0F / q0_2; + } + + /* return values */ + Q[0] = q1_2; + Q[1] = q2_2; + Q[2] = q3_2; + Q[3] = q0_2; +} + +void quat2rot(const float Q[4], float R[9]) +{ + float q0_2; + float q1_2; + float q2_2; + float q3_2; + + memset(&R[0], 0, 9U * sizeof(float)); + + q0_2 = Q[0] * Q[0]; + q1_2 = Q[1] * Q[1]; + q2_2 = Q[2] * Q[2]; + q3_2 = Q[3] * Q[3]; + + R[0] = ((q0_2 + q1_2) - q2_2) - q3_2; + R[3] = 2.0F * (Q[1] * Q[2] - Q[0] * Q[3]); + R[6] = 2.0F * (Q[1] * Q[3] + Q[0] * Q[2]); + R[1] = 2.0F * (Q[1] * Q[2] + Q[0] * Q[3]); + R[4] = ((q0_2 + q2_2) - q1_2) - q3_2; + R[7] = 2.0F * (Q[2] * Q[3] - Q[0] * Q[1]); + R[2] = 2.0F * (Q[1] * Q[3] - Q[0] * Q[2]); + R[5] = 2.0F * (Q[2] * Q[3] + Q[0] * Q[1]); + R[8] = ((q0_2 + q3_2) - q1_2) - q2_2; +} \ No newline at end of file diff --git a/apps/systemlib/conversions.h b/apps/systemlib/conversions.h index dc383e770f..3a24ca9b7c 100644 --- a/apps/systemlib/conversions.h +++ b/apps/systemlib/conversions.h @@ -44,6 +44,8 @@ #include #include +#define CONSTANTS_ONE_G 9.80665f + __BEGIN_DECLS /** @@ -56,6 +58,26 @@ __BEGIN_DECLS */ __EXPORT int16_t int16_t_from_bytes(uint8_t bytes[]); +/** + * Converts a 3 x 3 rotation matrix to an unit quaternion. + * + * All orientations are expressed in NED frame. + * + * @param R rotation matrix to convert + * @param Q quaternion to write back to + */ +__EXPORT void rot2quat(const float R[9], float Q[4]); + +/** + * Converts an unit quaternion to a 3 x 3 rotation matrix. + * + * All orientations are expressed in NED frame. + * + * @param Q quaternion to convert + * @param R rotation matrix to write back to + */ +__EXPORT void quat2rot(const float Q[4], float R[9]); + __END_DECLS #endif /* CONVERSIONS_H_ */