From e62cf039fb4f37cd7737a972720ce1c76649342b Mon Sep 17 00:00:00 2001 From: Eric Price Date: Thu, 2 Mar 2017 10:30:03 +0100 Subject: [PATCH 1/7] LP-490 move back to insgps13state --- flight/targets/boards/discoveryf4bare/firmware/Makefile | 2 +- flight/targets/boards/revolution/firmware/Makefile | 2 +- flight/targets/boards/revonano/firmware/Makefile | 2 +- flight/targets/boards/revoproto/firmware/Makefile | 2 +- flight/targets/boards/simposix/firmware/Makefile | 2 +- flight/targets/boards/sparky2/firmware/Makefile | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) 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 From 23e50ae5f89a4c81c4583e35ad7e19b1397cd68f Mon Sep 17 00:00:00 2001 From: Eric Price Date: Thu, 2 Mar 2017 10:57:00 +0100 Subject: [PATCH 2/7] 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 From f17d4eb7603d4e8c28fe637798b706298a044157 Mon Sep 17 00:00:00 2001 From: Eric Price Date: Thu, 2 Mar 2017 12:19:37 +0100 Subject: [PATCH 3/7] LP-490 added new setting for EKF Magnetometer Correction --- shared/uavobjectdefinition/ekfconfiguration.xml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/shared/uavobjectdefinition/ekfconfiguration.xml b/shared/uavobjectdefinition/ekfconfiguration.xml index bf248a84b..ca9696dd8 100644 --- a/shared/uavobjectdefinition/ekfconfiguration.xml +++ b/shared/uavobjectdefinition/ekfconfiguration.xml @@ -66,6 +66,9 @@ FakeGPSVelAirspeed + From 2124393d5fdeeb39324f4e6f7480cf8229a3b455 Mon Sep 17 00:00:00 2001 From: Eric Price Date: Thu, 2 Mar 2017 18:34:37 +0100 Subject: [PATCH 4/7] LP-490 EKF maps magnetometer into horizontal plane based on known HomeLocation.Be value, to not distort Roll+Pitch estimate from mags --- flight/libraries/insgps13state.c | 19 ++++---- flight/modules/StateEstimation/filterekf.c | 52 ++++++++++++++++++++-- 2 files changed, 57 insertions(+), 14 deletions(-) diff --git a/flight/libraries/insgps13state.c b/flight/libraries/insgps13state.c index 14f83f2d1..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,14 +281,12 @@ void INSSetGyroBiasVar(const float gyro_bias_var[3]) ekf.Q[8] = gyro_bias_var[2]; } +// must be called AFTER SetMagNorth void INSSetMagVar(const float mag_var[3]) { - // 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; + 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) @@ -297,11 +296,11 @@ void INSSetBaroVar(float baro_var) void INSSetMagNorth(const float B[3]) { - float invmag = invsqrtf(B[0] * B[0] + B[1] * B[1] + B[2] * B[2]); + ekf.BeScaleFactor = 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; + 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) diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index b0cb714b3..9c7f8e72f 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,46 @@ 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 + // rot_mult(R, (const float[3]) { 0, 0, VectorMagnitude(this->work.mag) }, this->work.mag); + 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, From 40ebd0a06282122551b7312c5d0d74bf105af86f Mon Sep 17 00:00:00 2001 From: Eric Price Date: Thu, 2 Mar 2017 18:36:06 +0100 Subject: [PATCH 5/7] LP-490 fixed ekf configuration default values for return to insgps13state - no more asymetric variances in body frame that assume certain board orientations to work --- shared/uavobjectdefinition/ekfconfiguration.xml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/shared/uavobjectdefinition/ekfconfiguration.xml b/shared/uavobjectdefinition/ekfconfiguration.xml index ca9696dd8..d5fe365ca 100644 --- a/shared/uavobjectdefinition/ekfconfiguration.xml +++ b/shared/uavobjectdefinition/ekfconfiguration.xml @@ -23,7 +23,7 @@ @@ -39,9 +39,9 @@ GPSPosNorth From 14d935a6dd74ebd65b5f8f7f86f4ac28c98cc750 Mon Sep 17 00:00:00 2001 From: Eric Price Date: Thu, 2 Mar 2017 20:47:50 +0100 Subject: [PATCH 6/7] LP-490 changed default gyro process noise to increase filter stability (converges quicker to a stable estimate) --- shared/uavobjectdefinition/ekfconfiguration.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/shared/uavobjectdefinition/ekfconfiguration.xml b/shared/uavobjectdefinition/ekfconfiguration.xml index d5fe365ca..493b89c4d 100644 --- a/shared/uavobjectdefinition/ekfconfiguration.xml +++ b/shared/uavobjectdefinition/ekfconfiguration.xml @@ -23,7 +23,7 @@ From e9a0e3ad697667242fbfa07fcc29b07c2b54977e Mon Sep 17 00:00:00 2001 From: Eric Price Date: Tue, 7 Mar 2017 12:12:18 +0100 Subject: [PATCH 7/7] LP-490 cosmetic code cleanup --- flight/modules/StateEstimation/filterekf.c | 1 - 1 file changed, 1 deletion(-) diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index 9c7f8e72f..88943a2f4 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -380,7 +380,6 @@ static filterResult filter(stateFilter *self, stateEstimation *state) rotvec[1] *= -this->magLockAlpha; rotvec[2] *= -this->magLockAlpha; // 3. rotate artificial magnetometer reading from straight down to correct roll+pitch - // rot_mult(R, (const float[3]) { 0, 0, VectorMagnitude(this->work.mag) }, this->work.mag); Rv2Rot(rotvec, R); float MagStrength = VectorMagnitude(this->homeLocation.Be); local_down[0] *= MagStrength;