Added accel magnitude check, added conversion functions for various standard cases

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 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);
diff --git a/apps/systemlib/conversions.c b/apps/systemlib/conversions.c
index 9105d83cb..32df446d9 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 dc383e770..3a24ca9b7 100644
--- a/apps/systemlib/conversions.h
+++ b/apps/systemlib/conversions.h
@@ -44,6 +44,8 @@
 #include <float.h>
 #include <stdint.h>
 
+#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_ */