implemented Kahan summation algorithm for adding process noise to delta

angle- and delta velocity bias variance

- the contribution of process noise per iteration for these states can be so
small that it gets lost if using standard floating point summation

Signed-off-by: Roman <bapstroman@gmail.com>

EKF/covariance.cpp

@@ -54,6 +54,9 @@ void Ekf::initialiseCovariance()
 		}
 	}
 
+	_delta_angle_bias_var_accum.setZero();
+	_delta_vel_bias_var_accum.setZero();
+
 	// calculate average prediction time step in sec
 	float dt = FILTER_UPDATE_PERIOD_S;
 
@@ -430,10 +433,17 @@ void Ekf::predictCovariance()
 	nextP[12][12] = P[12][12];
 
 	// add process noise that is not from the IMU
-	for (unsigned i = 0; i <= 12; i++) {
+	for (unsigned i = 0; i <= 9; i++) {
 		nextP[i][i] += process_noise[i];
 	}
 
+	// process noise contribution for delta angle states can be very small compared to
+	// the variances, therefore use algorithm to minimise numerical error
+	for (unsigned i = 10; i <=12; i++) {
+		const int index = i-10;
+		nextP[i][i] = kahanSummation(nextP[i][i], process_noise[i], _delta_angle_bias_var_accum(index));
+	}
+
 	// Don't calculate these covariance terms if IMU delta velocity bias estimation is inhibited
 	if (!(_params.fusion_mode & MASK_INHIBIT_ACC_BIAS) && !_accel_bias_inhibit) {
 
@@ -485,14 +495,18 @@ void Ekf::predictCovariance()
 		nextP[15][15] = P[15][15];
 
 		// add process noise that is not from the IMU
+		// process noise contributiton for delta velocity states can be very small compared to
+		// the variances, therefore use algorithm to minimise numerical error
 		for (unsigned i = 13; i <= 15; i++) {
-			nextP[i][i] += process_noise[i];
+			const int index = i-13;
+			nextP[i][i] = kahanSummation(nextP[i][i], process_noise[i], _delta_vel_bias_var_accum(index));
 		}
 
 	} else {
 		// Inhibit delta velocity bias learning by zeroing the covariance terms
 		zeroRows(nextP, 13, 15);
 		zeroCols(nextP, 13, 15);
+		_delta_vel_bias_var_accum.setZero();
 	}
 
 	// Don't do covariance prediction on magnetic field states unless we are using 3-axis fusion