mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-09 17:38:32 -04:00
AP_NavEKF: Filter accuracy and stability improvements
Improvements in PX4 firmware have reduced the computational load and mkae the previous practicwe of splitting magnetometer and optical flow fusion across multiple time steps unnecessary and make it possible to perform a covariance prediction prior to fusing data on the same time step. This patch: 1) Ensures that a covariance prediction is always performed prior to fusion of any observation 2) Removes the splitting of magnetometer fusion so that fusion of the X,Y and Z components occurs on the same time time step 3) Removes the splitting of optical flow fusion so that fusion of X and Y components occurs on the same time step
This commit is contained in:
Paul Riseborough
Andrew Tridgell
parent
de1f7f5e63
commit
c0d23ffc30
@ -725,10 +725,6 @@ void NavEKF::UpdateFilter()
|
||||
// or the time limit will be exceeded at the next IMU update
|
||||
if (((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax))) {
|
||||
CovariancePrediction();
|
||||
covPredStep = true;
|
||||
summedDelAng.zero();
|
||||
summedDelVel.zero();
|
||||
dt = 0.0;
|
||||
} else {
|
||||
covPredStep = false;
|
||||
}
|
||||
@ -877,7 +873,9 @@ void NavEKF::SelectVelPosFusion()
|
||||
|
||||
// perform fusion
|
||||
if (fuseVelData || fusePosData || fuseHgtData) {
|
||||
FuseVelPosNED();
|
||||
// ensure that the covariance prediction is up to date before fusing data
|
||||
if (!covPredStep) CovariancePrediction();
|
||||
FuseVelPosNED();
|
||||
}
|
||||
|
||||
// Fuse corrections to quaternion, position and velocity states across several time steps to reduce 5 and 10Hz pulsing in the output
|
||||
@ -898,6 +896,9 @@ void NavEKF::SelectVelPosFusion()
|
||||
// select fusion of magnetometer data
|
||||
void NavEKF::SelectMagFusion()
|
||||
{
|
||||
// start performance timer
|
||||
perf_begin(_perf_FuseMagnetometer);
|
||||
|
||||
// check for and read new magnetometer measurements
|
||||
readMagData();
|
||||
|
||||
@ -911,22 +912,15 @@ void NavEKF::SelectMagFusion()
|
||||
|
||||
// determine if conditions are right to start a new fusion cycle
|
||||
bool dataReady = statesInitialised && use_compass() && newDataMag;
|
||||
if (dataReady)
|
||||
{
|
||||
fuseMagData = true;
|
||||
if (dataReady) {
|
||||
// reset state updates and counter used to spread fusion updates across several frames to reduce 10Hz pulsing
|
||||
memset(&magIncrStateDelta[0], 0, sizeof(magIncrStateDelta));
|
||||
magUpdateCount = 0;
|
||||
// ensure that the covariance prediction is up to date before fusing data
|
||||
if (!covPredStep) CovariancePrediction();
|
||||
// fuse the three magnetometer componenents sequentially
|
||||
for (mag_state.obsIndex = 0; mag_state.obsIndex <= 2; mag_state.obsIndex++) FuseMagnetometer();
|
||||
}
|
||||
else
|
||||
{
|
||||
fuseMagData = false;
|
||||
}
|
||||
|
||||
// delay if covariance prediction is being performed on this prediction cycle unless load levelling is inhibited
|
||||
if (!covPredStep || inhibitLoadLeveling) {
|
||||
FuseMagnetometer();
|
||||
}
|
||||
|
||||
// Fuse corrections to quaternion, position and velocity states across several time steps to reduce 10Hz pulsing in the output
|
||||
if (magUpdateCount < magUpdateCountMax) {
|
||||
@ -935,11 +929,16 @@ void NavEKF::SelectMagFusion()
|
||||
states[i] += magIncrStateDelta[i];
|
||||
}
|
||||
}
|
||||
|
||||
// stop performance timer
|
||||
perf_end(_perf_FuseMagnetometer);
|
||||
}
|
||||
|
||||
// select fusion of optical flow measurements
|
||||
void NavEKF::SelectFlowFusion()
|
||||
{
|
||||
// start performance timer
|
||||
perf_begin(_perf_FuseOptFlow);
|
||||
// Perform Data Checks
|
||||
// Check if the optical flow data is still valid
|
||||
flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 1000);
|
||||
@ -980,9 +979,7 @@ void NavEKF::SelectFlowFusion()
|
||||
constVelMode = false;
|
||||
lastConstVelMode = false;
|
||||
}
|
||||
// if we do have valid flow measurements
|
||||
|
||||
// Fuse data into a 1-state EKF to estimate terrain height
|
||||
// if we do have valid flow measurements, fuse data into a 1-state EKF to estimate terrain height
|
||||
if ((newDataFlow || newDataRng) && tiltOK) {
|
||||
// fuse range data into the terrain estimator if available
|
||||
fuseRngData = newDataRng;
|
||||
@ -993,11 +990,9 @@ void NavEKF::SelectFlowFusion()
|
||||
// Indicate we have used the range data
|
||||
newDataRng = false;
|
||||
// we don't do subsequent fusion of optical flow data into the main filter if GPS is good and terrain offset data is invalid
|
||||
// because an invalid height above ground estimate will casue the optical flow measurements to fight the GPS
|
||||
// because an invalid height above ground estimate will cause the optical flow measurements to fight the GPS
|
||||
if (!gpsNotAvailable && !gndOffsetValid) {
|
||||
// turn of fusion permissions
|
||||
// reset the measurement axis index
|
||||
flow_state.obsIndex = 0;
|
||||
// turn off fusion permissions
|
||||
// reset the flags to indicate that no new range finder or flow data is available for fusion
|
||||
newDataFlow = false;
|
||||
}
|
||||
@ -1012,27 +1007,16 @@ void NavEKF::SelectFlowFusion()
|
||||
flowUpdateCount = 0;
|
||||
// Set the flow noise used by the fusion processes
|
||||
R_LOS = sq(max(_flowNoise, 0.05f));
|
||||
// set the measurement axis index to fuse the X axis data
|
||||
flow_state.obsIndex = 0;
|
||||
// Fuse the optical flow X axis data into the main filter
|
||||
FuseOptFlow();
|
||||
// increment the measurement axis index to fuse the Y axis data on the next prediction cycle
|
||||
flow_state.obsIndex = 1;
|
||||
// ensure that the covariance prediction is up to date before fusing data
|
||||
if (!covPredStep) CovariancePrediction();
|
||||
// Fuse the optical flow X and Y axis data into the main filter sequentially
|
||||
for (flow_state.obsIndex = 0; flow_state.obsIndex <= 1; flow_state.obsIndex++) FuseOptFlow();
|
||||
// reset flag to indicate that no new flow data is available for fusion
|
||||
newDataFlow = false;
|
||||
// indicate that flow fusion has been performed. This is used for load spreading.
|
||||
flowFusePerformed = true;
|
||||
// update the time stamp
|
||||
prevFlowUseTime_ms = imuSampleTime_ms;
|
||||
} else if (flowDataValid && flow_state.obsIndex == 1 && !delayFusion && !constPosMode && tiltOK) {
|
||||
// Fuse the optical flow Y axis data into the main filter
|
||||
FuseOptFlow();
|
||||
// Reset the measurement axis index to prevent further fusion of this data
|
||||
flow_state.obsIndex = 0;
|
||||
// reset flag to indicate that no new flow data is available for fusion
|
||||
newDataFlow = false;
|
||||
// indicate that flow fusion has been performed. This is used for load spreading.
|
||||
flowFusePerformed = true;
|
||||
}
|
||||
|
||||
// Apply corrections to quaternion, position and velocity states across several time steps to reduce 10Hz pulsing in the output
|
||||
@ -1042,6 +1026,8 @@ void NavEKF::SelectFlowFusion()
|
||||
states[i] += flowIncrStateDelta[i];
|
||||
}
|
||||
}
|
||||
// stop the performance timer
|
||||
perf_end(_perf_FuseOptFlow);
|
||||
}
|
||||
|
||||
// select fusion of true airspeed measurements
|
||||
@ -1064,6 +1050,8 @@ void NavEKF::SelectTasFusion()
|
||||
// setting fuseMeNow to true disables this load spreading feature
|
||||
if (tasDataWaiting && (!(covPredStep || magFusePerformed || flowFusePerformed) || timeout || inhibitLoadLeveling))
|
||||
{
|
||||
// ensure that the covariance prediction is up to date before fusing data
|
||||
if (!covPredStep) CovariancePrediction();
|
||||
FuseAirspeed();
|
||||
TASmsecPrev = imuSampleTime_ms;
|
||||
tasDataWaiting = false;
|
||||
@ -1085,6 +1073,8 @@ void NavEKF::SelectBetaFusion()
|
||||
bool f_feasible = (assume_zero_sideslip() && !inhibitWindStates);
|
||||
// use synthetic sideslip fusion if feasible, required, enough time has lapsed since the last fusion and it is not locked out
|
||||
if (f_feasible && f_required && f_timeTrigger && !f_lockedOut) {
|
||||
// ensure that the covariance prediction is up to date before fusing data
|
||||
if (!covPredStep) CovariancePrediction();
|
||||
FuseSideslip();
|
||||
BETAmsecPrev = imuSampleTime_ms;
|
||||
}
|
||||
@ -1850,6 +1840,12 @@ void NavEKF::CovariancePrediction()
|
||||
// constrain diagonals to prevent ill-conditioning
|
||||
ConstrainVariances();
|
||||
|
||||
// set the flag to indicate that covariance prediction has been performed and reset the increments used by the covariance prediction
|
||||
covPredStep = true;
|
||||
summedDelAng.zero();
|
||||
summedDelVel.zero();
|
||||
dt = 0.0;
|
||||
|
||||
perf_end(_perf_CovariancePrediction);
|
||||
}
|
||||
|
||||
@ -2238,9 +2234,6 @@ void NavEKF::FuseVelPosNED()
|
||||
// fuse each axis on consecutive time steps to spread computional load
|
||||
void NavEKF::FuseMagnetometer()
|
||||
{
|
||||
// start performance timer
|
||||
perf_begin(_perf_FuseMagnetometer);
|
||||
|
||||
// declarations
|
||||
ftype &q0 = mag_state.q0;
|
||||
ftype &q1 = mag_state.q1;
|
||||
@ -2268,363 +2261,350 @@ void NavEKF::FuseMagnetometer()
|
||||
// data fit is the only assumption we can make
|
||||
// so we might as well take advantage of the computational efficiencies
|
||||
// associated with sequential fusion
|
||||
if (fuseMagData || obsIndex == 1 || obsIndex == 2)
|
||||
// calculate observation jacobians and Kalman gains
|
||||
if (obsIndex == 0)
|
||||
{
|
||||
// calculate observation jacobians and Kalman gains
|
||||
if (fuseMagData)
|
||||
{
|
||||
// copy required states to local variable names
|
||||
q0 = statesAtMagMeasTime.quat[0];
|
||||
q1 = statesAtMagMeasTime.quat[1];
|
||||
q2 = statesAtMagMeasTime.quat[2];
|
||||
q3 = statesAtMagMeasTime.quat[3];
|
||||
magN = statesAtMagMeasTime.earth_magfield[0];
|
||||
magE = statesAtMagMeasTime.earth_magfield[1];
|
||||
magD = statesAtMagMeasTime.earth_magfield[2];
|
||||
magXbias = statesAtMagMeasTime.body_magfield[0];
|
||||
magYbias = statesAtMagMeasTime.body_magfield[1];
|
||||
magZbias = statesAtMagMeasTime.body_magfield[2];
|
||||
// copy required states to local variable names
|
||||
q0 = statesAtMagMeasTime.quat[0];
|
||||
q1 = statesAtMagMeasTime.quat[1];
|
||||
q2 = statesAtMagMeasTime.quat[2];
|
||||
q3 = statesAtMagMeasTime.quat[3];
|
||||
magN = statesAtMagMeasTime.earth_magfield[0];
|
||||
magE = statesAtMagMeasTime.earth_magfield[1];
|
||||
magD = statesAtMagMeasTime.earth_magfield[2];
|
||||
magXbias = statesAtMagMeasTime.body_magfield[0];
|
||||
magYbias = statesAtMagMeasTime.body_magfield[1];
|
||||
magZbias = statesAtMagMeasTime.body_magfield[2];
|
||||
|
||||
// rotate predicted earth components into body axes and calculate
|
||||
// predicted measurements
|
||||
DCM[0][0] = q0*q0 + q1*q1 - q2*q2 - q3*q3;
|
||||
DCM[0][1] = 2*(q1*q2 + q0*q3);
|
||||
DCM[0][2] = 2*(q1*q3-q0*q2);
|
||||
DCM[1][0] = 2*(q1*q2 - q0*q3);
|
||||
DCM[1][1] = q0*q0 - q1*q1 + q2*q2 - q3*q3;
|
||||
DCM[1][2] = 2*(q2*q3 + q0*q1);
|
||||
DCM[2][0] = 2*(q1*q3 + q0*q2);
|
||||
DCM[2][1] = 2*(q2*q3 - q0*q1);
|
||||
DCM[2][2] = q0*q0 - q1*q1 - q2*q2 + q3*q3;
|
||||
MagPred[0] = DCM[0][0]*magN + DCM[0][1]*magE + DCM[0][2]*magD + magXbias;
|
||||
MagPred[1] = DCM[1][0]*magN + DCM[1][1]*magE + DCM[1][2]*magD + magYbias;
|
||||
MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE + DCM[2][2]*magD + magZbias;
|
||||
// rotate predicted earth components into body axes and calculate
|
||||
// predicted measurements
|
||||
DCM[0][0] = q0*q0 + q1*q1 - q2*q2 - q3*q3;
|
||||
DCM[0][1] = 2*(q1*q2 + q0*q3);
|
||||
DCM[0][2] = 2*(q1*q3-q0*q2);
|
||||
DCM[1][0] = 2*(q1*q2 - q0*q3);
|
||||
DCM[1][1] = q0*q0 - q1*q1 + q2*q2 - q3*q3;
|
||||
DCM[1][2] = 2*(q2*q3 + q0*q1);
|
||||
DCM[2][0] = 2*(q1*q3 + q0*q2);
|
||||
DCM[2][1] = 2*(q2*q3 - q0*q1);
|
||||
DCM[2][2] = q0*q0 - q1*q1 - q2*q2 + q3*q3;
|
||||
MagPred[0] = DCM[0][0]*magN + DCM[0][1]*magE + DCM[0][2]*magD + magXbias;
|
||||
MagPred[1] = DCM[1][0]*magN + DCM[1][1]*magE + DCM[1][2]*magD + magYbias;
|
||||
MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE + DCM[2][2]*magD + magZbias;
|
||||
|
||||
// scale magnetometer observation error with total angular rate
|
||||
R_MAG = sq(constrain_float(_magNoise, 0.01f, 0.5f)) + sq(magVarRateScale*dAngIMU.length() / dtIMU);
|
||||
// scale magnetometer observation error with total angular rate
|
||||
R_MAG = sq(constrain_float(_magNoise, 0.01f, 0.5f)) + sq(magVarRateScale*dAngIMU.length() / dtIMU);
|
||||
|
||||
// calculate observation jacobians
|
||||
SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1;
|
||||
SH_MAG[1] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;
|
||||
SH_MAG[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;
|
||||
SH_MAG[3] = sq(q3);
|
||||
SH_MAG[4] = sq(q2);
|
||||
SH_MAG[5] = sq(q1);
|
||||
SH_MAG[6] = sq(q0);
|
||||
SH_MAG[7] = 2*magN*q0;
|
||||
SH_MAG[8] = 2*magE*q3;
|
||||
for (uint8_t i=0; i<=21; i++) H_MAG[i] = 0;
|
||||
H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
|
||||
H_MAG[1] = SH_MAG[0];
|
||||
H_MAG[2] = 2*magE*q1 - 2*magD*q0 - 2*magN*q2;
|
||||
H_MAG[3] = SH_MAG[2];
|
||||
H_MAG[16] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6];
|
||||
H_MAG[17] = 2*q0*q3 + 2*q1*q2;
|
||||
H_MAG[18] = 2*q1*q3 - 2*q0*q2;
|
||||
H_MAG[19] = 1;
|
||||
// calculate observation jacobians
|
||||
SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1;
|
||||
SH_MAG[1] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;
|
||||
SH_MAG[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;
|
||||
SH_MAG[3] = sq(q3);
|
||||
SH_MAG[4] = sq(q2);
|
||||
SH_MAG[5] = sq(q1);
|
||||
SH_MAG[6] = sq(q0);
|
||||
SH_MAG[7] = 2*magN*q0;
|
||||
SH_MAG[8] = 2*magE*q3;
|
||||
for (uint8_t i=0; i<=21; i++) H_MAG[i] = 0;
|
||||
H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
|
||||
H_MAG[1] = SH_MAG[0];
|
||||
H_MAG[2] = 2*magE*q1 - 2*magD*q0 - 2*magN*q2;
|
||||
H_MAG[3] = SH_MAG[2];
|
||||
H_MAG[16] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6];
|
||||
H_MAG[17] = 2*q0*q3 + 2*q1*q2;
|
||||
H_MAG[18] = 2*q1*q3 - 2*q0*q2;
|
||||
H_MAG[19] = 1;
|
||||
|
||||
// calculate Kalman gain
|
||||
float temp = (P[19][19] + R_MAG + P[1][19]*SH_MAG[0] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[19][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2*q0*q3 + 2*q1*q2) - P[18][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2*q0*q3 + 2*q1*q2) - P[18][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[19][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2*q0*q3 + 2*q1*q2) - P[18][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[19][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2*q0*q3 + 2*q1*q2) - P[18][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2*q0*q3 + 2*q1*q2) - P[18][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[17][19]*(2*q0*q3 + 2*q1*q2) - P[18][19]*(2*q0*q2 - 2*q1*q3) - P[2][19]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2*q0*q3 + 2*q1*q2) - P[18][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2*q0*q3 + 2*q1*q2) - P[18][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
|
||||
if (temp >= R_MAG) {
|
||||
SK_MX[0] = 1.0f / temp;
|
||||
faultStatus.bad_xmag = false;
|
||||
} else {
|
||||
// the calculation is badly conditioned, so we cannot perform fusion on this step
|
||||
// we increase the state variances and try again next time
|
||||
P[19][19] += 0.1f*R_MAG;
|
||||
obsIndex = 1;
|
||||
faultStatus.bad_xmag = true;
|
||||
return;
|
||||
}
|
||||
SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6];
|
||||
SK_MX[2] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;
|
||||
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[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]);
|
||||
Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][1]*SH_MAG[0] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[3] - P[4][2]*SK_MX[2] - P[4][16]*SK_MX[1] + P[4][17]*SK_MX[5] - P[4][18]*SK_MX[4]);
|
||||
Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][1]*SH_MAG[0] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[3] - P[5][2]*SK_MX[2] - P[5][16]*SK_MX[1] + P[5][17]*SK_MX[5] - P[5][18]*SK_MX[4]);
|
||||
Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][1]*SH_MAG[0] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[3] - P[6][2]*SK_MX[2] - P[6][16]*SK_MX[1] + P[6][17]*SK_MX[5] - P[6][18]*SK_MX[4]);
|
||||
Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][1]*SH_MAG[0] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[3] - P[7][2]*SK_MX[2] - P[7][16]*SK_MX[1] + P[7][17]*SK_MX[5] - P[7][18]*SK_MX[4]);
|
||||
Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][1]*SH_MAG[0] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[3] - P[8][2]*SK_MX[2] - P[8][16]*SK_MX[1] + P[8][17]*SK_MX[5] - P[8][18]*SK_MX[4]);
|
||||
Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][1]*SH_MAG[0] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[3] - P[9][2]*SK_MX[2] - P[9][16]*SK_MX[1] + P[9][17]*SK_MX[5] - P[9][18]*SK_MX[4]);
|
||||
Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][1]*SH_MAG[0] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[3] - P[10][2]*SK_MX[2] - P[10][16]*SK_MX[1] + P[10][17]*SK_MX[5] - P[10][18]*SK_MX[4]);
|
||||
Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][1]*SH_MAG[0] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[3] - P[11][2]*SK_MX[2] - P[11][16]*SK_MX[1] + P[11][17]*SK_MX[5] - P[11][18]*SK_MX[4]);
|
||||
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]);
|
||||
// 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];
|
||||
|
||||
// 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 performed 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;
|
||||
// calculate Kalman gain
|
||||
float temp = (P[19][19] + R_MAG + P[1][19]*SH_MAG[0] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[19][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2*q0*q3 + 2*q1*q2) - P[18][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2*q0*q3 + 2*q1*q2) - P[18][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[19][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2*q0*q3 + 2*q1*q2) - P[18][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[19][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2*q0*q3 + 2*q1*q2) - P[18][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2*q0*q3 + 2*q1*q2) - P[18][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[17][19]*(2*q0*q3 + 2*q1*q2) - P[18][19]*(2*q0*q2 - 2*q1*q3) - P[2][19]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2*q0*q3 + 2*q1*q2) - P[18][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2*q0*q3 + 2*q1*q2) - P[18][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
|
||||
if (temp >= R_MAG) {
|
||||
SK_MX[0] = 1.0f / temp;
|
||||
faultStatus.bad_xmag = false;
|
||||
} else {
|
||||
// the calculation is badly conditioned, so we cannot perform fusion on this step
|
||||
// we increase the state variances and try again next time
|
||||
P[19][19] += 0.1f*R_MAG;
|
||||
obsIndex = 1;
|
||||
faultStatus.bad_xmag = true;
|
||||
return;
|
||||
}
|
||||
else if (obsIndex == 1) // we are now fusing the Y measurement
|
||||
{
|
||||
// calculate observation jacobians
|
||||
for (uint8_t i=0; i<=21; i++) H_MAG[i] = 0;
|
||||
H_MAG[0] = SH_MAG[2];
|
||||
H_MAG[1] = SH_MAG[1];
|
||||
H_MAG[2] = SH_MAG[0];
|
||||
H_MAG[3] = 2*magD*q2 - SH_MAG[8] - SH_MAG[7];
|
||||
H_MAG[16] = 2*q1*q2 - 2*q0*q3;
|
||||
H_MAG[17] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6];
|
||||
H_MAG[18] = 2*q0*q1 + 2*q2*q3;
|
||||
H_MAG[20] = 1;
|
||||
|
||||
// calculate Kalman gain
|
||||
float temp = (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));
|
||||
if (temp >= R_MAG) {
|
||||
SK_MY[0] = 1.0f / temp;
|
||||
faultStatus.bad_ymag = false;
|
||||
} else {
|
||||
// the calculation is badly conditioned, so we cannot perform fusion on this step
|
||||
// we increase the state variances and try again next time
|
||||
P[20][20] += 0.1f*R_MAG;
|
||||
obsIndex = 2;
|
||||
faultStatus.bad_ymag = true;
|
||||
return;
|
||||
}
|
||||
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[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[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]);
|
||||
Kfusion[4] = SK_MY[0]*(P[4][20] + P[4][0]*SH_MAG[2] + P[4][1]*SH_MAG[1] + P[4][2]*SH_MAG[0] - P[4][3]*SK_MY[2] - P[4][17]*SK_MY[1] - P[4][16]*SK_MY[3] + P[4][18]*SK_MY[4]);
|
||||
Kfusion[5] = SK_MY[0]*(P[5][20] + P[5][0]*SH_MAG[2] + P[5][1]*SH_MAG[1] + P[5][2]*SH_MAG[0] - P[5][3]*SK_MY[2] - P[5][17]*SK_MY[1] - P[5][16]*SK_MY[3] + P[5][18]*SK_MY[4]);
|
||||
Kfusion[6] = SK_MY[0]*(P[6][20] + P[6][0]*SH_MAG[2] + P[6][1]*SH_MAG[1] + P[6][2]*SH_MAG[0] - P[6][3]*SK_MY[2] - P[6][17]*SK_MY[1] - P[6][16]*SK_MY[3] + P[6][18]*SK_MY[4]);
|
||||
Kfusion[7] = SK_MY[0]*(P[7][20] + P[7][0]*SH_MAG[2] + P[7][1]*SH_MAG[1] + P[7][2]*SH_MAG[0] - P[7][3]*SK_MY[2] - P[7][17]*SK_MY[1] - P[7][16]*SK_MY[3] + P[7][18]*SK_MY[4]);
|
||||
Kfusion[8] = SK_MY[0]*(P[8][20] + P[8][0]*SH_MAG[2] + P[8][1]*SH_MAG[1] + P[8][2]*SH_MAG[0] - P[8][3]*SK_MY[2] - P[8][17]*SK_MY[1] - P[8][16]*SK_MY[3] + P[8][18]*SK_MY[4]);
|
||||
Kfusion[9] = SK_MY[0]*(P[9][20] + P[9][0]*SH_MAG[2] + P[9][1]*SH_MAG[1] + P[9][2]*SH_MAG[0] - P[9][3]*SK_MY[2] - P[9][17]*SK_MY[1] - P[9][16]*SK_MY[3] + P[9][18]*SK_MY[4]);
|
||||
Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][0]*SH_MAG[2] + P[10][1]*SH_MAG[1] + P[10][2]*SH_MAG[0] - P[10][3]*SK_MY[2] - P[10][17]*SK_MY[1] - P[10][16]*SK_MY[3] + P[10][18]*SK_MY[4]);
|
||||
Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][17]*SK_MY[1] - P[11][16]*SK_MY[3] + P[11][18]*SK_MY[4]);
|
||||
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]);
|
||||
// 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];
|
||||
|
||||
// set flags to indicate to other processes that fusion has been performede 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;
|
||||
SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6];
|
||||
SK_MX[2] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;
|
||||
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[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]);
|
||||
Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][1]*SH_MAG[0] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[3] - P[4][2]*SK_MX[2] - P[4][16]*SK_MX[1] + P[4][17]*SK_MX[5] - P[4][18]*SK_MX[4]);
|
||||
Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][1]*SH_MAG[0] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[3] - P[5][2]*SK_MX[2] - P[5][16]*SK_MX[1] + P[5][17]*SK_MX[5] - P[5][18]*SK_MX[4]);
|
||||
Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][1]*SH_MAG[0] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[3] - P[6][2]*SK_MX[2] - P[6][16]*SK_MX[1] + P[6][17]*SK_MX[5] - P[6][18]*SK_MX[4]);
|
||||
Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][1]*SH_MAG[0] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[3] - P[7][2]*SK_MX[2] - P[7][16]*SK_MX[1] + P[7][17]*SK_MX[5] - P[7][18]*SK_MX[4]);
|
||||
Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][1]*SH_MAG[0] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[3] - P[8][2]*SK_MX[2] - P[8][16]*SK_MX[1] + P[8][17]*SK_MX[5] - P[8][18]*SK_MX[4]);
|
||||
Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][1]*SH_MAG[0] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[3] - P[9][2]*SK_MX[2] - P[9][16]*SK_MX[1] + P[9][17]*SK_MX[5] - P[9][18]*SK_MX[4]);
|
||||
Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][1]*SH_MAG[0] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[3] - P[10][2]*SK_MX[2] - P[10][16]*SK_MX[1] + P[10][17]*SK_MX[5] - P[10][18]*SK_MX[4]);
|
||||
Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][1]*SH_MAG[0] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[3] - P[11][2]*SK_MX[2] - P[11][16]*SK_MX[1] + P[11][17]*SK_MX[5] - P[11][18]*SK_MX[4]);
|
||||
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]);
|
||||
// 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;
|
||||
}
|
||||
else if (obsIndex == 2) // we are now fusing the Z measurement
|
||||
{
|
||||
// calculate observation jacobians
|
||||
for (uint8_t i=0; i<=21; i++) H_MAG[i] = 0;
|
||||
H_MAG[0] = SH_MAG[1];
|
||||
H_MAG[1] = 2*magN*q3 - 2*magE*q0 - 2*magD*q1;
|
||||
H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
|
||||
H_MAG[3] = SH_MAG[0];
|
||||
H_MAG[16] = 2*q0*q2 + 2*q1*q3;
|
||||
H_MAG[17] = 2*q2*q3 - 2*q0*q1;
|
||||
H_MAG[18] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
|
||||
H_MAG[21] = 1;
|
||||
|
||||
// calculate Kalman gain
|
||||
float temp = (P[21][21] + R_MAG + P[0][21]*SH_MAG[1] + P[3][21]*SH_MAG[0] + P[18][21]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[21][1] + P[0][1]*SH_MAG[1] + P[3][1]*SH_MAG[0] + P[18][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][1]*(2*q0*q2 + 2*q1*q3) - P[17][1]*(2*q0*q1 - 2*q2*q3) - P[1][1]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[21][2] + P[0][2]*SH_MAG[1] + P[3][2]*SH_MAG[0] + P[18][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][2]*(2*q0*q2 + 2*q1*q3) - P[17][2]*(2*q0*q1 - 2*q2*q3) - P[1][2]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[21][0] + P[0][0]*SH_MAG[1] + P[3][0]*SH_MAG[0] + P[18][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][0]*(2*q0*q2 + 2*q1*q3) - P[17][0]*(2*q0*q1 - 2*q2*q3) - P[1][0]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[21][3] + P[0][3]*SH_MAG[1] + P[3][3]*SH_MAG[0] + P[18][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][3]*(2*q0*q2 + 2*q1*q3) - P[17][3]*(2*q0*q1 - 2*q2*q3) - P[1][3]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[21][18] + P[0][18]*SH_MAG[1] + P[3][18]*SH_MAG[0] + P[18][18]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][18]*(2*q0*q2 + 2*q1*q3) - P[17][18]*(2*q0*q1 - 2*q2*q3) - P[1][18]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][21]*(2*q0*q2 + 2*q1*q3) - P[17][21]*(2*q0*q1 - 2*q2*q3) - P[1][21]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[21][16] + P[0][16]*SH_MAG[1] + P[3][16]*SH_MAG[0] + P[18][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][16]*(2*q0*q2 + 2*q1*q3) - P[17][16]*(2*q0*q1 - 2*q2*q3) - P[1][16]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[21][17] + P[0][17]*SH_MAG[1] + P[3][17]*SH_MAG[0] + P[18][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][17]*(2*q0*q2 + 2*q1*q3) - P[17][17]*(2*q0*q1 - 2*q2*q3) - P[1][17]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[2][21]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
|
||||
if (temp >= R_MAG) {
|
||||
SK_MZ[0] = 1.0f / temp;
|
||||
faultStatus.bad_zmag = false;
|
||||
} else {
|
||||
// the calculation is badly conditioned, so we cannot perform fusion on this step
|
||||
// we increase the state variances and try again next time
|
||||
P[21][21] += 0.1f*R_MAG;
|
||||
obsIndex = 3;
|
||||
faultStatus.bad_zmag = true;
|
||||
return;
|
||||
// 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;
|
||||
}
|
||||
SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
|
||||
SK_MZ[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;
|
||||
SK_MZ[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
|
||||
SK_MZ[4] = 2*q0*q1 - 2*q2*q3;
|
||||
SK_MZ[5] = 2*q0*q2 + 2*q1*q3;
|
||||
Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][0]*SH_MAG[1] + P[0][3]*SH_MAG[0] - P[0][1]*SK_MZ[2] + P[0][2]*SK_MZ[3] + P[0][18]*SK_MZ[1] + P[0][16]*SK_MZ[5] - P[0][17]*SK_MZ[4]);
|
||||
Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][0]*SH_MAG[1] + P[1][3]*SH_MAG[0] - P[1][1]*SK_MZ[2] + P[1][2]*SK_MZ[3] + P[1][18]*SK_MZ[1] + P[1][16]*SK_MZ[5] - P[1][17]*SK_MZ[4]);
|
||||
Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][0]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[2][1]*SK_MZ[2] + P[2][2]*SK_MZ[3] + P[2][18]*SK_MZ[1] + P[2][16]*SK_MZ[5] - P[2][17]*SK_MZ[4]);
|
||||
Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][0]*SH_MAG[1] + P[3][3]*SH_MAG[0] - P[3][1]*SK_MZ[2] + P[3][2]*SK_MZ[3] + P[3][18]*SK_MZ[1] + P[3][16]*SK_MZ[5] - P[3][17]*SK_MZ[4]);
|
||||
Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][0]*SH_MAG[1] + P[4][3]*SH_MAG[0] - P[4][1]*SK_MZ[2] + P[4][2]*SK_MZ[3] + P[4][18]*SK_MZ[1] + P[4][16]*SK_MZ[5] - P[4][17]*SK_MZ[4]);
|
||||
Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][0]*SH_MAG[1] + P[5][3]*SH_MAG[0] - P[5][1]*SK_MZ[2] + P[5][2]*SK_MZ[3] + P[5][18]*SK_MZ[1] + P[5][16]*SK_MZ[5] - P[5][17]*SK_MZ[4]);
|
||||
Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][0]*SH_MAG[1] + P[6][3]*SH_MAG[0] - P[6][1]*SK_MZ[2] + P[6][2]*SK_MZ[3] + P[6][18]*SK_MZ[1] + P[6][16]*SK_MZ[5] - P[6][17]*SK_MZ[4]);
|
||||
Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][0]*SH_MAG[1] + P[7][3]*SH_MAG[0] - P[7][1]*SK_MZ[2] + P[7][2]*SK_MZ[3] + P[7][18]*SK_MZ[1] + P[7][16]*SK_MZ[5] - P[7][17]*SK_MZ[4]);
|
||||
Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][0]*SH_MAG[1] + P[8][3]*SH_MAG[0] - P[8][1]*SK_MZ[2] + P[8][2]*SK_MZ[3] + P[8][18]*SK_MZ[1] + P[8][16]*SK_MZ[5] - P[8][17]*SK_MZ[4]);
|
||||
Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][0]*SH_MAG[1] + P[9][3]*SH_MAG[0] - P[9][1]*SK_MZ[2] + P[9][2]*SK_MZ[3] + P[9][18]*SK_MZ[1] + P[9][16]*SK_MZ[5] - P[9][17]*SK_MZ[4]);
|
||||
Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][0]*SH_MAG[1] + P[10][3]*SH_MAG[0] - P[10][1]*SK_MZ[2] + P[10][2]*SK_MZ[3] + P[10][18]*SK_MZ[1] + P[10][16]*SK_MZ[5] - P[10][17]*SK_MZ[4]);
|
||||
Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][0]*SH_MAG[1] + P[11][3]*SH_MAG[0] - P[11][1]*SK_MZ[2] + P[11][2]*SK_MZ[3] + P[11][18]*SK_MZ[1] + P[11][16]*SK_MZ[5] - P[11][17]*SK_MZ[4]);
|
||||
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]);
|
||||
// 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];
|
||||
|
||||
// set flags to indicate to other processes that fusion has been performede 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 = false;
|
||||
}
|
||||
// calculate the measurement innovation
|
||||
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 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)) {
|
||||
// Attitude, velocity and position corrections are averaged across multiple prediction cycles between now and the anticipated time for the next measurement.
|
||||
// Don't do averaging of quaternion state corrections if total angle change across predicted interval is going to exceed 0.1 rad
|
||||
bool highRates = ((magUpdateCountMax * correctedDelAng.length()) > 0.1f);
|
||||
// Calculate the number of averaging frames left to go. This is required becasue magnetometer fusion is applied across three consecutive prediction cycles
|
||||
// There is no point averaging if the number of cycles left is less than 2
|
||||
float minorFramesToGo = float(magUpdateCountMax) - float(magUpdateCount);
|
||||
// correct the state vector or store corrections to be applied incrementally
|
||||
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;
|
||||
}
|
||||
if ((j <= 3 && highRates) || j >= 10 || constPosMode || minorFramesToGo < 1.5f ) {
|
||||
states[j] = states[j] - Kfusion[j] * innovMag[obsIndex];
|
||||
} else {
|
||||
// scale the correction based on the number of averaging frames left to go
|
||||
magIncrStateDelta[j] -= Kfusion[j] * innovMag[obsIndex] * (magUpdateCountMaxInv * float(magUpdateCountMax) / minorFramesToGo);
|
||||
}
|
||||
|
||||
// calculate the observation innovation variance
|
||||
varInnovMag[0] = 1.0f/SK_MX[0];
|
||||
|
||||
// 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 performed 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;
|
||||
}
|
||||
else if (obsIndex == 1) // we are now fusing the Y measurement
|
||||
{
|
||||
// calculate observation jacobians
|
||||
for (uint8_t i=0; i<=21; i++) H_MAG[i] = 0;
|
||||
H_MAG[0] = SH_MAG[2];
|
||||
H_MAG[1] = SH_MAG[1];
|
||||
H_MAG[2] = SH_MAG[0];
|
||||
H_MAG[3] = 2*magD*q2 - SH_MAG[8] - SH_MAG[7];
|
||||
H_MAG[16] = 2*q1*q2 - 2*q0*q3;
|
||||
H_MAG[17] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6];
|
||||
H_MAG[18] = 2*q0*q1 + 2*q2*q3;
|
||||
H_MAG[20] = 1;
|
||||
|
||||
// calculate Kalman gain
|
||||
float temp = (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));
|
||||
if (temp >= R_MAG) {
|
||||
SK_MY[0] = 1.0f / temp;
|
||||
faultStatus.bad_ymag = false;
|
||||
} else {
|
||||
// the calculation is badly conditioned, so we cannot perform fusion on this step
|
||||
// we increase the state variances and try again next time
|
||||
P[20][20] += 0.1f*R_MAG;
|
||||
obsIndex = 2;
|
||||
faultStatus.bad_ymag = true;
|
||||
return;
|
||||
}
|
||||
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[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[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]);
|
||||
Kfusion[4] = SK_MY[0]*(P[4][20] + P[4][0]*SH_MAG[2] + P[4][1]*SH_MAG[1] + P[4][2]*SH_MAG[0] - P[4][3]*SK_MY[2] - P[4][17]*SK_MY[1] - P[4][16]*SK_MY[3] + P[4][18]*SK_MY[4]);
|
||||
Kfusion[5] = SK_MY[0]*(P[5][20] + P[5][0]*SH_MAG[2] + P[5][1]*SH_MAG[1] + P[5][2]*SH_MAG[0] - P[5][3]*SK_MY[2] - P[5][17]*SK_MY[1] - P[5][16]*SK_MY[3] + P[5][18]*SK_MY[4]);
|
||||
Kfusion[6] = SK_MY[0]*(P[6][20] + P[6][0]*SH_MAG[2] + P[6][1]*SH_MAG[1] + P[6][2]*SH_MAG[0] - P[6][3]*SK_MY[2] - P[6][17]*SK_MY[1] - P[6][16]*SK_MY[3] + P[6][18]*SK_MY[4]);
|
||||
Kfusion[7] = SK_MY[0]*(P[7][20] + P[7][0]*SH_MAG[2] + P[7][1]*SH_MAG[1] + P[7][2]*SH_MAG[0] - P[7][3]*SK_MY[2] - P[7][17]*SK_MY[1] - P[7][16]*SK_MY[3] + P[7][18]*SK_MY[4]);
|
||||
Kfusion[8] = SK_MY[0]*(P[8][20] + P[8][0]*SH_MAG[2] + P[8][1]*SH_MAG[1] + P[8][2]*SH_MAG[0] - P[8][3]*SK_MY[2] - P[8][17]*SK_MY[1] - P[8][16]*SK_MY[3] + P[8][18]*SK_MY[4]);
|
||||
Kfusion[9] = SK_MY[0]*(P[9][20] + P[9][0]*SH_MAG[2] + P[9][1]*SH_MAG[1] + P[9][2]*SH_MAG[0] - P[9][3]*SK_MY[2] - P[9][17]*SK_MY[1] - P[9][16]*SK_MY[3] + P[9][18]*SK_MY[4]);
|
||||
Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][0]*SH_MAG[2] + P[10][1]*SH_MAG[1] + P[10][2]*SH_MAG[0] - P[10][3]*SK_MY[2] - P[10][17]*SK_MY[1] - P[10][16]*SK_MY[3] + P[10][18]*SK_MY[4]);
|
||||
Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][17]*SK_MY[1] - P[11][16]*SK_MY[3] + P[11][18]*SK_MY[4]);
|
||||
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]);
|
||||
// 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;
|
||||
}
|
||||
// normalise the quaternion states
|
||||
state.quat.normalize();
|
||||
// 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<=21; i++) {
|
||||
for (uint8_t j = 0; j<=3; j++) {
|
||||
}
|
||||
|
||||
// calculate the observation innovation variance
|
||||
varInnovMag[1] = 1.0f/SK_MY[0];
|
||||
|
||||
// set flags to indicate to other processes that fusion has been performede 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;
|
||||
}
|
||||
else if (obsIndex == 2) // we are now fusing the Z measurement
|
||||
{
|
||||
// calculate observation jacobians
|
||||
for (uint8_t i=0; i<=21; i++) H_MAG[i] = 0;
|
||||
H_MAG[0] = SH_MAG[1];
|
||||
H_MAG[1] = 2*magN*q3 - 2*magE*q0 - 2*magD*q1;
|
||||
H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
|
||||
H_MAG[3] = SH_MAG[0];
|
||||
H_MAG[16] = 2*q0*q2 + 2*q1*q3;
|
||||
H_MAG[17] = 2*q2*q3 - 2*q0*q1;
|
||||
H_MAG[18] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
|
||||
H_MAG[21] = 1;
|
||||
|
||||
// calculate Kalman gain
|
||||
float temp = (P[21][21] + R_MAG + P[0][21]*SH_MAG[1] + P[3][21]*SH_MAG[0] + P[18][21]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[21][1] + P[0][1]*SH_MAG[1] + P[3][1]*SH_MAG[0] + P[18][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][1]*(2*q0*q2 + 2*q1*q3) - P[17][1]*(2*q0*q1 - 2*q2*q3) - P[1][1]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[21][2] + P[0][2]*SH_MAG[1] + P[3][2]*SH_MAG[0] + P[18][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][2]*(2*q0*q2 + 2*q1*q3) - P[17][2]*(2*q0*q1 - 2*q2*q3) - P[1][2]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[21][0] + P[0][0]*SH_MAG[1] + P[3][0]*SH_MAG[0] + P[18][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][0]*(2*q0*q2 + 2*q1*q3) - P[17][0]*(2*q0*q1 - 2*q2*q3) - P[1][0]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[21][3] + P[0][3]*SH_MAG[1] + P[3][3]*SH_MAG[0] + P[18][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][3]*(2*q0*q2 + 2*q1*q3) - P[17][3]*(2*q0*q1 - 2*q2*q3) - P[1][3]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[21][18] + P[0][18]*SH_MAG[1] + P[3][18]*SH_MAG[0] + P[18][18]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][18]*(2*q0*q2 + 2*q1*q3) - P[17][18]*(2*q0*q1 - 2*q2*q3) - P[1][18]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][21]*(2*q0*q2 + 2*q1*q3) - P[17][21]*(2*q0*q1 - 2*q2*q3) - P[1][21]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[21][16] + P[0][16]*SH_MAG[1] + P[3][16]*SH_MAG[0] + P[18][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][16]*(2*q0*q2 + 2*q1*q3) - P[17][16]*(2*q0*q1 - 2*q2*q3) - P[1][16]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[21][17] + P[0][17]*SH_MAG[1] + P[3][17]*SH_MAG[0] + P[18][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][17]*(2*q0*q2 + 2*q1*q3) - P[17][17]*(2*q0*q1 - 2*q2*q3) - P[1][17]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[2][21]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
|
||||
if (temp >= R_MAG) {
|
||||
SK_MZ[0] = 1.0f / temp;
|
||||
faultStatus.bad_zmag = false;
|
||||
} else {
|
||||
// the calculation is badly conditioned, so we cannot perform fusion on this step
|
||||
// we increase the state variances and try again next time
|
||||
P[21][21] += 0.1f*R_MAG;
|
||||
obsIndex = 3;
|
||||
faultStatus.bad_zmag = true;
|
||||
return;
|
||||
}
|
||||
SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
|
||||
SK_MZ[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;
|
||||
SK_MZ[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
|
||||
SK_MZ[4] = 2*q0*q1 - 2*q2*q3;
|
||||
SK_MZ[5] = 2*q0*q2 + 2*q1*q3;
|
||||
Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][0]*SH_MAG[1] + P[0][3]*SH_MAG[0] - P[0][1]*SK_MZ[2] + P[0][2]*SK_MZ[3] + P[0][18]*SK_MZ[1] + P[0][16]*SK_MZ[5] - P[0][17]*SK_MZ[4]);
|
||||
Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][0]*SH_MAG[1] + P[1][3]*SH_MAG[0] - P[1][1]*SK_MZ[2] + P[1][2]*SK_MZ[3] + P[1][18]*SK_MZ[1] + P[1][16]*SK_MZ[5] - P[1][17]*SK_MZ[4]);
|
||||
Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][0]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[2][1]*SK_MZ[2] + P[2][2]*SK_MZ[3] + P[2][18]*SK_MZ[1] + P[2][16]*SK_MZ[5] - P[2][17]*SK_MZ[4]);
|
||||
Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][0]*SH_MAG[1] + P[3][3]*SH_MAG[0] - P[3][1]*SK_MZ[2] + P[3][2]*SK_MZ[3] + P[3][18]*SK_MZ[1] + P[3][16]*SK_MZ[5] - P[3][17]*SK_MZ[4]);
|
||||
Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][0]*SH_MAG[1] + P[4][3]*SH_MAG[0] - P[4][1]*SK_MZ[2] + P[4][2]*SK_MZ[3] + P[4][18]*SK_MZ[1] + P[4][16]*SK_MZ[5] - P[4][17]*SK_MZ[4]);
|
||||
Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][0]*SH_MAG[1] + P[5][3]*SH_MAG[0] - P[5][1]*SK_MZ[2] + P[5][2]*SK_MZ[3] + P[5][18]*SK_MZ[1] + P[5][16]*SK_MZ[5] - P[5][17]*SK_MZ[4]);
|
||||
Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][0]*SH_MAG[1] + P[6][3]*SH_MAG[0] - P[6][1]*SK_MZ[2] + P[6][2]*SK_MZ[3] + P[6][18]*SK_MZ[1] + P[6][16]*SK_MZ[5] - P[6][17]*SK_MZ[4]);
|
||||
Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][0]*SH_MAG[1] + P[7][3]*SH_MAG[0] - P[7][1]*SK_MZ[2] + P[7][2]*SK_MZ[3] + P[7][18]*SK_MZ[1] + P[7][16]*SK_MZ[5] - P[7][17]*SK_MZ[4]);
|
||||
Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][0]*SH_MAG[1] + P[8][3]*SH_MAG[0] - P[8][1]*SK_MZ[2] + P[8][2]*SK_MZ[3] + P[8][18]*SK_MZ[1] + P[8][16]*SK_MZ[5] - P[8][17]*SK_MZ[4]);
|
||||
Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][0]*SH_MAG[1] + P[9][3]*SH_MAG[0] - P[9][1]*SK_MZ[2] + P[9][2]*SK_MZ[3] + P[9][18]*SK_MZ[1] + P[9][16]*SK_MZ[5] - P[9][17]*SK_MZ[4]);
|
||||
Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][0]*SH_MAG[1] + P[10][3]*SH_MAG[0] - P[10][1]*SK_MZ[2] + P[10][2]*SK_MZ[3] + P[10][18]*SK_MZ[1] + P[10][16]*SK_MZ[5] - P[10][17]*SK_MZ[4]);
|
||||
Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][0]*SH_MAG[1] + P[11][3]*SH_MAG[0] - P[11][1]*SK_MZ[2] + P[11][2]*SK_MZ[3] + P[11][18]*SK_MZ[1] + P[11][16]*SK_MZ[5] - P[11][17]*SK_MZ[4]);
|
||||
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]);
|
||||
// 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];
|
||||
|
||||
// set flags to indicate to other processes that fusion has been performede 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 = false;
|
||||
}
|
||||
// calculate the measurement innovation
|
||||
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 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)) {
|
||||
// Attitude, velocity and position corrections are averaged across multiple prediction cycles between now and the anticipated time for the next measurement.
|
||||
// Don't do averaging of quaternion state corrections if total angle change across predicted interval is going to exceed 0.1 rad
|
||||
bool highRates = ((magUpdateCountMax * correctedDelAng.length()) > 0.1f);
|
||||
// Calculate the number of averaging frames left to go. This is required becasue magnetometer fusion is applied across three consecutive prediction cycles
|
||||
// There is no point averaging if the number of cycles left is less than 2
|
||||
float minorFramesToGo = float(magUpdateCountMax) - float(magUpdateCount);
|
||||
// correct the state vector or store corrections to be applied incrementally
|
||||
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;
|
||||
}
|
||||
if ((j <= 3 && highRates) || j >= 10 || constPosMode || minorFramesToGo < 1.5f ) {
|
||||
states[j] = states[j] - Kfusion[j] * innovMag[obsIndex];
|
||||
} else {
|
||||
// scale the correction based on the number of averaging frames left to go
|
||||
magIncrStateDelta[j] -= Kfusion[j] * innovMag[obsIndex] * (magUpdateCountMaxInv * float(magUpdateCountMax) / minorFramesToGo);
|
||||
}
|
||||
}
|
||||
// normalise the quaternion states
|
||||
state.quat.normalize();
|
||||
// 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<=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 (!inhibitMagStates) {
|
||||
for (uint8_t j = 16; j<=21; j++) {
|
||||
KH[i][j] = Kfusion[i] * H_MAG[j];
|
||||
}
|
||||
for (uint8_t j = 4; j<=15; j++) {
|
||||
} else {
|
||||
for (uint8_t j = 16; j<=21; 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++) {
|
||||
KH[i][j] = 0.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
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++) {
|
||||
KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
|
||||
}
|
||||
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<=21; i++) {
|
||||
for (uint8_t j = 0; j<=21; j++) {
|
||||
P[i][j] = P[i][j] - KHP[i][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
obsIndex = obsIndex + 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
// set flags to indicate to other processes that fusion has not been performed and is not required on the next time step
|
||||
magFusePerformed = false;
|
||||
magFuseRequired = false;
|
||||
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++) {
|
||||
KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
|
||||
}
|
||||
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<=21; i++) {
|
||||
for (uint8_t j = 0; j<=21; j++) {
|
||||
P[i][j] = P[i][j] - KHP[i][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// force the covariance matrix to be symmetrical and limit the variances to prevent
|
||||
// ill-condiioning.
|
||||
ForceSymmetry();
|
||||
ConstrainVariances();
|
||||
|
||||
// stop performance timer
|
||||
perf_end(_perf_FuseMagnetometer);
|
||||
}
|
||||
|
||||
/*
|
||||
@ -2809,9 +2789,6 @@ void NavEKF::EstimateTerrainOffset()
|
||||
|
||||
void NavEKF::FuseOptFlow()
|
||||
{
|
||||
// start performance timer
|
||||
perf_begin(_perf_FuseOptFlow);
|
||||
|
||||
Vector22 H_LOS;
|
||||
Vector9 tempVar;
|
||||
Vector3f velNED_local;
|
||||
@ -3027,7 +3004,7 @@ void NavEKF::FuseOptFlow()
|
||||
// Attitude, velocity and position corrections are averaged across multiple prediction cycles between now and the anticipated time for the next measurement.
|
||||
// Don't do averaging of quaternion state corrections if total angle change across predicted interval is going to exceed 0.1 rad
|
||||
bool highRates = ((flowUpdateCountMax * correctedDelAng.length()) > 0.1f);
|
||||
// Calculate the number of averaging frames left to go. This is required because flow fusion is applied across two consecutive prediction cycles
|
||||
// Calculate the number of averaging frames left to go.
|
||||
// There is no point averaging if the number of cycles left is less than 2
|
||||
float minorFramesToGo = float(flowUpdateCountMax) - float(flowUpdateCount);
|
||||
for (uint8_t i = 0; i<=21; i++) {
|
||||
@ -3085,8 +3062,6 @@ void NavEKF::FuseOptFlow()
|
||||
ForceSymmetry();
|
||||
ConstrainVariances();
|
||||
|
||||
// stop the performance timer
|
||||
perf_end(_perf_FuseOptFlow);
|
||||
}
|
||||
|
||||
// fuse true airspeed measurements
|
||||
|
||||
@ -540,7 +540,6 @@ private:
|
||||
state_elements statesAtHgtTime; // States at the effective time of hgtMea measurement
|
||||
Vector3f innovMag; // innovation output from fusion of X,Y,Z compass measurements
|
||||
Vector3f varInnovMag; // innovation variance output from fusion of X,Y,Z compass measurements
|
||||
bool fuseMagData; // boolean true when magnetometer data is to be fused
|
||||
Vector3f magData; // magnetometer flux readings in X,Y,Z body axes
|
||||
state_elements statesAtMagMeasTime; // filter states at the effective time of compass measurements
|
||||
ftype innovVtas; // innovation output from fusion of airspeed measurements
|
||||
|
||||
Loading…
Reference in New Issue
Block a user