ekf2: use more efficient covariance update computation

KSK' is faster to compute than KHP and is mathematically equivalent

src/modules/ekf2/EKF/airspeed_fusion.cpp

@@ -115,15 +115,13 @@ void Ekf::fuseAirspeed(estimator_aid_source1d_s &airspeed)
 
 	sym::ComputeAirspeedHAndK(getStateAtFusionHorizonAsVector(), P, innov_var, FLT_EPSILON, &H, &K);
 
-	SparseVector24f<4,5,6,22,23> H_sparse(H);
-
 	if (update_wind_only) {
 		for (unsigned row = 0; row <= 21; row++) {
 			K(row) = 0.f;
 		}
 	}
 
-	const bool is_fused = measurementUpdate(K, H_sparse, airspeed.innovation);
+	const bool is_fused = measurementUpdate(K, airspeed.innovation_variance, airspeed.innovation);
 
 	airspeed.fused = is_fused;
 	_fault_status.flags.bad_airspeed = !is_fused;

src/modules/ekf2/EKF/drag_fusion.cpp

@@ -331,7 +331,7 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
 
 		// if the innovation consistency check fails then don't fuse the sample
 		if (_drag_test_ratio(axis_index) <= 1.0f) {
-			measurementUpdate(Kfusion, Hfusion, _drag_innov(axis_index));
+			measurementUpdate(Kfusion, _drag_innov_var(axis_index), _drag_innov(axis_index));
 		}
 	}
 }

src/modules/ekf2/EKF/ekf.h

@@ -793,35 +793,7 @@ private:
 	// update the rotation matrix which transforms EV navigation frame measurements into NED
 	void calcExtVisRotMat();
 
-	// matrix vector multiplication for computing K<24,1> * H<1,24> * P<24,24>
-	// that is optimized by exploring the sparsity in H
-	template <size_t ...Idxs>
-	SquareMatrix24f computeKHP(const Vector24f &K, const SparseVector24f<Idxs...> &H) const
-	{
-		// K(HP) and (KH)P are equivalent (matrix multiplication is associative)
-		// but K(HP) is computationally much less expensive
-		Vector24f HP;
-		for (unsigned i = 0; i < H.non_zeros(); i++) {
-			const size_t row = H.index(i);
-			for (unsigned col = 0; col < _k_num_states; col++) {
-				HP(col) = HP(col) + H.atCompressedIndex(i) * P(row, col);
-			}
-		}
-
-		SquareMatrix24f KHP;
-		for (unsigned row = 0; row < _k_num_states; row++) {
-			for (unsigned col = 0; col < _k_num_states; col++) {
-				KHP(row, col) = K(row) * HP(col);
-			}
-		}
-
-		return KHP;
-	}
-
-	// measurement update with a single measurement
-	// returns true if fusion is performed
-	template <size_t ...Idxs>
-	bool measurementUpdate(Vector24f &K, const SparseVector24f<Idxs...> &H, float innovation)
+	bool measurementUpdate(Vector24f &K, float innovation_variance, float innovation)
 	{
 		for (unsigned i = 0; i < 3; i++) {
 			if (_accel_bias_inhibit[i]) {
@@ -829,10 +801,16 @@ private:
 			}
 		}
 
-		// apply covariance correction via P_new = (I -K*H)*P
-		// first calculate expression for KHP
-		// then calculate P - KHP
-		const SquareMatrix24f KHP = computeKHP(K, H);
+		const Vector24f KS = K * innovation_variance;
+		SquareMatrix24f KHP;
+
+		for (unsigned row = 0; row < _k_num_states; row++) {
+			for (unsigned col = 0; col < _k_num_states; col++) {
+				// Instad of literally computing KHP, use an equvalent
+				// equation involving less mathematical operations
+				KHP(row, col) = KS(row) * K(col);
+			}
+		}
 
 		const bool is_healthy = checkAndFixCovarianceUpdate(KHP);
 

src/modules/ekf2/EKF/gps_yaw_fusion.cpp

@@ -132,7 +132,7 @@ void Ekf::fuseGpsYaw()
 	// only calculate gains for states we are using
 	Vector24f Kfusion = P * Hfusion / gnss_yaw.innovation_variance;
 
-	const bool is_fused = measurementUpdate(Kfusion, Hfusion, gnss_yaw.innovation);
+	const bool is_fused = measurementUpdate(Kfusion, gnss_yaw.innovation_variance, gnss_yaw.innovation);
 	_fault_status.flags.bad_hdg = !is_fused;
 	gnss_yaw.fused = is_fused;
 

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -200,7 +200,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
 			}
 		}
 
-		if (measurementUpdate(Kfusion, Hfusion, aid_src_mag.innovation[index])) {
+		if (measurementUpdate(Kfusion, aid_src_mag.innovation_variance[index], aid_src_mag.innovation[index])) {
 			fused[index] = true;
 			limitDeclination();
 
@@ -312,9 +312,7 @@ bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_so
 		_innov_check_fail_status.flags.reject_yaw = false;
 	}
 
-	SparseVector24f<0,1,2,3> Hfusion(H_YAW);
-
-	if (measurementUpdate(Kfusion, Hfusion, aid_src_status.innovation)) {
+	if (measurementUpdate(Kfusion, aid_src_status.innovation_variance, aid_src_status.innovation)) {
 
 		_time_last_heading_fuse = _imu_sample_delayed.time_us;
 
@@ -353,7 +351,7 @@ bool Ekf::fuseDeclination(float decl_sigma)
 	// Calculate the Kalman gains
 	Vector24f Kfusion = P * Hfusion / innovation_variance;
 
-	const bool is_fused = measurementUpdate(Kfusion, Hfusion, innovation);
+	const bool is_fused = measurementUpdate(Kfusion, innovation_variance, innovation);
 
 	_fault_status.flags.bad_mag_decl = !is_fused;
 

src/modules/ekf2/EKF/optflow_fusion.cpp

@@ -142,7 +142,7 @@ void Ekf::fuseOptFlow()
 		SparseVector24f<0,1,2,3,4,5,6> Hfusion(H);
 		Vector24f Kfusion = P * Hfusion / _aid_src_optical_flow.innovation_variance[index];
 
-		if (measurementUpdate(Kfusion, Hfusion, _aid_src_optical_flow.innovation[index])) {
+		if (measurementUpdate(Kfusion, _aid_src_optical_flow.innovation_variance[index], _aid_src_optical_flow.innovation[index])) {
 			fused[index] = true;
 		}
 	}

src/modules/ekf2/EKF/sideslip_fusion.cpp

@@ -109,15 +109,13 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
 
 	sym::ComputeSideslipHAndK(getStateAtFusionHorizonAsVector(), P, sideslip.innovation_variance, FLT_EPSILON, &H, &K);
 
-	SparseVector24f<0,1,2,3,4,5,6,22,23> H_sparse(H);
-
 	if (update_wind_only) {
 		for (unsigned row = 0; row <= 21; row++) {
 			K(row) = 0.f;
 		}
 	}
 
-	const bool is_fused = measurementUpdate(K, H_sparse, sideslip.innovation);
+	const bool is_fused = measurementUpdate(K, sideslip.innovation_variance, sideslip.innovation);
 
 	sideslip.fused = is_fused;
 	_fault_status.flags.bad_sideslip = !is_fused;