// Set inertial navigation aiding mode
void NavEKF2_core::setAidingMode()
{
// Determine when to commence aiding for inertial navigation
// Save the previous status so we can detect when it has changed
prevIsAiding = isAiding;
// Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete
bool filterIsStable = tiltAlignComplete && yawAlignComplete;
// If GPS useage has been prohiited then we use flow aiding provided optical flow data is present
bool useFlowAiding = (frontend._fusionModeGPS == 3) && optFlowDataPresent();
// Start aiding if we have a source of aiding data and the filter attitude algnment is complete
// Latch to on. Aiding can be turned off by setting both
isAiding = ((readyToUseGPS() || useFlowAiding) && filterIsStable) || isAiding;
// check to see if we are starting or stopping aiding and set states and modes as required
if (isAiding != prevIsAiding) {
// We have transitioned either into or out of aiding
// zero stored velocities used to do dead-reckoning
heldVelNE.zero();
// reset the flag that indicates takeoff for use by optical flow navigation
takeOffDetected = false;
// set various useage modes based on the condition when we start aiding. These are then held until aiding is stopped.
if (!isAiding) {
// We have ceased aiding
// When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors
PV_AidingMode = AID_NONE;
posTimeout = true;
velTimeout = true;
// store the current position to be used to keep reporting the last known position
lastKnownPositionNE.x = stateStruct.position.x;
lastKnownPositionNE.y = stateStruct.position.y;
// initialise filtered altitude used to provide a takeoff reference to current baro on disarm
// this reduces the time required for the baro noise filter to settle before the filtered baro data can be used
meaHgtAtTakeOff = baroDataDelayed.hgt;
// reset the vertical position state to faster recover from baro errors experienced during touchdown
stateStruct.position.z = -meaHgtAtTakeOff;
} else if (frontend._fusionModeGPS == 3) {
// We have commenced aiding, but GPS useage has been prohibited so use optical flow only
hal.console->printf("EKF is using optical flow\n");
PV_AidingMode = AID_RELATIVE; // we have optical flow data and can estimate all vehicle states
posTimeout = true;
velTimeout = true;
// Reset the last valid flow measurement time
flowValidMeaTime_ms = imuSampleTime_ms;
// Reset the last valid flow fusion time
prevFlowFuseTime_ms = imuSampleTime_ms;
} else {
// We have commenced aiding and GPS useage is allowed
hal.console->printf("EKF is using GPS\n");
PV_AidingMode = AID_ABSOLUTE; // we have GPS data and can estimate all vehicle states
posTimeout = false;
velTimeout = false;
// we need to reset the GPS timers to prevent GPS timeout logic being invoked on entry into GPS aiding
// this is becasue the EKF can be interrupted for an arbitrary amount of time during vehicle arming checks
lastTimeGpsReceived_ms = imuSampleTime_ms;
secondLastGpsTime_ms = imuSampleTime_ms;
// reset the last valid position fix time to prevent unwanted activation of GPS glitch logic
lastPosPassTime_ms = imuSampleTime_ms;
}
// Reset all position, velocity and covariance
ResetVelocity();
ResetPosition();
CovarianceInit();
}
// Always turn aiding off when the vehicle is disarmed
if (!isAiding) {
PV_AidingMode = AID_NONE;
posTimeout = true;
velTimeout = true;
}
}
void NavEKF3_core::FuseRngBcn()
{
// declarations
float pn;
float pe;
float pd;
float bcn_pn;
float bcn_pe;
float bcn_pd;
const float R_BCN = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f));
float rngPred;
// health is set bad until test passed
rngBcnHealth = false;
if (activeHgtSource != HGT_SOURCE_BCN) {
// calculate the vertical offset from EKF datum to beacon datum
CalcRangeBeaconPosDownOffset(R_BCN, stateStruct.position, false);
} else {
bcnPosOffsetNED.z = 0.0f;
}
// copy required states to local variable names
pn = stateStruct.position.x;
pe = stateStruct.position.y;
pd = stateStruct.position.z;
bcn_pn = rngBcnDataDelayed.beacon_posNED.x;
bcn_pe = rngBcnDataDelayed.beacon_posNED.y;
bcn_pd = rngBcnDataDelayed.beacon_posNED.z + bcnPosOffsetNED.z;
// predicted range
Vector3f deltaPosNED = stateStruct.position - rngBcnDataDelayed.beacon_posNED;
rngPred = deltaPosNED.length();
// calculate measurement innovation
innovRngBcn = rngPred - rngBcnDataDelayed.rng;
// perform fusion of range measurement
if (rngPred > 0.1f)
{
// calculate observation jacobians
float H_BCN[24];
memset(H_BCN, 0, sizeof(H_BCN));
float t2 = bcn_pd-pd;
float t3 = bcn_pe-pe;
float t4 = bcn_pn-pn;
float t5 = t2*t2;
float t6 = t3*t3;
float t7 = t4*t4;
float t8 = t5+t6+t7;
float t9 = 1.0f/sqrtf(t8);
H_BCN[7] = -t4*t9;
H_BCN[8] = -t3*t9;
// If we are not using the beacons as a height reference, we pretend that the beacons
// are at the same height as the flight vehicle when calculating the observation derivatives
// and Kalman gains
// TODO - less hacky way of achieving this, preferably using an alternative derivation
if (activeHgtSource != HGT_SOURCE_BCN) {
t2 = 0.0f;
}
H_BCN[9] = -t2*t9;
// calculate Kalman gains
float t10 = P[9][9]*t2*t9;
float t11 = P[8][9]*t3*t9;
float t12 = P[7][9]*t4*t9;
float t13 = t10+t11+t12;
float t14 = t2*t9*t13;
float t15 = P[9][8]*t2*t9;
float t16 = P[8][8]*t3*t9;
float t17 = P[7][8]*t4*t9;
float t18 = t15+t16+t17;
float t19 = t3*t9*t18;
float t20 = P[9][7]*t2*t9;
float t21 = P[8][7]*t3*t9;
float t22 = P[7][7]*t4*t9;
float t23 = t20+t21+t22;
float t24 = t4*t9*t23;
varInnovRngBcn = R_BCN+t14+t19+t24;
float t26;
if (varInnovRngBcn >= R_BCN) {
t26 = 1.0f/varInnovRngBcn;
faultStatus.bad_rngbcn = false;
} else {
// the calculation is badly conditioned, so we cannot perform fusion on this step
// we reset the covariance matrix and try again next measurement
CovarianceInit();
faultStatus.bad_rngbcn = true;
return;
}
Kfusion[0] = -t26*(P[0][7]*t4*t9+P[0][8]*t3*t9+P[0][9]*t2*t9);
Kfusion[1] = -t26*(P[1][7]*t4*t9+P[1][8]*t3*t9+P[1][9]*t2*t9);
Kfusion[2] = -t26*(P[2][7]*t4*t9+P[2][8]*t3*t9+P[2][9]*t2*t9);
Kfusion[3] = -t26*(P[3][7]*t4*t9+P[3][8]*t3*t9+P[3][9]*t2*t9);
Kfusion[4] = -t26*(P[4][7]*t4*t9+P[4][8]*t3*t9+P[4][9]*t2*t9);
Kfusion[5] = -t26*(P[5][7]*t4*t9+P[5][8]*t3*t9+P[5][9]*t2*t9);
Kfusion[7] = -t26*(t22+P[7][8]*t3*t9+P[7][9]*t2*t9);
Kfusion[8] = -t26*(t16+P[8][7]*t4*t9+P[8][9]*t2*t9);
if (!inhibitDelAngBiasStates) {
Kfusion[10] = -t26*(P[10][7]*t4*t9+P[10][8]*t3*t9+P[10][9]*t2*t9);
Kfusion[11] = -t26*(P[11][7]*t4*t9+P[11][8]*t3*t9+P[11][9]*t2*t9);
Kfusion[12] = -t26*(P[12][7]*t4*t9+P[12][8]*t3*t9+P[12][9]*t2*t9);
} else {
// zero indexes 10 to 12 = 3*4 bytes
memset(&Kfusion[10], 0, 12);
}
if (!inhibitDelVelBiasStates) {
Kfusion[13] = -t26*(P[13][7]*t4*t9+P[13][8]*t3*t9+P[13][9]*t2*t9);
Kfusion[14] = -t26*(P[14][7]*t4*t9+P[14][8]*t3*t9+P[14][9]*t2*t9);
Kfusion[15] = -t26*(P[15][7]*t4*t9+P[15][8]*t3*t9+P[15][9]*t2*t9);
} else {
// zero indexes 13 to 15 = 3*4 bytes
memset(&Kfusion[13], 0, 12);
}
// only allow the range observations to modify the vertical states if we are using it as a height reference
if (activeHgtSource == HGT_SOURCE_BCN) {
Kfusion[6] = -t26*(P[6][7]*t4*t9+P[6][8]*t3*t9+P[6][9]*t2*t9);
Kfusion[9] = -t26*(t10+P[9][7]*t4*t9+P[9][8]*t3*t9);
} else {
Kfusion[6] = 0.0f;
Kfusion[9] = 0.0f;
}
if (!inhibitMagStates) {
Kfusion[16] = -t26*(P[16][7]*t4*t9+P[16][8]*t3*t9+P[16][9]*t2*t9);
Kfusion[17] = -t26*(P[17][7]*t4*t9+P[17][8]*t3*t9+P[17][9]*t2*t9);
Kfusion[18] = -t26*(P[18][7]*t4*t9+P[18][8]*t3*t9+P[18][9]*t2*t9);
Kfusion[19] = -t26*(P[19][7]*t4*t9+P[19][8]*t3*t9+P[19][9]*t2*t9);
Kfusion[20] = -t26*(P[20][7]*t4*t9+P[20][8]*t3*t9+P[20][9]*t2*t9);
Kfusion[21] = -t26*(P[21][7]*t4*t9+P[21][8]*t3*t9+P[21][9]*t2*t9);
} else {
// zero indexes 16 to 21 = 6*4 bytes
memset(&Kfusion[16], 0, 24);
}
if (!inhibitWindStates) {
Kfusion[22] = -t26*(P[22][7]*t4*t9+P[22][8]*t3*t9+P[22][9]*t2*t9);
Kfusion[23] = -t26*(P[23][7]*t4*t9+P[23][8]*t3*t9+P[23][9]*t2*t9);
} else {
// zero indexes 22 to 23 = 2*4 bytes
memset(&Kfusion[22], 0, 8);
}
// Calculate innovation using the selected offset value
Vector3f delta = stateStruct.position - rngBcnDataDelayed.beacon_posNED;
innovRngBcn = delta.length() - rngBcnDataDelayed.rng;
// calculate the innovation consistency test ratio
rngBcnTestRatio = sq(innovRngBcn) / (sq(MAX(0.01f * (float)frontend->_rngBcnInnovGate, 1.0f)) * varInnovRngBcn);
// fail if the ratio is > 1, but don't fail if bad IMU data
rngBcnHealth = ((rngBcnTestRatio < 1.0f) || badIMUdata);
// test the ratio before fusing data
if (rngBcnHealth) {
// restart the counter
lastRngBcnPassTime_ms = imuSampleTime_ms;
// correct the covariance P = (I - K*H)*P
// take advantage of the empty columns in KH to reduce the
// number of operations
for (unsigned i = 0; i<=stateIndexLim; i++) {
for (unsigned j = 0; j<=6; j++) {
KH[i][j] = 0.0f;
}
for (unsigned j = 7; j<=9; j++) {
KH[i][j] = Kfusion[i] * H_BCN[j];
}
for (unsigned j = 10; j<=23; j++) {
KH[i][j] = 0.0f;
}
}
for (unsigned j = 0; j<=stateIndexLim; j++) {
for (unsigned i = 0; i<=stateIndexLim; i++) {
ftype res = 0;
res += KH[i][7] * P[7][j];
res += KH[i][8] * P[8][j];
res += KH[i][9] * P[9][j];
KHP[i][j] = res;
}
}
// Check that we are not going to drive any variances negative and skip the update if so
bool healthyFusion = true;
for (uint8_t i= 0; i<=stateIndexLim; i++) {
if (KHP[i][i] > P[i][i]) {
healthyFusion = false;
}
}
if (healthyFusion) {
// update the covariance matrix
for (uint8_t i= 0; i<=stateIndexLim; i++) {
for (uint8_t j= 0; j<=stateIndexLim; j++) {
P[i][j] = P[i][j] - KHP[i][j];
}
}
// force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning.
ForceSymmetry();
ConstrainVariances();
// correct the state vector
for (uint8_t j= 0; j<=stateIndexLim; j++) {
statesArray[j] = statesArray[j] - Kfusion[j] * innovRngBcn;
}
// record healthy fusion
faultStatus.bad_rngbcn = false;
} else {
// record bad fusion
faultStatus.bad_rngbcn = true;
}
}
// Update the fusion report
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].beaconPosNED = rngBcnDataDelayed.beacon_posNED;
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innov = innovRngBcn;
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innovVar = varInnovRngBcn;
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].rng = rngBcnDataDelayed.rng;
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].testRatio = rngBcnTestRatio;
}
}
