rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-26 15:54:15 +01:00
Code Issues Releases Activity

LP-490 EKF maps magnetometer into horizontal plane based on known HomeLocation.Be value, to not distort Roll+Pitch estimate from mags

Browse Source
This commit is contained in:
Eric Price 2017-03-02 18:34:37 +01:00
parent f17d4eb760
commit 2124393d5f
2 changed files with 57 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
flight/libraries/insgps13state.c
View File

@ -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)

52
flight/modules/StateEstimation/filterekf.c
View File

@ -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,