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 <[email protected]>

EKF/CMakeLists.txt

@@ -55,6 +55,8 @@ target_link_libraries(ecl_EKF PRIVATE ecl_geo ecl_geo_lookup mathlib)
 
 set_target_properties(ecl_EKF PROPERTIES PUBLIC_HEADER "ekf.h")
 
+target_compile_options(ecl_EKF PRIVATE -fno-associative-math)
+
 if(EKF_PYTHON_TESTS)
 	add_subdirectory(swig)
 endif()

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

EKF/ekf.h

@@ -363,6 +363,9 @@ class Ekf : public EstimatorInterface
 
 	float P[_k_num_states][_k_num_states] {};	///< state covariance matrix
 
+	Vector3f _delta_vel_bias_var_accum;		///< kahan summation algorithm accumulator for delta velocity bias variance
+	Vector3f _delta_angle_bias_var_accum;	///< kahan summation algorithm accumulator for delta angle bias variance
+
 	float _vel_pos_innov[6] {};	///< NED velocity and position innovations: 0-2 vel (m/sec),  3-5 pos (m)
 	float _vel_pos_innov_var[6] {};	///< NED velocity and position innovation variances: 0-2 vel ((m/sec)**2), 3-5 pos (m**2)
 	float _aux_vel_innov[2] {};	///< NE auxiliary velocity innovations: (m/sec)
@@ -707,4 +710,10 @@ class Ekf : public EstimatorInterface
 	// uncorrelate quaternion states from other states
 	void uncorrelateQuatStates();
 
+	// Use Kahan summation algorithm to get the sum of "sum_previous" and "input".
+	// This function relies on the caller to be responsible for keeping a copy of
+	// "accumulator" and passing this value at the next iteration.
+	// Ref: https://en.wikipedia.org/wiki/Kahan_summation_algorithm
+	float kahanSummation(float sum_previous, float input, float &accumulator) const;
+
 };

EKF/ekf_helper.cpp

@@ -1722,3 +1722,11 @@ void Ekf::save_mag_cov_data()
 		}
 	}
 }
+
+float Ekf::kahanSummation(float sum_previous, float input, float &accumulator) const
+{
+	float y = input - accumulator;
+	float t = sum_previous + y;
+	accumulator = (t - sum_previous) - y;
+	return t;
+}