diff --git a/flight/libraries/insgps13state.c b/flight/libraries/insgps13state.c
index f6aa54bee..2631bdb5a 100644
--- 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
diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c
index b0cb714b3..88943a2f4 100644
--- 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,
diff --git a/flight/targets/boards/discoveryf4bare/firmware/Makefile b/flight/targets/boards/discoveryf4bare/firmware/Makefile
index 41f1837d0..d9e9c67f4 100644
--- 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
diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile
index 62e0dd156..5ee49d1a2 100644
--- 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
diff --git a/flight/targets/boards/revonano/firmware/Makefile b/flight/targets/boards/revonano/firmware/Makefile
index 4c473dbf9..1abf5b82a 100644
--- 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
diff --git a/flight/targets/boards/revoproto/firmware/Makefile b/flight/targets/boards/revoproto/firmware/Makefile
index f40e1a308..d8d5124c0 100644
--- 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
diff --git a/flight/targets/boards/simposix/firmware/Makefile b/flight/targets/boards/simposix/firmware/Makefile
index 033993773..1849b9740 100644
--- 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
diff --git a/flight/targets/boards/sparky2/firmware/Makefile b/flight/targets/boards/sparky2/firmware/Makefile
index 942944f6d..45c7f2891 100644
--- 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
diff --git a/shared/uavobjectdefinition/ekfconfiguration.xml b/shared/uavobjectdefinition/ekfconfiguration.xml
index bf248a84b..493b89c4d 100644
--- 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"/>