Merged in corvusvcorax/librepilot/LP-490_insgps13state_mag_fixes (pull request #398)

LP-490 insgps13state mag fixes Approved-by: Alessio Morale <alessiomorale@gmail.com> Approved-by: Eric Price <corvuscorax@cybertrench.com> Approved-by: Philippe Renon <philippe_renon@yahoo.fr> Approved-by: Lalanne Laurent <f5soh@free.fr>
2025-01-30 15:52:12 +01:00 · 2017-03-21 21:25:10 +00:00 · 2017-03-21 21:25:10 +00:00 · cf1ac886b3
commit cf1ac886b3
parent 86d6bec7d1 e9a0e3ad69
9 changed files with 74 additions and 41 deletions
--- a/flight/libraries/insgps13state.c
+++ b/flight/libraries/insgps13state.c
@ -92,6 +92,7 @@ static struct EKFData {
    float H[NUMV][NUMX];
    // local magnetic unit vector in NED frame
    float Be[3];
+    float BeScaleFactor;
    // covariance matrix and state vector
    float P[NUMX][NUMX];
    float X[NUMX];
@ -280,11 +281,12 @@ void INSSetGyroBiasVar(const float gyro_bias_var[3])
    ekf.Q[8] = gyro_bias_var[2];
 }

-void INSSetMagVar(const float scaled_mag_var[3])
+// must be called AFTER SetMagNorth
+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];
+    ekf.R[6] = mag_var[0] * ekf.BeScaleFactor;
+    ekf.R[7] = mag_var[1] * ekf.BeScaleFactor;
+    ekf.R[8] = mag_var[2] * ekf.BeScaleFactor;
 }

 void INSSetBaroVar(float baro_var)
@ -294,9 +296,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];
+    ekf.BeScaleFactor = invsqrtf(B[0] * B[0] + B[1] * B[1] + B[2] * B[2]);
+
+    ekf.Be[0] = B[0] * ekf.BeScaleFactor;
+    ekf.Be[1] = B[1] * ekf.BeScaleFactor;
+    ekf.Be[2] = B[2] * ekf.BeScaleFactor;
 }

 void INSStatePrediction(const float gyro_data[3], const float accel_data[3], float dT)
@ -403,27 +407,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
--- a/flight/modules/StateEstimation/filterekf.c
+++ b/flight/modules/StateEstimation/filterekf.c
@ -68,10 +68,11 @@ struct data {

    stateEstimation work;

-    bool inited;
+    bool  inited;

    PiOSDeltatimeConfig dtconfig;
-    bool navOnly;
+    bool  navOnly;
+    float magLockAlpha;
 };


@ -166,6 +167,15 @@ static int32_t init(stateFilter *self)
        return 2;
    }

+    // calculate Angle between Down vector and homeLocation.Be
+    float cross[3];
+    float magnorm[3]    = { this->homeLocation.Be[0], this->homeLocation.Be[1], this->homeLocation.Be[2] };
+    vector_normalizef(magnorm, 3);
+    const float down[3] = { 0, 0, 1 };
+    CrossProduct(down, magnorm, cross);
+    // VectorMagnitude(cross) = sin(Alpha)
+    // [0,0,1] dot magnorm = magnorm[2] = cos(Alpha)
+    this->magLockAlpha = atan2f(VectorMagnitude(cross), magnorm[2]);

    return 0;
 }
@ -184,6 +194,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
    uint16_t sensors = 0;

    INSSetArmed(state->armed);
+    INSSetMagNorth(this->homeLocation.Be);
    state->navUsed      = (this->usePos || this->navOnly);
    this->work.updated |= state->updated;
    // check magnetometer alarm, discard any magnetometer readings if not OK
@ -212,6 +223,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
        UNSET_MASK(state->updated, SENSORUPDATES_vel);
        UNSET_MASK(state->updated, SENSORUPDATES_attitude);
        UNSET_MASK(state->updated, SENSORUPDATES_gyro);
+        UNSET_MASK(state->updated, SENSORUPDATES_mag);
        return FILTERRESULT_OK;
    }

@ -350,14 +362,45 @@ static filterResult filter(stateFilter *self, stateEstimation *state)

    if (IS_SET(this->work.updated, SENSORUPDATES_mag)) {
        sensors |= MAG_SENSORS;
+        if (this->ekfConfiguration.MapMagnetometerToHorizontalPlane == EKFCONFIGURATION_MAPMAGNETOMETERTOHORIZONTALPLANE_TRUE) {
+            // Map Magnetometer vector to correspond to the Roll+Pitch of the current Attitude State Estimate (no conflicting gravity)
+            // Idea: Alpha between Local Down and Mag is invariant of orientation, and identical to Alpha between [0,0,1] and HomeLocation.Be
+            // which is measured in init()
+            float R[3][3];
+
+            // 1. rotate down vector into body frame
+            Quaternion2R(Nav.q, R);
+            float local_down[3];
+            rot_mult(R, (float[3]) { 0, 0, 1 }, local_down);
+            // 2. create a rotation vector that is perpendicular to rotated down vector, magnetic field vector and of size magLockAlpha
+            float rotvec[3];
+            CrossProduct(local_down, this->work.mag, rotvec);
+            vector_normalizef(rotvec, 3);
+            rotvec[0] *= -this->magLockAlpha;
+            rotvec[1] *= -this->magLockAlpha;
+            rotvec[2] *= -this->magLockAlpha;
+            // 3. rotate artificial magnetometer reading from straight down to correct roll+pitch
+            Rv2Rot(rotvec, R);
+            float MagStrength = VectorMagnitude(this->homeLocation.Be);
+            local_down[0] *= MagStrength;
+            local_down[1] *= MagStrength;
+            local_down[2] *= MagStrength;
+            rot_mult(R, local_down, this->work.mag);
+        }
+        // debug rotated mags
+        state->mag[0]   = this->work.mag[0];
+        state->mag[1]   = this->work.mag[1];
+        state->mag[2]   = this->work.mag[2];
+        state->updated |= SENSORUPDATES_mag;
+    } else {
+        // mag state is delayed until EKF processed it, allows overriding/debugging magnetometer estimate
+        UNSET_MASK(state->updated, SENSORUPDATES_mag);
    }

    if (IS_SET(this->work.updated, SENSORUPDATES_baro)) {
        sensors |= BARO_SENSOR;
    }

-    INSSetMagNorth(this->homeLocation.Be);
-
    if (!this->usePos) {
        // position and velocity variance used in indoor mode
        INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR.FakeGPSPosIndoor,
--- a/flight/targets/boards/discoveryf4bare/firmware/Makefile
+++ b/flight/targets/boards/discoveryf4bare/firmware/Makefile
@ -97,7 +97,7 @@ ifndef TESTAPP
    SRC += $(FLIGHTLIB)/paths.c
 	SRC += $(FLIGHTLIB)/plans.c
    SRC += $(FLIGHTLIB)/WorldMagModel.c
-    SRC += $(FLIGHTLIB)/insgps14state.c
+    SRC += $(FLIGHTLIB)/insgps13state.c
    SRC += $(FLIGHTLIB)/auxmagsupport.c
    SRC += $(FLIGHTLIB)/lednotification.c    

--- a/flight/targets/boards/revolution/firmware/Makefile
+++ b/flight/targets/boards/revolution/firmware/Makefile
@ -95,7 +95,7 @@ ifndef TESTAPP
    SRC += $(FLIGHTLIB)/paths.c
    SRC += $(FLIGHTLIB)/plans.c
    SRC += $(FLIGHTLIB)/WorldMagModel.c
-    SRC += $(FLIGHTLIB)/insgps14state.c
+    SRC += $(FLIGHTLIB)/insgps13state.c
    SRC += $(FLIGHTLIB)/auxmagsupport.c
    SRC += $(FLIGHTLIB)/lednotification.c    
    SRC += $(FLIGHTLIB)/sha1.c
--- a/flight/targets/boards/revonano/firmware/Makefile
+++ b/flight/targets/boards/revonano/firmware/Makefile
@ -94,7 +94,7 @@ ifndef TESTAPP
    SRC += $(FLIGHTLIB)/paths.c
    SRC += $(FLIGHTLIB)/plans.c
    SRC += $(FLIGHTLIB)/WorldMagModel.c
-    SRC += $(FLIGHTLIB)/insgps14state.c
+    SRC += $(FLIGHTLIB)/insgps13state.c
    SRC += $(FLIGHTLIB)/lednotification.c
    SRC += $(FLIGHTLIB)/auxmagsupport.c
    ## UAVObjects
--- a/flight/targets/boards/revoproto/firmware/Makefile
+++ b/flight/targets/boards/revoproto/firmware/Makefile
@ -92,7 +92,7 @@ ifndef TESTAPP
    SRC += $(FLIGHTLIB)/paths.c
    SRC += $(FLIGHTLIB)/plans.c
    SRC += $(FLIGHTLIB)/WorldMagModel.c
-    SRC += $(FLIGHTLIB)/insgps14state.c
+    SRC += $(FLIGHTLIB)/insgps13state.c
    SRC += $(FLIGHTLIB)/auxmagsupport.c

    ## UAVObjects
--- a/flight/targets/boards/simposix/firmware/Makefile
+++ b/flight/targets/boards/simposix/firmware/Makefile
@ -106,7 +106,7 @@ SRC += $(FLIGHT_UAVOBJ_DIR)/uavobjectsinit.c
 SRC += $(FLIGHTLIB)/CoordinateConversions.c
 SRC += $(FLIGHTLIB)/fifo_buffer.c
 SRC += $(FLIGHTLIB)/WorldMagModel.c
-SRC += $(FLIGHTLIB)/insgps14state.c
+SRC += $(FLIGHTLIB)/insgps13state.c
 SRC += $(FLIGHTLIB)/paths.c
 SRC += $(FLIGHTLIB)/plans.c
 SRC += $(FLIGHTLIB)/sanitycheck.c
--- a/flight/targets/boards/sparky2/firmware/Makefile
+++ b/flight/targets/boards/sparky2/firmware/Makefile
@ -95,7 +95,7 @@ ifndef TESTAPP
    SRC += $(FLIGHTLIB)/paths.c
    SRC += $(FLIGHTLIB)/plans.c
    SRC += $(FLIGHTLIB)/WorldMagModel.c
-    SRC += $(FLIGHTLIB)/insgps14state.c
+    SRC += $(FLIGHTLIB)/insgps13state.c
    SRC += $(FLIGHTLIB)/auxmagsupport.c
    SRC += $(FLIGHTLIB)/lednotification.c    
    SRC += $(FLIGHTLIB)/sha1.c
--- a/shared/uavobjectdefinition/ekfconfiguration.xml
+++ b/shared/uavobjectdefinition/ekfconfiguration.xml
@ -23,7 +23,7 @@
 		</elementnames>
 	</field>
 	<field name="Q" units="1^2" type="float" defaultvalue="
-		0.00001, 0.00001, 0.0001,
+		0.001, 0.001, 0.001,
 		0.003, 0.003, 0.003,
 		0.000001, 0.000001, 0.000001">
 		<elementnames>
@ -39,9 +39,9 @@
 		</elementnames>
 	</field>
 	<field name="R" units="1^2" type="float" defaultvalue="
-		0.01, .01, 1000000,
+		0.1, 0.1, 1000000,
 		0.01, 0.01, 0.01,
-		10, 10, 100,
+		10, 10, 10,
 		0.01">
 		<elementnames>
 			<elementname>GPSPosNorth</elementname>
@ -66,6 +66,9 @@
 			<elementname>FakeGPSVelAirspeed</elementname>
 		</elementnames>
 	</field>
+	<field name="MapMagnetometerToHorizontalPlane" type="enum" units="bool" elements="1"
+		options="False,True" defaultvalue="True"
+		description="Set to True to suppress effect of magnetometers on Roll+Pitch State estimate" />
        <access gcs="readwrite" flight="readwrite"/>
        <telemetrygcs acked="true" updatemode="onchange" period="0"/>
        <telemetryflight acked="true" updatemode="onchange" period="0"/>