AP_NavEKF3: Improve partitioning and efficiency of the covariance prediction
This patch ensures that covariance matrix entries for inactive states are always set to zero. It also halves the number of copy operations from the updated to stored matrix.
This commit is contained in:
priseborough
Francisco Ferreira
parent
a1c117360c
commit
7adaea6019
@ -206,7 +206,6 @@ void NavEKF3_core::InitialiseVariables()
|
||||
prevTnb.zero();
|
||||
memset(&P[0][0], 0, sizeof(P));
|
||||
memset(&nextP[0][0], 0, sizeof(nextP));
|
||||
memset(&processNoise[0], 0, sizeof(processNoise));
|
||||
flowDataValid = false;
|
||||
rangeDataToFuse = false;
|
||||
fuseOptFlowData = false;
|
||||
@ -822,11 +821,6 @@ void NavEKF3_core::calcOutputStates()
|
||||
void NavEKF3_core::CovariancePrediction()
|
||||
{
|
||||
hal.util->perf_begin(_perf_CovariancePrediction);
|
||||
float windVelSigma; // wind velocity 1-sigma process noise - m/s
|
||||
float dAngBiasSigma;// delta angle bias 1-sigma process noise - rad/s
|
||||
float dVelBiasSigma;// delta velocity bias 1-sigma process noise - m/s
|
||||
float magEarthSigma;// earth magnetic field 1-sigma process noise
|
||||
float magBodySigma; // body magnetic field 1-sigma process noise
|
||||
float daxVar; // X axis delta angle noise variance rad^2
|
||||
float dayVar; // Y axis delta angle noise variance rad^2
|
||||
float dazVar; // Z axis delta angle noise variance rad^2
|
||||
@ -859,25 +853,34 @@ void NavEKF3_core::CovariancePrediction()
|
||||
float alpha = 0.1f * dt;
|
||||
hgtRate = hgtRate * (1.0f - alpha) - stateStruct.velocity.z * alpha;
|
||||
|
||||
// calculate covariance prediction process noise
|
||||
windVelSigma = dt * constrain_float(frontend->_windVelProcessNoise, 0.0f, 1.0f) * (1.0f + constrain_float(frontend->_wndVarHgtRateScale, 0.0f, 1.0f) * fabsf(hgtRate));
|
||||
dAngBiasSigma = sq(dt) * constrain_float(frontend->_gyroBiasProcessNoise, 0.0f, 1.0f);
|
||||
dVelBiasSigma = sq(dt) * constrain_float(frontend->_accelBiasProcessNoise, 0.0f, 1.0f);
|
||||
magEarthSigma = dt * constrain_float(frontend->_magEarthProcessNoise, 0.0f, 1.0f);
|
||||
magBodySigma = dt * constrain_float(frontend->_magBodyProcessNoise, 0.0f, 1.0f);
|
||||
for (uint8_t i= 0; i<=9; i++) processNoise[i] = 0.0f;
|
||||
for (uint8_t i=10; i<=12; i++) processNoise[i] = dAngBiasSigma;
|
||||
for (uint8_t i=13; i<=15; i++) processNoise[i] = dVelBiasSigma;
|
||||
if (expectGndEffectTakeoff) {
|
||||
processNoise[15] = 0.0f;
|
||||
} else {
|
||||
processNoise[15] = dVelBiasSigma;
|
||||
}
|
||||
for (uint8_t i=16; i<=18; i++) processNoise[i] = magEarthSigma;
|
||||
for (uint8_t i=19; i<=21; i++) processNoise[i] = magBodySigma;
|
||||
for (uint8_t i=22; i<=23; i++) processNoise[i] = windVelSigma;
|
||||
// calculate covariance prediction process noise added to diagonals of predicted covariance matrix
|
||||
// error growth of first 10 kinematic states is built into auto-code for covariance prediction and driven by IMU noise parameters
|
||||
Vector14 processNoiseVariance = {};
|
||||
|
||||
for (uint8_t i= 0; i<=stateIndexLim; i++) processNoise[i] = sq(processNoise[i]);
|
||||
if (!inhibitDelAngBiasStates) {
|
||||
float dAngBiasVar = sq(sq(dt) * constrain_float(frontend->_gyroBiasProcessNoise, 0.0f, 1.0f));
|
||||
for (uint8_t i=0; i<=2; i++) processNoiseVariance[i] = dAngBiasVar;
|
||||
}
|
||||
|
||||
if (!inhibitDelVelBiasStates) {
|
||||
float dVelBiasVar = sq(sq(dt) * constrain_float(frontend->_accelBiasProcessNoise, 0.0f, 1.0f));
|
||||
for (uint8_t i=3; i<=4; i++) processNoiseVariance[i] = dVelBiasVar;
|
||||
if (!expectGndEffectTakeoff) {
|
||||
processNoiseVariance[5] = dVelBiasVar;
|
||||
}
|
||||
}
|
||||
|
||||
if (!inhibitMagStates) {
|
||||
float magEarthVar = sq(dt * constrain_float(frontend->_magEarthProcessNoise, 0.0f, 1.0f));
|
||||
float magBodyVar = sq(dt * constrain_float(frontend->_magBodyProcessNoise, 0.0f, 1.0f));
|
||||
for (uint8_t i=6; i<=8; i++) processNoiseVariance[i] = magEarthVar;
|
||||
for (uint8_t i=9; i<=11; i++) processNoiseVariance[i] = magBodyVar;
|
||||
}
|
||||
|
||||
if (!inhibitWindStates) {
|
||||
float windVelVar = sq(dt * constrain_float(frontend->_windVelProcessNoise, 0.0f, 1.0f) * (1.0f + constrain_float(frontend->_wndVarHgtRateScale, 0.0f, 1.0f) * fabsf(hgtRate)));
|
||||
for (uint8_t i=12; i<=13; i++) processNoiseVariance[i] = windVelVar;
|
||||
}
|
||||
|
||||
// set variables used to calculate covariance growth
|
||||
dvx = imuDataDelayed.delVel.x;
|
||||
@ -958,26 +961,6 @@ void NavEKF3_core::CovariancePrediction()
|
||||
SPP[9] = 2.0f*q0*q2 + 2.0f*q1*q3;
|
||||
SPP[10] = SF[16];
|
||||
|
||||
if (inhibitDelAngBiasStates) {
|
||||
zeroRows(P,10,12);
|
||||
zeroCols(P,10,12);
|
||||
}
|
||||
|
||||
if (inhibitDelVelBiasStates) {
|
||||
zeroRows(P,13,15);
|
||||
zeroCols(P,13,15);
|
||||
}
|
||||
|
||||
if (inhibitMagStates) {
|
||||
zeroRows(P,16,21);
|
||||
zeroCols(P,16,21);
|
||||
}
|
||||
|
||||
if (inhibitWindStates) {
|
||||
zeroRows(P,22,23);
|
||||
zeroCols(P,22,23);
|
||||
}
|
||||
|
||||
nextP[0][0] = P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10] + (daxVar*SQ[10])*0.25f + SF[9]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[11]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[10]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SF[14]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) + SF[15]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) + SPP[10]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) + (dayVar*sq(q2))*0.25f + (dazVar*sq(q3))*0.25f;
|
||||
nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10] + SF[8]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[7]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[11]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) - SF[15]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) + SPP[10]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) - (q0*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]))*0.5f;
|
||||
nextP[1][1] = P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] + daxVar*SQ[9] - (P[10][1]*q0)*0.5f + SF[8]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[7]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[11]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) - SF[15]*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2) + SPP[10]*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2) + (dayVar*sq(q3))*0.25f + (dazVar*sq(q2))*0.25f - (q0*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2))*0.5f;
|
||||
@ -1033,6 +1016,8 @@ void NavEKF3_core::CovariancePrediction()
|
||||
nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt);
|
||||
nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt);
|
||||
nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt);
|
||||
|
||||
if (stateIndexLim > 9) {
|
||||
nextP[0][10] = P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10];
|
||||
nextP[1][10] = P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)*0.5f;
|
||||
nextP[2][10] = P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)*0.5f;
|
||||
@ -1069,6 +1054,8 @@ void NavEKF3_core::CovariancePrediction()
|
||||
nextP[10][12] = P[10][12];
|
||||
nextP[11][12] = P[11][12];
|
||||
nextP[12][12] = P[12][12];
|
||||
|
||||
if (stateIndexLim > 12) {
|
||||
nextP[0][13] = P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10];
|
||||
nextP[1][13] = P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)*0.5f;
|
||||
nextP[2][13] = P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)*0.5f;
|
||||
@ -1284,28 +1271,21 @@ void NavEKF3_core::CovariancePrediction()
|
||||
nextP[23][23] = P[23][23];
|
||||
}
|
||||
}
|
||||
|
||||
// Copy upper diagonal to lower diagonal taking advantage of symmetry
|
||||
for (uint8_t colIndex=0; colIndex<=stateIndexLim; colIndex++)
|
||||
{
|
||||
for (uint8_t rowIndex=0; rowIndex<colIndex; rowIndex++)
|
||||
{
|
||||
nextP[colIndex][rowIndex] = nextP[rowIndex][colIndex];
|
||||
}
|
||||
}
|
||||
|
||||
// add the general state process noise variances
|
||||
for (uint8_t i=0; i<=stateIndexLim; i++)
|
||||
{
|
||||
nextP[i][i] = nextP[i][i] + processNoise[i];
|
||||
if (stateIndexLim > 9) {
|
||||
for (uint8_t i=10; i<=stateIndexLim; i++) {
|
||||
nextP[i][i] = nextP[i][i] + processNoiseVariance[i-10];
|
||||
}
|
||||
}
|
||||
|
||||
// if the total position variance exceeds 1e4 (100m), then stop covariance
|
||||
// growth by setting the predicted to the previous values
|
||||
// This prevent an ill conditioned matrix from occurring for long periods
|
||||
// without GPS
|
||||
if ((P[7][7] + P[8][8]) > 1e4f)
|
||||
{
|
||||
if ((P[7][7] + P[8][8]) > 1e4f) {
|
||||
for (uint8_t i=7; i<=8; i++)
|
||||
{
|
||||
for (uint8_t j=0; j<=stateIndexLim; j++)
|
||||
@ -1316,10 +1296,18 @@ void NavEKF3_core::CovariancePrediction()
|
||||
}
|
||||
}
|
||||
|
||||
// copy covariances to output
|
||||
CopyCovariances();
|
||||
// covariance matrix is symmetrical, so copy diagonals and copy lower half in nextP
|
||||
// to lower and upper half in P
|
||||
for (uint8_t row = 0; row <= stateIndexLim; row++) {
|
||||
// copy diagonals
|
||||
P[row][row] = nextP[row][row];
|
||||
// copy off diagonals
|
||||
for (uint8_t column = 0 ; column < row; column++) {
|
||||
P[row][column] = P[column][row] = nextP[column][row];
|
||||
}
|
||||
}
|
||||
|
||||
// constrain diagonals to prevent ill-conditioning
|
||||
// constrain values to prevent ill-conditioning
|
||||
ConstrainVariances();
|
||||
|
||||
hal.util->perf_end(_perf_CovariancePrediction);
|
||||
@ -1404,30 +1392,43 @@ void NavEKF3_core::ForceSymmetry()
|
||||
}
|
||||
}
|
||||
|
||||
// copy covariances across from covariance prediction calculation
|
||||
void NavEKF3_core::CopyCovariances()
|
||||
{
|
||||
// copy predicted covariances
|
||||
for (uint8_t i=0; i<=stateIndexLim; i++) {
|
||||
for (uint8_t j=0; j<=stateIndexLim; j++)
|
||||
{
|
||||
P[i][j] = nextP[i][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// constrain variances (diagonal terms) in the state covariance matrix to prevent ill-conditioning
|
||||
// if states are inactive, zero the corresponding off-diagonals
|
||||
void NavEKF3_core::ConstrainVariances()
|
||||
{
|
||||
for (uint8_t i=0; i<=3; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0f); // attitude error
|
||||
for (uint8_t i=4; i<=6; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e3f); // velocities
|
||||
for (uint8_t i=7; i<=8; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e6f);
|
||||
P[9][9] = constrain_float(P[9][9],0.0f,1.0e6f); // vertical position
|
||||
for (uint8_t i=10; i<=12; i++) P[i][i] = constrain_float(P[i][i],0.0f,sq(0.175f * dtEkfAvg)); // delta angle biases
|
||||
for (uint8_t i=13; i<=15; i++) P[i][i] = constrain_float(P[i][i],0.0f,sq(10.0f * dtEkfAvg)); // delta velocity bias
|
||||
|
||||
if (!inhibitDelAngBiasStates) {
|
||||
for (uint8_t i=10; i<=12; i++) P[i][i] = constrain_float(P[i][i],0.0f,sq(0.175f * dtEkfAvg));
|
||||
} else {
|
||||
zeroCols(P,10,12);
|
||||
zeroRows(P,10,12);
|
||||
}
|
||||
|
||||
if (!inhibitDelVelBiasStates) {
|
||||
for (uint8_t i=13; i<=15; i++) P[i][i] = constrain_float(P[i][i],0.0f,sq(10.0f * dtEkfAvg));
|
||||
} else {
|
||||
zeroCols(P,13,15);
|
||||
zeroRows(P,13,15);
|
||||
}
|
||||
|
||||
if (!inhibitMagStates) {
|
||||
for (uint8_t i=16; i<=18; i++) P[i][i] = constrain_float(P[i][i],0.0f,0.01f); // earth magnetic field
|
||||
for (uint8_t i=19; i<=21; i++) P[i][i] = constrain_float(P[i][i],0.0f,0.01f); // body magnetic field
|
||||
for (uint8_t i=22; i<=23; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e3f); // wind velocity
|
||||
} else {
|
||||
zeroCols(P,16,21);
|
||||
zeroRows(P,16,21);
|
||||
}
|
||||
|
||||
if (!inhibitWindStates) {
|
||||
for (uint8_t i=22; i<=23; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e3f);
|
||||
} else {
|
||||
zeroCols(P,22,23);
|
||||
zeroRows(P,22,23);
|
||||
}
|
||||
}
|
||||
|
||||
// constrain states to prevent ill-conditioning
|
||||
|
||||
@ -493,9 +493,6 @@ private:
|
||||
// force symmetry on the state covariance matrix
|
||||
void ForceSymmetry();
|
||||
|
||||
// copy covariances across from covariance prediction calculation and fix numerical errors
|
||||
void CopyCovariances();
|
||||
|
||||
// constrain variances (diagonal terms) in the state covariance matrix
|
||||
void ConstrainVariances();
|
||||
|
||||
@ -843,7 +840,6 @@ private:
|
||||
uint32_t lastSynthYawTime_ms; // time stamp when synthetic yaw measurement was last fused to maintain covariance health (msec)
|
||||
uint32_t ekfStartTime_ms; // time the EKF was started (msec)
|
||||
Matrix24 nextP; // Predicted covariance matrix before addition of process noise to diagonals
|
||||
Vector24 processNoise; // process noise added to diagonals of predicted covariance matrix
|
||||
Vector21 SF; // intermediate variables used to calculate predicted covariance matrix
|
||||
Vector8 SG; // intermediate variables used to calculate predicted covariance matrix
|
||||
Vector11 SQ; // intermediate variables used to calculate predicted covariance matrix
|
||||
|
||||
Loading…
Reference in New Issue
Block a user