EKF: fix bugs in stationary process model covariance prediction

Noise variance for stationary states was  being overwritten by the covariance prediction operations for the kinematic states
Unwanted states could end up with non-zero covariance values
The forced symmetry operation was being applied too late to be effective

EKF/covariance.cpp

@@ -140,7 +140,7 @@ void Ekf::predictCovariance()
 
 	float dt = _imu_sample_delayed.delta_ang_dt;
 
-	// compute process noise
+	// compute noise variance for stationary processes
 	float process_noise[_k_num_states] = {};
 
 	float d_ang_bias_sig = dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1.0f);
@@ -198,6 +198,14 @@ void Ekf::predictCovariance()
 	float accel_noise = math::constrain(_params.accel_noise, 0.0f, 1.0f);
 	dvxVar = dvyVar = dvzVar = sq(dt * accel_noise);
 
+	// force symmetry on the covariance matrix before we use it for the prediction step
+	for (unsigned row = 1; row < _k_num_states; row++) {
+		for (unsigned column = 0; column < row; column++) {
+			P[row][column] = 0.5f * (P[row][column] + P[column][row]);
+			P[column][row] = P[row][column];
+		}
+	}
+
 	// predict the covariance
 
 	// intermediate calculations
@@ -263,11 +271,6 @@ void Ekf::predictCovariance()
 	// covariance update
 	float nextP[24][24];
 
-	// add noise variances for states with stationary process models
-	for (unsigned i = 0; i < _k_num_states; i++) {
-		nextP[i][i] += process_noise[i];
-	}
-
 	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;
@@ -360,6 +363,11 @@ void Ekf::predictCovariance()
 	nextP[11][12] = P[11][12];
 	nextP[12][12] = P[12][12];
 
+	// add process noise that is not from the IMU
+	for (unsigned i = 0; i <= 12; i++) {
+		nextP[i][i] += process_noise[i];
+	}
+
 	// Don't calculate these covariance terms if we are not estimating XY delta velocity bias errors
 	if (_params.fusion_mode & MASK_USE_3D_ACC_BIAS) {
 		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];
@@ -391,9 +399,12 @@ void Ekf::predictCovariance()
 		nextP[12][14] = P[12][14];
 		nextP[13][14] = P[13][14];
 		nextP[14][14] = P[14][14];
-	} else {
+
-		zeroRows(nextP,13,14);
+		// add process noise that is not from the IMU
-		zeroCols(nextP,13,14);
+		for (unsigned i = 13; i <= 14; i++) {
+			nextP[i][i] += process_noise[i];
+		}
+
 	}
 
 	nextP[0][15] = P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10];
@@ -413,6 +424,9 @@ void Ekf::predictCovariance()
 	nextP[14][15] = P[14][15];
 	nextP[15][15] = P[15][15];
 
+	// add process noise that is not from the IMU
+	nextP[15][15] += process_noise[15];
+
 	// Don't do covariance prediction on magnetic field states unless we are using 3-axis fusion
 	if (_control_status.flags.mag_3D) {
 		// Check if we have just transitioned into 3-axis fusion and set the state variances
@@ -540,9 +554,10 @@ void Ekf::predictCovariance()
 		nextP[20][21] = P[20][21];
 		nextP[21][21] = P[21][21];
 
-	} else {
+		// add process noise that is not from the IMU
-		zeroRows(nextP,16,21);
+		for (unsigned i = 16; i <= 21; i++) {
-		zeroCols(nextP,16,21);
+			nextP[i][i] += process_noise[i];
+		}
 
 	}
 
@@ -641,9 +656,10 @@ void Ekf::predictCovariance()
 		nextP[22][23] = P[22][23];
 		nextP[23][23] = P[23][23];
 
-	} else {
+		// add process noise that is not from the IMU
-		zeroRows(nextP,22,23);
+		for (unsigned i = 22; i <= 23; i++) {
-		zeroCols(nextP,22,23);
+			nextP[i][i] += process_noise[i];
+		}
 
 	}
 
@@ -661,27 +677,35 @@ void Ekf::predictCovariance()
 	// covariance matrix is symmetrical, so copy upper half to lower half
 	for (unsigned row = 1; row < _k_num_states; row++) {
 		for (unsigned column = 0 ; column < row; column++) {
-			nextP[row][column] = nextP[column][row];
+			P[row][column] = P[column][row] = nextP[column][row];
 		}
 	}
 
 	// copy variances (diagonals)
 	for (unsigned i = 0; i < _k_num_states; i++) {
-		if(!_control_status.flags.wind && i > 21) {
-			continue;
-		}
 		P[i][i] = nextP[i][i];
 	}
 
-	// force symmetry
+	// prevent variances from becoming too large or negative
-	for (unsigned row = 1; row < _k_num_states; row++) {
+	limitCov();
-		for (unsigned column = 0; column < row; column++) {
+
-			P[row][column] = 0.5f * (nextP[row][column] + nextP[column][row]);
+	// ensure coresponding rows and zeros for unwanted states are zero
-			P[column][row] = P[row][column];
+	if (!(_params.fusion_mode & MASK_USE_3D_ACC_BIAS)) {
+		// deactivate XY accel bias states
+		zeroRows(P,13,14);
+		zeroCols(P,13,14);
 		}
+	if (!_control_status.flags.mag_3D) {
+		// deactivate magnetic field states
+		zeroRows(P,16,21);
+		zeroCols(P,16,21);
+	}
+	if (!_control_status.flags.wind) {
+		// deactivate wind states
+		zeroRows(P,22,23);
+		zeroCols(P,22,23);
 	}
 
-	limitCov();
 }
 
 void Ekf::limitCov()