EKF: Enable optional activation of XY delta velocity bias estimation

EKF/common.h

@@ -140,6 +140,8 @@ struct flowSample {
 #define MASK_USE_GPS    (1<<0)  // set to true to use GPS data
 #define MASK_USE_OF     (1<<1)  // set to true to use optical flow data
 
+#define MASK_USE_3D_ACC_BIAS (1<<3)  // set to true to estimate delta velocity bias for XYZ axes, set to false to estimate only for Z
+
 // Integer definitions for mag_fusion_type
 #define MAG_FUSE_TYPE_AUTO      0   // The selection of either heading or 3D magnetometer fusion will be automatic
 #define MAG_FUSE_TYPE_HEADING   1   // Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg

EKF/covariance.cpp

@@ -198,7 +198,7 @@ void Ekf::predictCovariance()
 	float accel_noise = math::constrain(_params.accel_noise, 0.0f, 1.0f);
 	dvxVar = dvyVar = dvzVar = sq(dt * accel_noise);
 
-	// predict covarinace matrix
+	// predict the covariance
 
 	// intermediate calculations
 	float SF[21] = {};
@@ -261,7 +261,12 @@ void Ekf::predictCovariance()
 	SPP[10] = SF[16];
 
 	// covariance update
-	float nextP[24][24] = {};
+	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;
@@ -354,6 +359,9 @@ void Ekf::predictCovariance()
 	nextP[10][12] = P[10][12];
 	nextP[11][12] = P[11][12];
 	nextP[12][12] = P[12][12];
+
+	// 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];
 		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;
@@ -383,6 +391,11 @@ 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);
+		zeroCols(nextP,13,14);
+	}
+
 	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];
 	nextP[1][15] = P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)*0.5f;
 	nextP[2][15] = P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)*0.5f;
@@ -527,6 +540,10 @@ void Ekf::predictCovariance()
 		nextP[20][21] = P[20][21];
 		nextP[21][21] = P[21][21];
 
+	} else {
+		zeroRows(nextP,16,21);
+		zeroCols(nextP,16,21);
+
 	}
 
 	// Don't do covariance prediction on wind states unless we are using them
@@ -624,11 +641,10 @@ void Ekf::predictCovariance()
 		nextP[22][23] = P[22][23];
 		nextP[23][23] = P[23][23];
 
-	}
+	} else {
+		zeroRows(nextP,22,23);
+		zeroCols(nextP,22,23);
 
-	// add process noise
-	for (unsigned i = 0; i < _k_num_states; i++) {
-		nextP[i][i] += process_noise[i];
 	}
 
 	// stop position covariance growth if our total position variance reaches 100m