From 8e484dd86266bb2a700b711d2f955057e0aa1bab Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 22 Jul 2016 00:41:13 +0200 Subject: [PATCH] LP-443 - EKF14 - Apply "2D" mag handling for EKF13 as well --- flight/libraries/insgps13state.c | 25 ++++++++++++++++++++++--- 1 file changed, 22 insertions(+), 3 deletions(-) diff --git a/flight/libraries/insgps13state.c b/flight/libraries/insgps13state.c index a81802035..f6aa54bee 100644 --- a/flight/libraries/insgps13state.c +++ b/flight/libraries/insgps13state.c @@ -402,9 +402,28 @@ void INSCorrection(const float mag_data[3], const float Pos[3], const float Vel[ if (SensorsUsed & MAG_SENSORS) { - Z[6] = mag_data[0]; - Z[7] = mag_data[1]; - Z[8] = mag_data[2]; + // magnetometer data in any units (use unit vector) and in body frame + float Rbe_a[3][3]; + float q0 = ekf.X[6]; + float q1 = ekf.X[7]; + float q2 = ekf.X[8]; + float q3 = ekf.X[9]; + float k1 = 1.0f / sqrtf(powf(q0 * q1 * 2.0f + q2 * q3 * 2.0f, 2.0f) + powf(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3, 2.0f)); + float k2 = sqrtf(-powf(q0 * q2 * 2.0f - q1 * q3 * 2.0f, 2.0f) + 1.0f); + + Rbe_a[0][0] = k2; + Rbe_a[0][1] = 0.0f; + Rbe_a[0][2] = q0 * q2 * -2.0f + q1 * q3 * 2.0f; + Rbe_a[1][0] = k1 * (q0 * q1 * 2.0f + q2 * q3 * 2.0f) * (q0 * q2 * 2.0f - q1 * q3 * 2.0f); + Rbe_a[1][1] = k1 * (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3); + Rbe_a[1][2] = k1 * sqrtf(-powf(q0 * q2 * 2.0f - q1 * q3 * 2.0f, 2.0f) + 1.0f) * (q0 * q1 * 2.0f + q2 * q3 * 2.0f); + Rbe_a[2][0] = k1 * (q0 * q2 * 2.0f - q1 * q3 * 2.0f) * (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3); + Rbe_a[2][1] = -k1 * (q0 * q1 * 2.0f + q2 * q3 * 2.0f); + Rbe_a[2][2] = k1 * k2 * (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3); + + Z[6] = Rbe_a[0][0] * mag_data[0] + Rbe_a[1][0] * mag_data[1] + Rbe_a[2][0] * mag_data[2]; + Z[7] = Rbe_a[0][1] * mag_data[0] + Rbe_a[1][1] * mag_data[1] + Rbe_a[2][1] * mag_data[2]; + Z[8] = Rbe_a[0][2] * mag_data[0] + Rbe_a[1][2] * mag_data[1] + Rbe_a[2][2] * mag_data[2]; } // barometric altimeter in meters and in local NED frame