zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding 

This will enable in-flight magnetometer calibration to be inhibited unconditionally,
This is required for long balloon carriage flights where ground speed can be high
enough to put it into in-air state, but with very poor observability of magnetic field
states causing bad state estimates and heading offsets to develop over time.

The covariance matrix no longer has rows and columns artificially zeroed when in static
mode. Instead booleans indicating whether wind or magentic field state estimation is
active are used to:

a) Set the process noise on these states to zero to stop their variances from increasing
unchecked when not being updated, and
b) Turn off updates for these states when measurement fusion is being performed.

This reduces the likelihood of a badly conditioned covariance matrix forming
during static mode operation.

A filter divergence check has also been added that will declare the filter unhealthy if
position, velocity and magnetic field observations are all failing their innovation
consistency checks. This unhealthy status will persist for 10 seconds after the
condition clears.

AP_NavEKF: Remove unnecessary zeroing of wind covariances
mission-4.1.18
priseborough 11 years ago committed by Andrew Tridgell
parent
3026ccee13
commit
925c5751bd
2 changed files with 254 additions and 254 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 497
      libraries/AP_NavEKF/AP_NavEKF.cpp
  2. 11
      libraries/AP_NavEKF/AP_NavEKF.h

497
libraries/AP_NavEKF/AP_NavEKF.cpp
Unescape View File

@ -279,8 +279,8 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = { @@ -279,8 +279,8 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
// @Param: MAG_CAL
// @DisplayName: Magnetometer calibration mode
// @Description: Setting this parameter to 1 forces magnetic field state calibration to be active all the time the vehicle is manoeuvring regardless of its speed and altitude. This parameter should be set to 0 for aircraft use. This parameter can be set to 1 to enable in-flight compass calibration on Copter and Rover vehicles.
// @Values: 0:WhenMoving,1:Always
// @Description: EKF_MAG_CAL = 0 enables calibration based on flying speed and altitude and is the default setting for Plane users. EKF_MAG_CAL = 1 enables calibration based on manoeuvre level and is the default setting for Copter and Rover users. EKF_MAG_CAL = 2 prevents magnetometer calibration regardless of flight condition and is recommended if in-flight magnetometer calibration is unreliable.
// @Values: 0:Speed and Height,1:Acceleration,2:Never
// @Increment: 1
// @User: advanced
AP_GROUPINFO("MAG_CAL", 22, NavEKF, _magCal, MAG_CAL_DEFAULT),
@ -316,7 +316,9 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) : @@ -316,7 +316,9 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
fuseMeNow(false), // forces airspeed and sythetic sideslip fusion to occur on the IMU frame that data arrives
staticMode(true), // staticMode forces position and velocity fusion with zero values
prevStaticMode(true), // staticMode from previous filter update
yawAligned(false) // set true when heading or yaw angle has been aligned
yawAligned(false), // set true when heading or yaw angle has been aligned
inhibitWindStates(true), // inhibit wind state updates on startup
inhibitMagStates(true) // inhibit magnetometer state updates on startup
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
,_perf_UpdateFilter(perf_alloc(PC_ELAPSED, "EKF_UpdateFilter")),
@ -341,7 +343,7 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) : @@ -341,7 +343,7 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
_hgtRetryTimeMode12 = 5000; // Height retry time without vertical velocity measurement (msec)
_magFailTimeLimit_ms = 10000; // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec)
_magVarRateScale = 0.05f; // scale factor applied to magnetometer variance due to angular rate
_gyroBiasNoiseScaler = 3.0f; // scale factor applied to gyro bias state process variance when on ground
_gyroBiasNoiseScaler = 2.0f; // scale factor applied to gyro bias state process noise when on ground
_msecGpsAvg = 200; // average number of msec between GPS measurements
_msecHgtAvg = 100; // average number of msec between height measurements
_msecBetaAvg = 100; // average number of msec between synthetic sideslip measurements
@ -365,6 +367,9 @@ bool NavEKF::healthy(void) const @@ -365,6 +367,9 @@ bool NavEKF::healthy(void) const
if (state.velocity.is_nan()) {
return false;
}
if (filterDiverged) {
return false;
}
// If measurements have failed innovation consistency checks for long enough to time-out
// and force fusion then the nav solution can be conidered to be unhealthy
// This will only be set as a transient
@ -523,7 +528,7 @@ void NavEKF::InitialiseFilterBootstrap(void) @@ -523,7 +528,7 @@ void NavEKF::InitialiseFilterBootstrap(void)
readHgtData();
// check on ground status
OnGroundCheck();
SetFlightAndFusionModes();
// write to state vector
state.quat = initQuat;
@ -575,7 +580,7 @@ void NavEKF::UpdateFilter() @@ -575,7 +580,7 @@ void NavEKF::UpdateFilter()
}
// check if on ground
OnGroundCheck();
SetFlightAndFusionModes();
// define rules used to set staticMode
// staticMode enables ground operation without GPS by fusing zeros for position and height measurements
@ -733,7 +738,6 @@ void NavEKF::SelectMagFusion() @@ -733,7 +738,6 @@ void NavEKF::SelectMagFusion()
}
}
// determine if conditions are right to start a new fusion cycle
bool dataReady = statesInitialised && use_compass() && newDataMag;
if (dataReady)
@ -757,9 +761,8 @@ void NavEKF::SelectTasFusion() @@ -757,9 +761,8 @@ void NavEKF::SelectTasFusion()
// get true airspeed measurement
readAirSpdData();
// if the filter is initialised and we are using airspeed measurements and we are flying and
// we either have new data or are waiting to fuse old data, then perform fusion
tasDataWaiting = (statesInitialised && useAirspeed() && !onGround && (tasDataWaiting || newDataTas));
// if the filter is initialised, wind states are not inhibited and we have data to fuse, then queue TAS fusion
tasDataWaiting = (statesInitialised && !inhibitWindStates && (tasDataWaiting || newDataTas));
// if we have waited too long, set a timeout flag which will force fusion to occur
bool timeout = ((IMUmsec - TASmsecPrev) >= TASmsecMax);
@ -785,7 +788,7 @@ void NavEKF::SelectBetaFusion() @@ -785,7 +788,7 @@ void NavEKF::SelectBetaFusion()
// we are a fly forward vehicle type AND NOT using a full range of sensors with healthy position
// AND NOT on the ground AND enough time has lapsed since our last fusion
// AND (we have not fused magnetometer data on this time step OR the immediate fusion flag is set)
if (assume_zero_sideslip() && !(use_compass() && useAirspeed() && posHealth) && !onGround && ((IMUmsec - BETAmsecPrev) >= _msecBetaAvg) && (!magFusePerformed || fuseMeNow)) {
if (assume_zero_sideslip() && !(use_compass() && useAirspeed() && posHealth) && !inhibitWindStates && ((IMUmsec - BETAmsecPrev) >= _msecBetaAvg) && (!magFusePerformed || fuseMeNow)) {
FuseSideslip();
BETAmsecPrev = IMUmsec;
}
@ -932,17 +935,26 @@ void NavEKF::CovariancePrediction() @@ -932,17 +935,26 @@ void NavEKF::CovariancePrediction()
// use filtered height rate to increase wind process noise when climbing or descending
// this allows for wind gradient effects.
windVelSigma = dt * constrain_float(_windVelProcessNoise, 0.01f, 1.0f) * (1.0f + constrain_float(_wndVarHgtRateScale, 0.0f, 1.0f) * fabsf(hgtRate));
if (!inhibitWindStates) {
windVelSigma = dt * constrain_float(_windVelProcessNoise, 0.01f, 1.0f) * (1.0f + constrain_float(_wndVarHgtRateScale, 0.0f, 1.0f) * fabsf(hgtRate));
} else {
windVelSigma = 0.0f;
}
dAngBiasSigma = dt * constrain_float(_gyroBiasProcessNoise, 1e-7f, 1e-5f);
dVelBiasSigma = dt * constrain_float(_accelBiasProcessNoise, 1e-4f, 1e-3f);
magEarthSigma = dt * constrain_float(_magEarthProcessNoise, 1e-4f, 1e-2f);
magBodySigma = dt * constrain_float(_magBodyProcessNoise, 1e-4f, 1e-2f);
if (!inhibitMagStates) {
magEarthSigma = dt * constrain_float(_magEarthProcessNoise, 1e-4f, 1e-2f);
magBodySigma = dt * constrain_float(_magBodyProcessNoise, 1e-4f, 1e-2f);
} else {
magEarthSigma = 0.0f;
magBodySigma = 0.0f;
}
for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f;
for (uint8_t i=10; i<=12; i++) processNoise[i] = dAngBiasSigma;
// scale gyro bias noise when on ground to allow for faster bias estimation
// scale gyro bias noise when in static mode to allow for faster bias estimation
for (uint8_t i=10; i<=12; i++) {
processNoise[i] = dAngBiasSigma;
if (onGround) {
if (staticMode) {
processNoise[i] *= _gyroBiasNoiseScaler;
}
}
@ -1563,7 +1575,6 @@ void NavEKF::FuseVelPosNED() @@ -1563,7 +1575,6 @@ void NavEKF::FuseVelPosNED()
bool fuseData[6] = {false,false,false,false,false,false};
uint8_t stateIndex;
uint8_t obsIndex;
uint8_t indexLimit; // used to prevent access to wind and magnetic field states and variances when on ground
// declare variables used by state and covariance update calculations
float NEvelErr;
@ -1579,8 +1590,7 @@ void NavEKF::FuseVelPosNED() @@ -1579,8 +1590,7 @@ void NavEKF::FuseVelPosNED()
// data from the GPS receiver it is the only assumption we can make
// so we might as well take advantage of the computational efficiencies
// associated with sequential fusion
if (fuseVelData || fusePosData || fuseHgtData)
{
if (fuseVelData || fusePosData || fuseHgtData) {
// if static mode is active use the current states to calculate the predicted
// measurement rather than use states from a previous time. We need to do this
@ -1637,11 +1647,9 @@ void NavEKF::FuseVelPosNED() @@ -1637,11 +1647,9 @@ void NavEKF::FuseVelPosNED()
}
}
// calculate innovations and check GPS data validity using an innovation consistency check
// test position measurements
if (fusePosData)
{
if (fusePosData) {
// test horizontal position measurements
posInnov[0] = statesAtPosTime.position.x - observation[3];
posInnov[1] = statesAtPosTime.position.y - observation[4];
@ -1657,8 +1665,7 @@ void NavEKF::FuseVelPosNED() @@ -1657,8 +1665,7 @@ void NavEKF::FuseVelPosNED()
// declare a timeout condition if we have been too long without data
posTimeout = ((hal.scheduler->millis() - posFailTime) > gpsRetryTime);
// use position data if healthy, timed out, or in static mode
if (posHealth || posTimeout || staticMode)
{
if (posHealth || posTimeout || staticMode) {
posHealth = true;
posFailTime = hal.scheduler->millis();
// if timed out or outside the specified glitch radius, increment the offset applied to GPS data to compensate for large GPS position jumps
@ -1672,15 +1679,13 @@ void NavEKF::FuseVelPosNED() @@ -1672,15 +1679,13 @@ void NavEKF::FuseVelPosNED()
// don't fuse data on this time step
fusePosData = false;
}
}
else
{
} else {
posHealth = false;
}
}
// test velocity measurements
if (fuseVelData)
{
if (fuseVelData) {
// test velocity measurements
uint8_t imax = 2;
if (_fusionModeGPS == 1) {
@ -1690,8 +1695,7 @@ void NavEKF::FuseVelPosNED() @@ -1690,8 +1695,7 @@ void NavEKF::FuseVelPosNED()
float K2 = 0; // innovation to error ratio for IMU2
float innovVelSumSq = 0; // sum of squares of velocity innovations
float varVelSum = 0; // sum of velocity innovation variances
for (uint8_t i = 0; i<=imax; i++)
{
for (uint8_t i = 0; i<=imax; i++) {
// velocity states start at index 4
stateIndex = i + 4;
// calculate innovations using blended and single IMU predicted states
@ -1726,26 +1730,22 @@ void NavEKF::FuseVelPosNED() @@ -1726,26 +1730,22 @@ void NavEKF::FuseVelPosNED()
// declare a timeout if we have not fused velocity data for too long
velTimeout = (hal.scheduler->millis() - velFailTime) > gpsRetryTime;
// if data is healthy or in static mode we fuse it
if (velHealth || staticMode)
{
if (velHealth || staticMode) {
velHealth = true;
velFailTime = hal.scheduler->millis();
}
// if data is not healthy and timed out and position is unhealthy we reset the velocity, but do not fuse data on this time step
else if (velTimeout && !posHealth) {
} else if (velTimeout && !posHealth) {
// if data is not healthy and timed out and position is unhealthy we reset the velocity, but do not fuse data on this time step
ResetVelocity();
StoreStatesReset();
fuseVelData = false;
}
// if data is unhealthy and position is healthy, we do not fuse it
else
{
} else {
// if data is unhealthy and position is healthy, we do not fuse it
velHealth = false;
}
}
// test height measurements
if (fuseHgtData)
{
if (fuseHgtData) {
// set the height data timeout depending on whether vertical velocity data is being used
uint32_t hgtRetryTime;
if (_fusionModeGPS == 0) hgtRetryTime = _hgtRetryTimeMode0;
@ -1759,80 +1759,84 @@ void NavEKF::FuseVelPosNED() @@ -1759,80 +1759,84 @@ void NavEKF::FuseVelPosNED()
hgtHealth = ((hgtTestRatio < 1.0f) || badIMUdata);
hgtTimeout = (hal.scheduler->millis() - hgtFailTime) > hgtRetryTime;
// Fuse height data if healthy or timed out or in static mode
if (hgtHealth || hgtTimeout || staticMode)
{
if (hgtHealth || hgtTimeout || staticMode) {
hgtHealth = true;
hgtFailTime = hal.scheduler->millis();
// if timed out, reset the height, but do not fuse data on this time step
if (hgtTimeout)
{
if (hgtTimeout) {
ResetHeight();
StoreStatesReset();
fuseHgtData = false;
}
}
else
{
else {
hgtHealth = false;
}
}
// set range for sequential fusion of velocity and position measurements depending on which data is available and its health
if (fuseVelData && _fusionModeGPS == 0 && velHealth && !staticMode)
{
if (fuseVelData && _fusionModeGPS == 0 && velHealth && !staticMode) {
fuseData[0] = true;
fuseData[1] = true;
fuseData[2] = true;
}
if (fuseVelData && _fusionModeGPS == 1 && velHealth && !staticMode)
{
if (fuseVelData && _fusionModeGPS == 1 && velHealth && !staticMode) {
fuseData[0] = true;
fuseData[1] = true;
}
if ((fusePosData && _fusionModeGPS <= 2 && posHealth) || staticMode)
{
if ((fusePosData && _fusionModeGPS <= 2 && posHealth) || staticMode) {
fuseData[3] = true;
fuseData[4] = true;
}
if ((fuseHgtData && hgtHealth) || staticMode)
{
if ((fuseHgtData && hgtHealth) || staticMode) {
fuseData[5] = true;
}
// Limit access to first 14 states when on ground or in static mode.
if (!onGround || staticMode)
{
indexLimit = 21;
}
else
{
indexLimit = 13;
}
// fuse measurements sequentially
for (obsIndex=0; obsIndex<=5; obsIndex++)
{
if (fuseData[obsIndex])
{
for (obsIndex=0; obsIndex<=5; obsIndex++) {
if (fuseData[obsIndex]) {
stateIndex = 4 + obsIndex;
// calculate the measurement innovation, using states from a different time coordinate if fusing height data
if (obsIndex <= 2)
{
innovVelPos[obsIndex] = statesAtVelTime.velocity[obsIndex] - observation[obsIndex];
}
else if (obsIndex == 3 || obsIndex == 4)
{
else if (obsIndex == 3 || obsIndex == 4) {
innovVelPos[obsIndex] = statesAtPosTime.position[obsIndex-3] - observation[obsIndex];
}
else
{
} else {
innovVelPos[obsIndex] = statesAtHgtTime.position[obsIndex-3] - observation[obsIndex];
}
// calculate the Kalman gain and calculate innovation variances
varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex];
SK = 1.0f/varInnovVelPos[obsIndex];
for (uint8_t i= 0; i<=indexLimit; i++)
{
for (uint8_t i= 0; i<=12; i++) {
Kfusion[i] = P[i][stateIndex]*SK;
}
// Only height observations are used to update z accel bias estimate
if (obsIndex == 5) {
Kfusion[13] = P[13][stateIndex]*SK;
} else {
Kfusion[13] = 0.0f;
}
// inhibit wind state estimation by setting Kalman gains to zero
if (!inhibitWindStates) {
Kfusion[14] = P[14][stateIndex]*SK;
Kfusion[15] = P[15][stateIndex]*SK;
} else {
Kfusion[14] = 0.0f;
Kfusion[15] = 0.0f;
}
// inhibit magnetic field state estimation by setting Kalman gains to zero
if (!inhibitMagStates) {
for (uint8_t i = 16; i<=21; i++) {
Kfusion[i] = P[i][stateIndex]*SK;
}
} else {
for (uint8_t i = 16; i<=21; i++) {
Kfusion[i] = 0.0f;
}
}
// Set the Kalman gain values for the single IMU states
Kfusion[22] = Kfusion[13]; // IMU2 Z accel bias
Kfusion[26] = Kfusion[9]; // IMU1 posD
@ -1841,6 +1845,7 @@ void NavEKF::FuseVelPosNED() @@ -1841,6 +1845,7 @@ void NavEKF::FuseVelPosNED()
Kfusion[i+23] = Kfusion[i+4]; // IMU1 velNED
Kfusion[i+27] = Kfusion[i+4]; // IMU2 velNED
}
// Correct states that have been predicted using single (not blended) IMU data
if (obsIndex == 5){
// Calculate height measurement innovations using single IMU states
@ -1850,54 +1855,38 @@ void NavEKF::FuseVelPosNED() @@ -1850,54 +1855,38 @@ void NavEKF::FuseVelPosNED()
float correctionLimit = 0.02f * dtIMU *dtVelPos;
states[13] = states[13] - constrain_float(Kfusion[13] * hgtInnov1, -correctionLimit, correctionLimit); // IMU1 Z accel bias
states[22] = states[22] - constrain_float(Kfusion[22] * hgtInnov2, -correctionLimit, correctionLimit); // IMU2 Z accel bias
for (uint8_t i = 23; i<=26; i++)
{
for (uint8_t i = 23; i<=26; i++) {
states[i] = states[i] - Kfusion[i] * hgtInnov1; // IMU1 velNED,posD
}
for (uint8_t i = 27; i<=30; i++)
{
for (uint8_t i = 27; i<=30; i++) {
states[i] = states[i] - Kfusion[i] * hgtInnov2; // IMU2 velNED,posD
}
} else if (obsIndex == 0 || obsIndex == 1 || obsIndex == 2){
// don't modify the Z accel bias states when fusing GPS velocity measurements
Kfusion[13] = 0.0f;
Kfusion[22] = 0.0f;
} else if (obsIndex == 0 || obsIndex == 1 || obsIndex == 2) {
// Correct single IMU prediction states using velocity measurements
for (uint8_t i = 23; i<=26; i++)
{
for (uint8_t i = 23; i<=26; i++) {
states[i] = states[i] - Kfusion[i] * velInnov1[obsIndex]; // IMU1 velNED,posD
}
for (uint8_t i = 27; i<=30; i++)
{
for (uint8_t i = 27; i<=30; i++) {
states[i] = states[i] - Kfusion[i] * velInnov2[obsIndex]; // IMU2 velNED,posD
}
} else {
// don't modify the Z accel bias states for IMU1 and IMU2 when fusing GPS horizontal position measurements
Kfusion[13] = 0.0f;
Kfusion[22] = 0.0f;
}
// calculate state corrections and re-normalise the quaternions for blended IMU data predicted states
// don't update the Zacc bias state because it has already been updated
for (uint8_t i = 0; i<=indexLimit; i++)
{
if (i != 13) {
states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex];
}
for (uint8_t i = 0; i<=21; i++) {
states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex];
}
state.quat.normalize();
// update the covariance - take advantage of direct observation of a single state at index = stateIndex to reduce computations
// this is a numerically optimised implementation of standard equation P = (I - K*H)*P;
for (uint8_t i= 0; i<=indexLimit; i++)
{
for (uint8_t j= 0; j<=indexLimit; j++)
for (uint8_t i= 0; i<=21; i++) {
for (uint8_t j= 0; j<=21; j++)
{
KHP[i][j] = Kfusion[i] * P[stateIndex][j];
}
}
for (uint8_t i= 0; i<=indexLimit; i++)
{
for (uint8_t j= 0; j<=indexLimit; j++)
{
for (uint8_t i= 0; i<=21; i++) {
for (uint8_t j= 0; j<=21; j++) {
P[i][j] = P[i][j] - KHP[i][j];
}
}
@ -1940,7 +1929,6 @@ void NavEKF::FuseMagnetometer() @@ -1940,7 +1929,6 @@ void NavEKF::FuseMagnetometer()
Vector6 SK_MX;
Vector6 SK_MY;
Vector6 SK_MZ;
uint8_t indexLimit; // used to prevent access to wind and magnetic field states and variances when on ground
// perform sequential fusion of magnetometer measurements.
// this assumes that the errors in the different components are
@ -1950,15 +1938,6 @@ void NavEKF::FuseMagnetometer() @@ -1950,15 +1938,6 @@ void NavEKF::FuseMagnetometer()
// associated with sequential fusion
if (fuseMagData || obsIndex == 1 || obsIndex == 2)
{
// prevent access last 9 states when on ground (acc bias, wind and magnetometer states).
if (!onGround)
{
indexLimit = 21;
}
else
{
indexLimit = 12;
}
// calculate observation jacobians and Kalman gains
if (fuseMagData)
{
@ -2019,7 +1998,7 @@ void NavEKF::FuseMagnetometer() @@ -2019,7 +1998,7 @@ void NavEKF::FuseMagnetometer()
SK_MX[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
SK_MX[4] = 2*q0*q2 - 2*q1*q3;
SK_MX[5] = 2*q0*q3 + 2*q1*q2;
Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][1]*SH_MAG[0] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[3] - P[0][2]*SK_MX[2] - P[0][16]*SK_MX[1] + P[0][17]*SK_MX[5] - P[0][18]*SK_MX[4]);
Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][1]*SH_MAG[0] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[3] - P[0][2]*SK_MX[2] - P[0][16]*SK_MX[1] + P[0][17]*SK_MX[5] - P[0][18]*SK_MX[4]);
Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][1]*SH_MAG[0] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[3] - P[1][2]*SK_MX[2] - P[1][16]*SK_MX[1] + P[1][17]*SK_MX[5] - P[1][18]*SK_MX[4]);
Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][1]*SH_MAG[0] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[3] - P[2][2]*SK_MX[2] - P[2][16]*SK_MX[1] + P[2][17]*SK_MX[5] - P[2][18]*SK_MX[4]);
Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][1]*SH_MAG[0] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[3] - P[3][2]*SK_MX[2] - P[3][16]*SK_MX[1] + P[3][17]*SK_MX[5] - P[3][18]*SK_MX[4]);
@ -2034,14 +2013,27 @@ void NavEKF::FuseMagnetometer() @@ -2034,14 +2013,27 @@ void NavEKF::FuseMagnetometer()
Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][1]*SH_MAG[0] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[3] - P[12][2]*SK_MX[2] - P[12][16]*SK_MX[1] + P[12][17]*SK_MX[5] - P[12][18]*SK_MX[4]);
// this term has been zeroed to improve stability of the Z accel bias
Kfusion[13] = 0.0f;//SK_MX[0]*(P[13][19] + P[13][1]*SH_MAG[0] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[3] - P[13][2]*SK_MX[2] - P[13][16]*SK_MX[1] + P[13][17]*SK_MX[5] - P[13][18]*SK_MX[4]);
Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[5] - P[14][18]*SK_MX[4]);
Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[5] - P[15][18]*SK_MX[4]);
Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][16]*SK_MX[1] + P[16][17]*SK_MX[5] - P[16][18]*SK_MX[4]);
Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][16]*SK_MX[1] + P[17][17]*SK_MX[5] - P[17][18]*SK_MX[4]);
Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][16]*SK_MX[1] + P[18][17]*SK_MX[5] - P[18][18]*SK_MX[4]);
Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][16]*SK_MX[1] + P[19][17]*SK_MX[5] - P[19][18]*SK_MX[4]);
Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]);
Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]);
// zero Kalman gains to inhibit wind state estimation
if (!inhibitWindStates) {
Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[5] - P[14][18]*SK_MX[4]);
Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[5] - P[15][18]*SK_MX[4]);
} else {
Kfusion[14] = 0.0;
Kfusion[15] = 0.0;
}
// zero Kalman gains to inhibit magnetic field state estimation
if (!inhibitMagStates) {
Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][16]*SK_MX[1] + P[16][17]*SK_MX[5] - P[16][18]*SK_MX[4]);
Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][16]*SK_MX[1] + P[17][17]*SK_MX[5] - P[17][18]*SK_MX[4]);
Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][16]*SK_MX[1] + P[18][17]*SK_MX[5] - P[18][18]*SK_MX[4]);
Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][16]*SK_MX[1] + P[19][17]*SK_MX[5] - P[19][18]*SK_MX[4]);
Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]);
Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]);
} else {
for (uint8_t i=16; i<=21; i++) {
Kfusion[i] = 0.0f;
}
}
// calculate the observation innovation variance
varInnovMag[0] = 1.0f/SK_MX[0];
@ -2049,7 +2041,7 @@ void NavEKF::FuseMagnetometer() @@ -2049,7 +2041,7 @@ void NavEKF::FuseMagnetometer()
// reset the observation index to 0 (we start by fusing the X measurement)
obsIndex = 0;
// set flags to indicate to other processes that fusion has been performede and is required on the next frame
// set flags to indicate to other processes that fusion has been perfromed and is required on the next frame
// this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
magFusePerformed = true;
magFuseRequired = true;
@ -2071,9 +2063,9 @@ void NavEKF::FuseMagnetometer() @@ -2071,9 +2063,9 @@ void NavEKF::FuseMagnetometer()
SK_MY[0] = 1/(P[20][20] + R_MAG + P[0][20]*SH_MAG[2] + P[1][20]*SH_MAG[1] + P[2][20]*SH_MAG[0] - P[17][20]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[20][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[17][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][16]*(2*q0*q3 - 2*q1*q2) + P[18][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[20][18] + P[0][18]*SH_MAG[2] + P[1][18]*SH_MAG[1] + P[2][18]*SH_MAG[0] - P[17][18]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][18]*(2*q0*q3 - 2*q1*q2) + P[18][18]*(2*q0*q1 + 2*q2*q3) - P[3][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[17][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][3]*(2*q0*q3 - 2*q1*q2) + P[18][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[16][20]*(2*q0*q3 - 2*q1*q2) + P[18][20]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[20][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[17][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][0]*(2*q0*q3 - 2*q1*q2) + P[18][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[17][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][1]*(2*q0*q3 - 2*q1*q2) + P[18][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[17][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][2]*(2*q0*q3 - 2*q1*q2) + P[18][2]*(2*q0*q1 + 2*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[17][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][17]*(2*q0*q3 - 2*q1*q2) + P[18][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6];
SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
SK_MY[3] = 2*q0*q3 - 2*q1*q2;
SK_MY[3] = 2*q0*q3 - 2*q1*q2;
SK_MY[4] = 2*q0*q1 + 2*q2*q3;
Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][0]*SH_MAG[2] + P[0][1]*SH_MAG[1] + P[0][2]*SH_MAG[0] - P[0][3]*SK_MY[2] - P[0][17]*SK_MY[1] - P[0][16]*SK_MY[3] + P[0][18]*SK_MY[4]);
Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][0]*SH_MAG[2] + P[0][1]*SH_MAG[1] + P[0][2]*SH_MAG[0] - P[0][3]*SK_MY[2] - P[0][17]*SK_MY[1] - P[0][16]*SK_MY[3] + P[0][18]*SK_MY[4]);
Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][0]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[1][2]*SH_MAG[0] - P[1][3]*SK_MY[2] - P[1][17]*SK_MY[1] - P[1][16]*SK_MY[3] + P[1][18]*SK_MY[4]);
Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][0]*SH_MAG[2] + P[2][1]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[2][3]*SK_MY[2] - P[2][17]*SK_MY[1] - P[2][16]*SK_MY[3] + P[2][18]*SK_MY[4]);
Kfusion[3] = SK_MY[0]*(P[3][20] + P[3][0]*SH_MAG[2] + P[3][1]*SH_MAG[1] + P[3][2]*SH_MAG[0] - P[3][3]*SK_MY[2] - P[3][17]*SK_MY[1] - P[3][16]*SK_MY[3] + P[3][18]*SK_MY[4]);
@ -2088,14 +2080,27 @@ void NavEKF::FuseMagnetometer() @@ -2088,14 +2080,27 @@ void NavEKF::FuseMagnetometer()
Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][17]*SK_MY[1] - P[12][16]*SK_MY[3] + P[12][18]*SK_MY[4]);
// this term has been zeroed to improve stability of the Z accel bias
Kfusion[13] = 0.0f;//SK_MY[0]*(P[13][20] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][17]*SK_MY[1] - P[13][16]*SK_MY[3] + P[13][18]*SK_MY[4]);
Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]);
Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]);
Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][17]*SK_MY[1] - P[16][16]*SK_MY[3] + P[16][18]*SK_MY[4]);
Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][17]*SK_MY[1] - P[17][16]*SK_MY[3] + P[17][18]*SK_MY[4]);
Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][17]*SK_MY[1] - P[18][16]*SK_MY[3] + P[18][18]*SK_MY[4]);
Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][17]*SK_MY[1] - P[19][16]*SK_MY[3] + P[19][18]*SK_MY[4]);
Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]);
Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]);
// zero Kalman gains to inhibit wind state estimation
if (!inhibitWindStates) {
Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]);
Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]);
} else {
Kfusion[14] = 0.0;
Kfusion[15] = 0.0;
}
// zero Kalman gains to inhibit magnetic field state estimation
if (!inhibitMagStates) {
Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][17]*SK_MY[1] - P[16][16]*SK_MY[3] + P[16][18]*SK_MY[4]);
Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][17]*SK_MY[1] - P[17][16]*SK_MY[3] + P[17][18]*SK_MY[4]);
Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][17]*SK_MY[1] - P[18][16]*SK_MY[3] + P[18][18]*SK_MY[4]);
Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][17]*SK_MY[1] - P[19][16]*SK_MY[3] + P[19][18]*SK_MY[4]);
Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]);
Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]);
} else {
for (uint8_t i=16; i<=21; i++) {
Kfusion[i] = 0.0f;
}
}
// calculate the observation innovation variance
varInnovMag[1] = 1.0f/SK_MY[0];
@ -2140,14 +2145,27 @@ void NavEKF::FuseMagnetometer() @@ -2140,14 +2145,27 @@ void NavEKF::FuseMagnetometer()
Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][0]*SH_MAG[1] + P[12][3]*SH_MAG[0] - P[12][1]*SK_MZ[2] + P[12][2]*SK_MZ[3] + P[12][18]*SK_MZ[1] + P[12][16]*SK_MZ[5] - P[12][17]*SK_MZ[4]);
// this term has been zeroed to improve stability of the Z accel bias
Kfusion[13] = 0.0f;//SK_MZ[0]*(P[13][21] + P[13][0]*SH_MAG[1] + P[13][3]*SH_MAG[0] - P[13][1]*SK_MZ[2] + P[13][2]*SK_MZ[3] + P[13][18]*SK_MZ[1] + P[13][16]*SK_MZ[5] - P[13][17]*SK_MZ[4]);
Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] + P[14][3]*SH_MAG[0] - P[14][1]*SK_MZ[2] + P[14][2]*SK_MZ[3] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[5] - P[14][17]*SK_MZ[4]);
Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] + P[15][3]*SH_MAG[0] - P[15][1]*SK_MZ[2] + P[15][2]*SK_MZ[3] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[5] - P[15][17]*SK_MZ[4]);
Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][0]*SH_MAG[1] + P[16][3]*SH_MAG[0] - P[16][1]*SK_MZ[2] + P[16][2]*SK_MZ[3] + P[16][18]*SK_MZ[1] + P[16][16]*SK_MZ[5] - P[16][17]*SK_MZ[4]);
Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][0]*SH_MAG[1] + P[17][3]*SH_MAG[0] - P[17][1]*SK_MZ[2] + P[17][2]*SK_MZ[3] + P[17][18]*SK_MZ[1] + P[17][16]*SK_MZ[5] - P[17][17]*SK_MZ[4]);
Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][0]*SH_MAG[1] + P[18][3]*SH_MAG[0] - P[18][1]*SK_MZ[2] + P[18][2]*SK_MZ[3] + P[18][18]*SK_MZ[1] + P[18][16]*SK_MZ[5] - P[18][17]*SK_MZ[4]);
Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][0]*SH_MAG[1] + P[19][3]*SH_MAG[0] - P[19][1]*SK_MZ[2] + P[19][2]*SK_MZ[3] + P[19][18]*SK_MZ[1] + P[19][16]*SK_MZ[5] - P[19][17]*SK_MZ[4]);
Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[5] - P[20][17]*SK_MZ[4]);
Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] + P[21][3]*SH_MAG[0] - P[21][1]*SK_MZ[2] + P[21][2]*SK_MZ[3] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[5] - P[21][17]*SK_MZ[4]);
// zero Kalman gains to inhibit wind state estimation
if (!inhibitWindStates) {
Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] + P[14][3]*SH_MAG[0] - P[14][1]*SK_MZ[2] + P[14][2]*SK_MZ[3] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[5] - P[14][17]*SK_MZ[4]);
Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] + P[15][3]*SH_MAG[0] - P[15][1]*SK_MZ[2] + P[15][2]*SK_MZ[3] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[5] - P[15][17]*SK_MZ[4]);
} else {
Kfusion[14] = 0.0;
Kfusion[15] = 0.0;
}
// zero Kalman gains to inhibit magnetic field state estimation
if (!inhibitMagStates) {
Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][0]*SH_MAG[1] + P[16][3]*SH_MAG[0] - P[16][1]*SK_MZ[2] + P[16][2]*SK_MZ[3] + P[16][18]*SK_MZ[1] + P[16][16]*SK_MZ[5] - P[16][17]*SK_MZ[4]);
Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][0]*SH_MAG[1] + P[17][3]*SH_MAG[0] - P[17][1]*SK_MZ[2] + P[17][2]*SK_MZ[3] + P[17][18]*SK_MZ[1] + P[17][16]*SK_MZ[5] - P[17][17]*SK_MZ[4]);
Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][0]*SH_MAG[1] + P[18][3]*SH_MAG[0] - P[18][1]*SK_MZ[2] + P[18][2]*SK_MZ[3] + P[18][18]*SK_MZ[1] + P[18][16]*SK_MZ[5] - P[18][17]*SK_MZ[4]);
Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][0]*SH_MAG[1] + P[19][3]*SH_MAG[0] - P[19][1]*SK_MZ[2] + P[19][2]*SK_MZ[3] + P[19][18]*SK_MZ[1] + P[19][16]*SK_MZ[5] - P[19][17]*SK_MZ[4]);
Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[5] - P[20][17]*SK_MZ[4]);
Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] + P[21][3]*SH_MAG[0] - P[21][1]*SK_MZ[2] + P[21][2]*SK_MZ[3] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[5] - P[21][17]*SK_MZ[4]);
} else {
for (uint8_t i=16; i<=21; i++) {
Kfusion[i] = 0.0f;
}
}
// calculate the observation innovation variance
varInnovMag[2] = 1.0f/SK_MZ[0];
@ -2161,15 +2179,13 @@ void NavEKF::FuseMagnetometer() @@ -2161,15 +2179,13 @@ void NavEKF::FuseMagnetometer()
innovMag[obsIndex] = MagPred[obsIndex] - magData[obsIndex];
// calculate the innovation test ratio
magTestRatio[obsIndex] = sq(innovMag[obsIndex]) / (sq(_magInnovGate) * varInnovMag[obsIndex]);
// check the last values from all componenets and set magnetometer health accordingly
// check the last values from all components and set magnetometer health accordingly
magHealth = (magTestRatio[0] < 1.0f && magTestRatio[1] < 1.0f && magTestRatio[2] < 1.0f);
// Don't fuse unless all componenets pass. The exception is if the bad health has timed out and we are not a fly forward vehicle
// In this case we might as well try using the magnetometer, but with a reduced weighting
if (magHealth || ((magTestRatio[obsIndex] < 1.0f) && !assume_zero_sideslip() && magTimeout))
{
if (magHealth || ((magTestRatio[obsIndex] < 1.0f) && !assume_zero_sideslip() && magTimeout)) {
// correct the state vector
for (uint8_t j= 0; j<=indexLimit; j++)
{
for (uint8_t j= 0; j<=21; j++) {
// If we are forced to use a bad compass, we reduce the weighting by a factor of 4
if (!magHealth) {
Kfusion[j] *= 0.25f;
@ -2181,50 +2197,38 @@ void NavEKF::FuseMagnetometer() @@ -2181,50 +2197,38 @@ void NavEKF::FuseMagnetometer()
// correct the covariance P = (I - K*H)*P
// take advantage of the empty columns in KH to reduce the
// number of operations
for (uint8_t i = 0; i<=indexLimit; i++)
{
for (uint8_t j = 0; j<=3; j++)
{
for (uint8_t i = 0; i<=21; i++) {
for (uint8_t j = 0; j<=3; j++) {
KH[i][j] = Kfusion[i] * H_MAG[j];
}
for (uint8_t j = 4; j<=15; j++) KH[i][j] = 0.0f;
if (!onGround)
{
for (uint8_t j = 16; j<=21; j++)
{
for (uint8_t j = 4; j<=15; j++) {
KH[i][j] = 0.0f;
}
if (!inhibitMagStates) {
for (uint8_t j = 16; j<=21; j++) {
KH[i][j] = Kfusion[i] * H_MAG[j];
}
}
else
{
for (uint8_t j = 16; j<=21; j++)
{
} else {
for (uint8_t j = 16; j<=21; j++) {
KH[i][j] = 0.0f;
}
}
}
for (uint8_t i = 0; i<=indexLimit; i++)
{
for (uint8_t j = 0; j<=indexLimit; j++)
{
for (uint8_t i = 0; i<=21; i++) {
for (uint8_t j = 0; j<=21; j++) {
KHP[i][j] = 0;
for (uint8_t k = 0; k<=3; k++)
{
for (uint8_t k = 0; k<=3; k++) {
KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
}
if (!onGround)
{
for (uint8_t k = 16; k<=21; k++)
{
if (!inhibitMagStates) {
for (uint8_t k = 16; k<=21; k++) {
KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
}
}
}
}
for (uint8_t i = 0; i<=indexLimit; i++)
{
for (uint8_t j = 0; j<=indexLimit; j++)
{
for (uint8_t i = 0; i<=21; i++) {
for (uint8_t j = 0; j<=21; j++) {
P[i][j] = P[i][j] - KHP[i][j];
}
}
@ -2308,12 +2312,19 @@ void NavEKF::FuseAirspeed() @@ -2308,12 +2312,19 @@ void NavEKF::FuseAirspeed()
Kfusion[13] = 0.0f;//SK_TAS*(P[13][4]*SH_TAS[2] - P[13][14]*SH_TAS[2] + P[13][5]*SH_TAS[1] - P[13][15]*SH_TAS[1] + P[13][6]*vd*SH_TAS[0]);
Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][14]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][15]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]);
Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][14]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][15]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]);
Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][14]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][15]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]);
Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][14]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][15]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]);
Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][14]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][15]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]);
Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][14]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][15]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]);
Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][14]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][15]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]);
Kfusion[21] = SK_TAS*(P[21][4]*SH_TAS[2] - P[21][14]*SH_TAS[2] + P[21][5]*SH_TAS[1] - P[21][15]*SH_TAS[1] + P[21][6]*vd*SH_TAS[0]);
// zero Kalman gains to inhibit magnetic field state estimation
if (!inhibitMagStates) {
Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][14]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][15]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]);
Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][14]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][15]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]);
Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][14]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][15]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]);
Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][14]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][15]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]);
Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][14]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][15]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]);
Kfusion[21] = SK_TAS*(P[21][4]*SH_TAS[2] - P[21][14]*SH_TAS[2] + P[21][5]*SH_TAS[1] - P[21][15]*SH_TAS[1] + P[21][6]*vd*SH_TAS[0]);
} else {
for (uint8_t i=16; i<=21; i++) {
Kfusion[i] = 0.0f;
}
}
// calculate measurement innovation variance
varInnovVtas = 1.0f/SK_TAS;
@ -2493,12 +2504,19 @@ void NavEKF::FuseSideslip() @@ -2493,12 +2504,19 @@ void NavEKF::FuseSideslip()
Kfusion[13] = 0.0f;//SK_BETA[0]*(P[13][0]*SK_BETA[5] + P[13][1]*SK_BETA[4] - P[13][4]*SK_BETA[1] + P[13][5]*SK_BETA[2] + P[13][2]*SK_BETA[6] + P[13][6]*SK_BETA[3] - P[13][3]*SK_BETA[7] + P[13][14]*SK_BETA[1] - P[13][15]*SK_BETA[2]);
Kfusion[14] = SK_BETA[0]*(P[14][0]*SK_BETA[5] + P[14][1]*SK_BETA[4] - P[14][4]*SK_BETA[1] + P[14][5]*SK_BETA[2] + P[14][2]*SK_BETA[6] + P[14][6]*SK_BETA[3] - P[14][3]*SK_BETA[7] + P[14][14]*SK_BETA[1] - P[14][15]*SK_BETA[2]);
Kfusion[15] = SK_BETA[0]*(P[15][0]*SK_BETA[5] + P[15][1]*SK_BETA[4] - P[15][4]*SK_BETA[1] + P[15][5]*SK_BETA[2] + P[15][2]*SK_BETA[6] + P[15][6]*SK_BETA[3] - P[15][3]*SK_BETA[7] + P[15][14]*SK_BETA[1] - P[15][15]*SK_BETA[2]);
Kfusion[16] = SK_BETA[0]*(P[16][0]*SK_BETA[5] + P[16][1]*SK_BETA[4] - P[16][4]*SK_BETA[1] + P[16][5]*SK_BETA[2] + P[16][2]*SK_BETA[6] + P[16][6]*SK_BETA[3] - P[16][3]*SK_BETA[7] + P[16][14]*SK_BETA[1] - P[16][15]*SK_BETA[2]);
Kfusion[17] = SK_BETA[0]*(P[17][0]*SK_BETA[5] + P[17][1]*SK_BETA[4] - P[17][4]*SK_BETA[1] + P[17][5]*SK_BETA[2] + P[17][2]*SK_BETA[6] + P[17][6]*SK_BETA[3] - P[17][3]*SK_BETA[7] + P[17][14]*SK_BETA[1] - P[17][15]*SK_BETA[2]);
Kfusion[18] = SK_BETA[0]*(P[18][0]*SK_BETA[5] + P[18][1]*SK_BETA[4] - P[18][4]*SK_BETA[1] + P[18][5]*SK_BETA[2] + P[18][2]*SK_BETA[6] + P[18][6]*SK_BETA[3] - P[18][3]*SK_BETA[7] + P[18][14]*SK_BETA[1] - P[18][15]*SK_BETA[2]);
Kfusion[19] = SK_BETA[0]*(P[19][0]*SK_BETA[5] + P[19][1]*SK_BETA[4] - P[19][4]*SK_BETA[1] + P[19][5]*SK_BETA[2] + P[19][2]*SK_BETA[6] + P[19][6]*SK_BETA[3] - P[19][3]*SK_BETA[7] + P[19][14]*SK_BETA[1] - P[19][15]*SK_BETA[2]);
Kfusion[20] = SK_BETA[0]*(P[20][0]*SK_BETA[5] + P[20][1]*SK_BETA[4] - P[20][4]*SK_BETA[1] + P[20][5]*SK_BETA[2] + P[20][2]*SK_BETA[6] + P[20][6]*SK_BETA[3] - P[20][3]*SK_BETA[7] + P[20][14]*SK_BETA[1] - P[20][15]*SK_BETA[2]);
Kfusion[21] = SK_BETA[0]*(P[21][0]*SK_BETA[5] + P[21][1]*SK_BETA[4] - P[21][4]*SK_BETA[1] + P[21][5]*SK_BETA[2] + P[21][2]*SK_BETA[6] + P[21][6]*SK_BETA[3] - P[21][3]*SK_BETA[7] + P[21][14]*SK_BETA[1] - P[21][15]*SK_BETA[2]);
// zero Kalman gains to inhibit magnetic field state estimation
if (!inhibitMagStates) {
Kfusion[16] = SK_BETA[0]*(P[16][0]*SK_BETA[5] + P[16][1]*SK_BETA[4] - P[16][4]*SK_BETA[1] + P[16][5]*SK_BETA[2] + P[16][2]*SK_BETA[6] + P[16][6]*SK_BETA[3] - P[16][3]*SK_BETA[7] + P[16][14]*SK_BETA[1] - P[16][15]*SK_BETA[2]);
Kfusion[17] = SK_BETA[0]*(P[17][0]*SK_BETA[5] + P[17][1]*SK_BETA[4] - P[17][4]*SK_BETA[1] + P[17][5]*SK_BETA[2] + P[17][2]*SK_BETA[6] + P[17][6]*SK_BETA[3] - P[17][3]*SK_BETA[7] + P[17][14]*SK_BETA[1] - P[17][15]*SK_BETA[2]);
Kfusion[18] = SK_BETA[0]*(P[18][0]*SK_BETA[5] + P[18][1]*SK_BETA[4] - P[18][4]*SK_BETA[1] + P[18][5]*SK_BETA[2] + P[18][2]*SK_BETA[6] + P[18][6]*SK_BETA[3] - P[18][3]*SK_BETA[7] + P[18][14]*SK_BETA[1] - P[18][15]*SK_BETA[2]);
Kfusion[19] = SK_BETA[0]*(P[19][0]*SK_BETA[5] + P[19][1]*SK_BETA[4] - P[19][4]*SK_BETA[1] + P[19][5]*SK_BETA[2] + P[19][2]*SK_BETA[6] + P[19][6]*SK_BETA[3] - P[19][3]*SK_BETA[7] + P[19][14]*SK_BETA[1] - P[19][15]*SK_BETA[2]);
Kfusion[20] = SK_BETA[0]*(P[20][0]*SK_BETA[5] + P[20][1]*SK_BETA[4] - P[20][4]*SK_BETA[1] + P[20][5]*SK_BETA[2] + P[20][2]*SK_BETA[6] + P[20][6]*SK_BETA[3] - P[20][3]*SK_BETA[7] + P[20][14]*SK_BETA[1] - P[20][15]*SK_BETA[2]);
Kfusion[21] = SK_BETA[0]*(P[21][0]*SK_BETA[5] + P[21][1]*SK_BETA[4] - P[21][4]*SK_BETA[1] + P[21][5]*SK_BETA[2] + P[21][2]*SK_BETA[6] + P[21][6]*SK_BETA[3] - P[21][3]*SK_BETA[7] + P[21][14]*SK_BETA[1] - P[21][15]*SK_BETA[2]);
} else {
for (uint8_t i=16; i<=21; i++) {
Kfusion[i] = 0.0f;
}
}
// calculate predicted sideslip angle and innovation using small angle approximation
innovBeta = vel_rel_wind.y / vel_rel_wind.x;
@ -2733,7 +2751,7 @@ bool NavEKF::getLLH(struct Location &loc) const @@ -2733,7 +2751,7 @@ bool NavEKF::getLLH(struct Location &loc) const
}
// calculate whether the flight vehicle is on the ground or flying from height, airspeed and GPS speed
void NavEKF::OnGroundCheck()
void NavEKF::SetFlightAndFusionModes()
{
const AP_Airspeed *airspeed = _ahrs->get_airspeed();
uint8_t highAirSpd = (airspeed && airspeed->use() && airspeed->get_airspeed() * airspeed->get_EAS2TAS() > 8.0f);
@ -2743,8 +2761,9 @@ void NavEKF::OnGroundCheck() @@ -2743,8 +2761,9 @@ void NavEKF::OnGroundCheck()
uint8_t highGndSpdStage3 = (uint8_t)(gndSpdSq > 81.0f);
uint8_t largeHgt = (uint8_t)(fabsf(hgtMea) > 15.0f);
uint8_t inAirSum = highAirSpd + highGndSpdStage1 + highGndSpdStage2 + highGndSpdStage3 + largeHgt;
// inhibit onGround mode if magnetometer calibration is enabled, movement is detected and static mode isn't demanded
if ((_magCal == 1) && (accNavMagHoriz > 0.5f) && !static_mode_demanded() && use_compass()) {
// if magnetometer calibration mode 1 is selected, use a manoeuvre threshold test
// otherwise use a height and velocity test
if ((_magCal == 1) && (accNavMagHoriz > 0.5f) && !static_mode_demanded() && use_compass()) {
onGround = false;
} else {
// detect on-ground to in-air transition
@ -2769,7 +2788,12 @@ void NavEKF::OnGroundCheck() @@ -2769,7 +2788,12 @@ void NavEKF::OnGroundCheck()
setWindVelStates();
}
}
// store current on-ground status for next time
prevOnGround = onGround;
// If we are on ground, or in static mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states
inhibitWindStates = ((!useAirspeed() && !assume_zero_sideslip()) || onGround || staticMode);
// If magnetometer calibration mode is turned off by the user or we are on ground or in static mode, then inhibit magnetometer states
inhibitMagStates = ((_magCal == 2) || !use_compass() || onGround || staticMode);
}
// initialise the covariance matrix
@ -2803,14 +2827,14 @@ void NavEKF::CovarianceInit() @@ -2803,14 +2827,14 @@ void NavEKF::CovarianceInit()
// Z delta velocity bias
P[13][13] = 0.0f;
// wind velocities
P[14][14] = sq(8.0f);
P[14][14] = 0.0f;
P[15][15] = P[14][14];
// earth magnetic field
P[16][16] = sq(0.001f);
P[16][16] = 0.0f;
P[17][17] = P[16][16];
P[18][18] = P[16][16];
// body magnetic field
P[19][19] = sq(0.001f);
P[19][19] = 0.0f;
P[20][20] = P[19][19];
P[21][21] = P[19][19];
}
@ -2838,65 +2862,16 @@ void NavEKF::ForceSymmetry() @@ -2838,65 +2862,16 @@ void NavEKF::ForceSymmetry()
// copy covariances across from covariance prediction calculation and fix numerical errors
void NavEKF::CopyAndFixCovariances()
{
// if we are on-ground or in static mode, we want all the off-diagonals for the wind
// and magnetic field states to remain zero and want to keep the old variances
// for these states
if (onGround || staticMode) {
// copy calculated variances we want to propagate
for (uint8_t i=0; i<=13; i++) {
P[i][i] = nextP[i][i];
}
// force covariances to be either zero or symetrical
for (uint8_t i=1; i<=21; i++)
{
for (uint8_t j=0; j<=i-1; j++)
{
if ((i >= 14) || (j >= 14)) {
P[i][j] = 0.0f;
} else {
P[i][j] = 0.5f*(nextP[i][j] + nextP[j][i]);
}
P[j][i] = P[i][j];
}
}
}
// if we have a non-forward flight vehicle type and no airspeed sensor, we want the wind
// states to remain zero and want to keep the old variances for these states
else if (!useAirspeed() && !assume_zero_sideslip()) {
// copy calculated variances we want to propagate
for (uint8_t i=0; i<=13; i++) {
P[i][i] = nextP[i][i];
}
for (uint8_t i=16; i<=21; i++) {
P[i][i] = nextP[i][i];
}
// force covariances to be either zero or symetrical
for (uint8_t i=1; i<=21; i++)
{
for (uint8_t j=0; j<=i-1; j++)
{
if (i == 14 || i == 15 || j == 14 || j == 15) {
P[i][j] = 0.0f;
} else {
P[i][j] = 0.5f*(nextP[i][j] + nextP[j][i]);
}
P[j][i] = P[i][j];
}
}
// copy predicted variances
for (uint8_t i=0; i<=21; i++) {
P[i][i] = nextP[i][i];
}
// if we are flying with all sensors then all covariance terms are active
else {
// copy calculated variances we want to propagate
for (uint8_t i=0; i<=21; i++) {
P[i][i] = nextP[i][i];
}
for (uint8_t i=1; i<=21; i++)
// copy predicted covariances and force symmetry
for (uint8_t i=1; i<=21; i++) {
for (uint8_t j=0; j<=i-1; j++)
{
for (uint8_t j=0; j<=i-1; j++)
{
P[i][j] = 0.5f*(nextP[i][j] + nextP[j][i]);
P[j][i] = P[i][j];
}
P[i][j] = 0.5f*(nextP[i][j] + nextP[j][i]);
P[j][i] = P[i][j];
}
}
}
@ -3205,7 +3180,7 @@ void NavEKF::setWindVelStates() @@ -3205,7 +3180,7 @@ void NavEKF::setWindVelStates()
{
float gndSpd = sqrtf(sq(states[4]) + sq(states[5]));
if (gndSpd > 4.0f) {
// set the wind states to be the reciprocal of the velocity and scale to 6 m/s
// set the wind states to be the reciprocal of the velocity and scale
float scaleFactor = STARTUP_WIND_SPEED / gndSpd;
states[14] = - states[4] * scaleFactor;
states[15] = - states[5] * scaleFactor;
@ -3265,6 +3240,7 @@ void NavEKF::ZeroVariables() @@ -3265,6 +3240,7 @@ void NavEKF::ZeroVariables()
lastStateStoreTime_ms = 0;
lastFixTime_ms = 0;
secondLastFixTime_ms = 0;
lastDivergeTime_ms = 0;
lastMagUpdate = 0;
lastAirspeedUpdate = 0;
velFailTime = 0;
@ -3351,4 +3327,21 @@ bool NavEKF::assume_zero_sideslip(void) const @@ -3351,4 +3327,21 @@ bool NavEKF::assume_zero_sideslip(void) const
return _ahrs->get_fly_forward() && _ahrs->get_vehicle_class() != AHRS_VEHICLE_GROUND;
}
// Check for filter divergence
void NavEKF::checkDivergence()
{
// If position, velocity and magnetometer measurements have all diverged, then fail for 10 seconds
// This is designed to catch a filter divergence and persist for long enough to prevent a badly oscillating solution from being periodically declared healthy
bool divergenceDetected = ((posTestRatio > 1.0f) && (velTestRatio > 1.0f) && (magTestRatio.x > 1.0f) && (magTestRatio.y > 1.0f) && (magTestRatio.z > 1.0f));
bool divergenceTimeout = (hal.scheduler->millis() - lastDivergeTime_ms > 10000);
if (!divergenceTimeout) {
if (divergenceDetected) {
lastDivergeTime_ms = hal.scheduler->millis();
}
filterDiverged = true;
} else {
filterDiverged = false;
}
}
#endif // HAL_CPU_CLASS

11
libraries/AP_NavEKF/AP_NavEKF.h
Unescape View File

@ -213,7 +213,7 @@ private: @@ -213,7 +213,7 @@ private:
void calcEarthRateNED(Vector3f &omega, int32_t latitude) const;
// calculate whether the flight vehicle is on the ground or flying from height, airspeed and GPS speed
void OnGroundCheck();
void SetFlightAndFusionModes();
// initialise the covariance matrix
void CovarianceInit();
@ -282,6 +282,9 @@ private: @@ -282,6 +282,9 @@ private:
// this allows large GPS position jumps to be accomodated gradually
void decayGpsOffset(void);
// Check for filter divergence
void checkDivergence(void);
// EKF Mavlink Tuneable Parameters
AP_Float _gpsHorizVelNoise; // GPS horizontal velocity measurement noise : m/s
AP_Float _gpsVertVelNoise; // GPS vertical velocity measurement noise : m/s
@ -305,7 +308,7 @@ private: @@ -305,7 +308,7 @@ private:
AP_Int8 _hgtInnovGate; // Number of standard deviations applied to height innovation consistency check
AP_Int8 _magInnovGate; // Number of standard deviations applied to magnetometer innovation consistency check
AP_Int8 _tasInnovGate; // Number of standard deviations applied to true airspeed innovation consistency check
AP_Int8 _magCal; // Forces magnetic field states to be always active to aid magnetometer calibration
AP_Int8 _magCal; // Sets activation condition for in-flight magnetometer calibration
AP_Int16 _gpsGlitchAccelMax; // Maximum allowed discrepancy between inertial and GPS Horizontal acceleration before GPS data is ignored : cm/s^2
AP_Int8 _gpsGlitchRadiusMax; // Maximum allowed discrepancy between inertial and GPS Horizontal position before GPS glitch is declared : m
@ -321,6 +324,7 @@ private: @@ -321,6 +324,7 @@ private:
AP_Int16 _hgtRetryTimeMode0; // height measurement retry time following innovation consistency fail if GPS fusion mode is = 0 (msec)
AP_Int16 _hgtRetryTimeMode12; // height measurement retry time following innovation consistency fail if GPS fusion mode is > 0 (msec)
uint32_t _magFailTimeLimit_ms; // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec)
uint32_t lastDivergeTime_ms; // time in msec divergence of filter last detected
float _gyroBiasNoiseScaler; // scale factor applied to gyro bias state process variance when on ground
float _magVarRateScale; // scale factor applied to magnetometer variance due to angular rate
uint16_t _msecGpsAvg; // average number of msec between GPS measurements
@ -339,6 +343,7 @@ private: @@ -339,6 +343,7 @@ private:
bool posTimeout; // boolean true if position measurements have failed innovation consistency check and timed out
bool hgtTimeout; // boolean true if height measurements have failed innovation consistency check and timed out
bool magTimeout; // boolean true if magnetometer measurements have failed for too long and have timed out
bool filterDiverged; // boolean true if the filter has diverged
Vector31 Kfusion; // Kalman gain vector
Matrix22 KH; // intermediate result used for covariance updates
@ -444,6 +449,8 @@ private: @@ -444,6 +449,8 @@ private:
float hgtTestRatio; // sum of squares of baro height innovation divided by fail threshold
Vector3f magTestRatio; // sum of squares of magnetometer innovations divided by fail threshold
float tasTestRatio; // sum of squares of true airspeed innovation divided by fail threshold
bool inhibitWindStates; // true when wind states and covariances are to remain constant
bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant
// states held by magnetomter fusion across time steps
// magnetometer X,Y,Z measurements are fused across three time steps

Write Preview
Loading…
Cancel
Save