mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
LP-490 EKF maps magnetometer into horizontal plane based on known HomeLocation.Be value, to not distort Roll+Pitch estimate from mags
This commit is contained in:
parent
f17d4eb760
commit
2124393d5f
@ -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)
|
||||
|
@ -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,
|
||||
|
Loading…
x
Reference in New Issue
Block a user