From 23e50ae5f89a4c81c4583e35ad7e19b1397cd68f Mon Sep 17 00:00:00 2001 From: Eric Price Date: Thu, 2 Mar 2017 10:57:00 +0100 Subject: [PATCH] LP-490 re-introduce correct mag handling (scaling to unit vector) in insgps13state --- flight/libraries/insgps13state.c | 44 +++++++++++++++------------------------- 1 file changed, 16 insertions(+), 28 deletions(-) diff --git a/flight/libraries/insgps13state.c b/flight/libraries/insgps13state.c index f6aa54bee..14f83f2d1 100644 --- a/flight/libraries/insgps13state.c +++ b/flight/libraries/insgps13state.c @@ -280,11 +280,14 @@ void INSSetGyroBiasVar(const float gyro_bias_var[3]) ekf.Q[8] = gyro_bias_var[2]; } -void INSSetMagVar(const float scaled_mag_var[3]) +void INSSetMagVar(const float mag_var[3]) { - ekf.R[6] = scaled_mag_var[0]; - ekf.R[7] = scaled_mag_var[1]; - ekf.R[8] = scaled_mag_var[2]; + // scale variance down to unit vector + float invBmag = invsqrtf(mag_var[0] * mag_var[0] + mag_var[1] * mag_var[1] + mag_var[2] * mag_var[2]); + + ekf.R[6] = mag_var[0] * invBmag; + ekf.R[7] = mag_var[1] * invBmag; + ekf.R[8] = mag_var[2] * invBmag; } void INSSetBaroVar(float baro_var) @@ -294,9 +297,11 @@ void INSSetBaroVar(float baro_var) void INSSetMagNorth(const float B[3]) { - ekf.Be[0] = B[0]; - ekf.Be[1] = B[1]; - ekf.Be[2] = B[2]; + float invmag = invsqrtf(B[0] * B[0] + B[1] * B[1] + B[2] * B[2]); + + ekf.Be[0] = B[0] * invmag; + ekf.Be[1] = B[1] * invmag; + ekf.Be[2] = B[2] * invmag; } void INSStatePrediction(const float gyro_data[3], const float accel_data[3], float dT) @@ -403,27 +408,10 @@ void INSCorrection(const float mag_data[3], const float Pos[3], const float Vel[ if (SensorsUsed & MAG_SENSORS) { // 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]; + float invBmag = invsqrtf(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] + mag_data[2] * mag_data[2]); + Z[6] = mag_data[0] * invBmag; + Z[7] = mag_data[1] * invBmag; + Z[8] = mag_data[2] * invBmag; } // barometric altimeter in meters and in local NED frame -- 2.11.4.GIT