@ -725,10 +725,6 @@ void NavEKF::UpdateFilter()
@@ -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()
@@ -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()
@@ -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,23 +912,16 @@ void NavEKF::SelectMagFusion()
@@ -911,23 +912,16 @@ 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 ;
}
els e
{
fuseMagData = false ;
// ensure that the covariance prediction is up to date before fusing data
if ( ! covPr edStep ) Covarianc ePrediction ( ) ;
// fuse the three magnetometer componenents sequentially
for ( mag_state . obsIndex = 0 ; mag_state . obsIndex < = 2 ; mag_state . obsIndex + + ) FuseMagnetometer ( ) ;
}
// 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 ) {
magUpdateCount + + ;
@ -935,11 +929,16 @@ void NavEKF::SelectMagFusion()
@@ -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()
@@ -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()
@@ -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 cas ue the optical flow measurements to fight the GPS
// because an invalid height above ground estimate will caus e 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()
@@ -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()
@@ -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()
@@ -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()
@@ -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()
@@ -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()
@@ -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()
@@ -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 ] ;
// 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 ) ;
// 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 ;
// 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 ;
// scale magnetometer observation error with total angular rate
R_MAG = sq ( constrain_float ( _magNoise , 0.01f , 0.5f ) ) + sq ( magVarRateScale * dAngIMU . length ( ) / dtIMU ) ;
// 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 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 ] ;
// 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 ;
// 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 ;
// 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 ;
}
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 ;
}
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 ] ;
// 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 ;
// 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 ;
}
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 ;
}
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 ;
}
// 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 ;
}
// 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 ] ) ;
if ( ( j < = 3 & & highRates ) | | j > = 10 | | constPosMode | | minorFramesToGo < 1.5f ) {
states [ j ] = states [ j ] - Kfusion [ j ] * innovMag [ obsIndex ] ;
} else {
for ( uint8_t i = 16 ; i < = 21 ; i + + ) {
Kfusion [ i ] = 0.0f ;
}
// 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 [ 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 ] ;
}
// 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 + + ) {
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 + + ) {
}
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 ] ;
}
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 ] ;
}
}
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 ;
}
// 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()
@@ -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()
@@ -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()
@@ -3085,8 +3062,6 @@ void NavEKF::FuseOptFlow()
ForceSymmetry ( ) ;
ConstrainVariances ( ) ;
// stop the performance timer
perf_end ( _perf_FuseOptFlow ) ;
}
// fuse true airspeed measurements