revert and cleanup

src/lib/matrix/matrix/Matrix.hpp

@@ -162,24 +162,6 @@ class Matrix
 		return res;
 	}
 
-	// Using this function reduces the number of temporary variables needed to compute A * B.T
-	template<size_t P>
-	Matrix<Type, M, M> multiplyByTranspose(const Matrix<Type, P, N> &other) const
-	{
-		Matrix<Type, M, P> res;
-		const Matrix<Type, M, N> &self = *this;
-
-		for (size_t i = 0; i < M; i++) {
-			for (size_t k = 0; k < P; k++) {
-				for (size_t j = 0; j < N; j++) {
-					res(i, k) += self(i, j) * other(k, j);
-				}
-			}
-		}
-
-		return res;
-	}
-
 	// Element-wise multiplication
 	Matrix<Type, M, N> emult(const Matrix<Type, M, N> &other) const
 	{

src/modules/ekf2/EKF/airspeed_fusion.cpp

@@ -208,7 +208,7 @@ void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_sour
 		K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) = K_wind;
 	}
 
-	const bool is_fused = measurementUpdate(K, H, aid_src.observation_variance, aid_src.innovation, aid_src.innovation_variance);
+	const bool is_fused = measurementUpdate(K, H, aid_src.observation_variance, aid_src.innovation);
 
 	aid_src.fused = is_fused;
 	_fault_status.flags.bad_airspeed = !is_fused;

src/modules/ekf2/EKF/covariance.cpp

@@ -222,14 +222,6 @@ void Ekf::constrainStateVariances()
 #endif // CONFIG_EKF2_WIND
 }
 
-void Ekf::forceCovarianceSymmetry()
-{
-	// DEBUG
-	// P.isBlockSymmetric(0, 1e-9f);
-
-	P.makeRowColSymmetric<State::size>(0);
-}
-
 void Ekf::constrainStateVar(const IdxDof &state, float min, float max)
 {
 	for (unsigned i = state.idx; i < (state.idx + state.dof); i++) {
@@ -256,22 +248,6 @@ void Ekf::constrainStateVarLimitRatio(const IdxDof &state, float min, float max,
 	}
 }
 
-// if the covariance correction will result in a negative variance, then
-// the covariance matrix is unhealthy and must be corrected
-bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrixState &KHP)
-{
-	bool healthy = true;
-
-	for (int i = 0; i < State::size; i++) {
-		if (P(i, i) < KHP(i, i)) {
-			P.uncorrelateCovarianceSetVariance<1>(i, 0.f);
-			healthy = false;
-		}
-	}
-
-	return healthy;
-}
-
 void Ekf::resetQuatCov(const float yaw_noise)
 {
 	const float tilt_var = sq(math::max(_params.initial_tilt_err, 0.01f));

src/modules/ekf2/EKF/drag_fusion.cpp

@@ -160,7 +160,7 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
 
 			VectorState K = P * H / _aid_src_drag.innovation_variance[axis_index];
 
-			if (measurementUpdate(K, H, R_ACC, _aid_src_drag.innovation[axis_index], _aid_src_drag.innovation_variance[axis_index])) {
+			if (measurementUpdate(K, H, R_ACC, _aid_src_drag.innovation[axis_index])) {
 				fused[axis_index] = true;
 			}
 		}

src/modules/ekf2/EKF/ekf.h

@@ -468,7 +468,7 @@ class Ekf final : public EstimatorInterface
 	const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; }
 #endif // CONFIG_EKF2_AUXVEL
 
-	bool measurementUpdate(VectorState &K, const VectorState &H, const float R, const float innovation, const float innovation_variance)
+	bool measurementUpdate(VectorState &K, const VectorState &H, const float R, const float innovation)
 	{
 		clearInhibitedStateKalmanGains(K);
 
@@ -580,7 +580,6 @@ class Ekf final : public EstimatorInterface
 	float _yaw_rate_lpf_ef{0.0f};		///< Filtered angular rate about earth frame D axis (rad/sec)
 
 	SquareMatrixState P{};	///< state covariance matrix
-	SquareMatrixState _temp_square_matrix_state{}; ///< Used to pre-allocate a large matrix
 
 #if defined(CONFIG_EKF2_DRAG_FUSION)
 	estimator_aid_source2d_s _aid_src_drag{};
@@ -910,49 +909,29 @@ class Ekf final : public EstimatorInterface
 	float getMagDeclination();
 #endif // CONFIG_EKF2_MAGNETOMETER
 
-	// returns true if the Kalman gains were modified
-	bool clearInhibitedStateKalmanGains(VectorState &K) const
+	void clearInhibitedStateKalmanGains(VectorState &K) const
 	{
-		static constexpr float kMinKalmanGain = 1e-9f;
-		bool kalman_gains_modified = false;
-
 		for (unsigned i = 0; i < State::gyro_bias.dof; i++) {
 			if (_gyro_bias_inhibit[i]) {
-				if (K(State::gyro_bias.idx + i) > kMinKalmanGain || K(State::gyro_bias.idx + i) < -kMinKalmanGain) {
-					kalman_gains_modified = true;
-				}
-
 				K(State::gyro_bias.idx + i) = 0.f;
 			}
 		}
 
 		for (unsigned i = 0; i < State::accel_bias.dof; i++) {
 			if (_accel_bias_inhibit[i]) {
-				if (K(State::accel_bias.idx + i) > kMinKalmanGain || K(State::accel_bias.idx + i) < -kMinKalmanGain) {
-					kalman_gains_modified = true;
-				}
-
 				K(State::accel_bias.idx + i) = 0.f;
 			}
 		}
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 		if (!_control_status.flags.mag) {
 			for (unsigned i = 0; i < State::mag_I.dof; i++) {
-				if (K(State::mag_I.idx + i) > kMinKalmanGain || K(State::mag_I.idx + i) < -kMinKalmanGain) {
-					kalman_gains_modified = true;
-				}
-
 				K(State::mag_I.idx + i) = 0.f;
 			}
 		}
 
 		if (!_control_status.flags.mag) {
 			for (unsigned i = 0; i < State::mag_B.dof; i++) {
-				if (K(State::mag_B.idx + i) > kMinKalmanGain || K(State::mag_B.idx + i) < -kMinKalmanGain) {
-					kalman_gains_modified = true;
-				}
-
 				K(State::mag_B.idx + i) = 0.f;
 			}
 		}
@@ -961,28 +940,15 @@ class Ekf final : public EstimatorInterface
 #if defined(CONFIG_EKF2_WIND)
 		if (!_control_status.flags.wind) {
 			for (unsigned i = 0; i < State::wind_vel.dof; i++) {
-
-				if (K(State::wind_vel.idx + i) > kMinKalmanGain || K(State::wind_vel.idx + i) < -kMinKalmanGain) {
-					kalman_gains_modified = true;
-				}
-
 				K(State::wind_vel.idx + i) = 0.f;
 			}
 		}
 #endif // CONFIG_EKF2_WIND
-
-		return kalman_gains_modified;
 	}
 
-	// if the covariance correction will result in a negative variance, then
-	// the covariance matrix is unhealthy and must be corrected
-	bool checkAndFixCovarianceUpdate(const SquareMatrixState &KHP);
-
 	// limit the diagonal of the covariance matrix
 	void constrainStateVariances();
 
-	void forceCovarianceSymmetry();
-
 	void constrainStateVar(const IdxDof &state, float min, float max);
 	void constrainStateVarLimitRatio(const IdxDof &state, float min, float max, float max_ratio = 1.e6f);
 

src/modules/ekf2/EKF/gps_yaw_fusion.cpp

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

src/modules/ekf2/EKF/gravity_fusion.cpp

@@ -111,7 +111,7 @@ void Ekf::controlGravityFusion(const imuSample &imu)
 		const bool accel_clipping = imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2];
 
 		if (_control_status.flags.gravity_vector && !_aid_src_gravity.innovation_rejected && !accel_clipping) {
-			fused[index] = measurementUpdate(K, H, _aid_src_gravity.observation_variance[index], _aid_src_gravity.innovation[index], _aid_src_gravity.innovation_variance[index]);
+			fused[index] = measurementUpdate(K, H, _aid_src_gravity.observation_variance[index], _aid_src_gravity.innovation[index]);
 		}
 	}
 

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -191,7 +191,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
 			Kfusion.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) = K_mag_B;
 		}
 
-		if (measurementUpdate(Kfusion, H, aid_src_mag.observation_variance[index], aid_src_mag.innovation[index], aid_src_mag.innovation_variance[index])) {
+		if (measurementUpdate(Kfusion, H, aid_src_mag.observation_variance[index], aid_src_mag.innovation[index])) {
 			fused[index] = true;
 			limitDeclination();
 
@@ -261,7 +261,7 @@ bool Ekf::fuseDeclination(float decl_sigma)
 		// Calculate the Kalman gains
 		VectorState Kfusion = P * H / innovation_variance;
 
-		const bool is_fused = measurementUpdate(Kfusion, H, R_DECL, innovation, innovation_variance);
+		const bool is_fused = measurementUpdate(Kfusion, H, R_DECL, innovation);
 
 		_fault_status.flags.bad_mag_decl = !is_fused;
 

src/modules/ekf2/EKF/optflow_fusion.cpp

@@ -146,7 +146,7 @@ void Ekf::fuseOptFlow()
 
 		VectorState Kfusion = P * H / _aid_src_optical_flow.innovation_variance[index];
 
-		if (measurementUpdate(Kfusion, H, _aid_src_optical_flow.observation_variance[index], _aid_src_optical_flow.innovation[index], _aid_src_optical_flow.observation_variance[index])) {
+		if (measurementUpdate(Kfusion, H, _aid_src_optical_flow.observation_variance[index], _aid_src_optical_flow.innovation[index])) {
 			fused[index] = true;
 		}
 	}

src/modules/ekf2/EKF/sideslip_fusion.cpp

@@ -142,7 +142,7 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
 		K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) = K_wind;
 	}
 
-	const bool is_fused = measurementUpdate(K, H, sideslip.observation_variance, sideslip.innovation, sideslip.innovation_variance);
+	const bool is_fused = measurementUpdate(K, H, sideslip.observation_variance, sideslip.innovation);
 
 	sideslip.fused = is_fused;
 	_fault_status.flags.bad_sideslip = !is_fused;

src/modules/ekf2/EKF/yaw_fusion.cpp

@@ -113,7 +113,7 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H
 		_innov_check_fail_status.flags.reject_yaw = false;
 	}
 
-	if (measurementUpdate(Kfusion, H_YAW, aid_src_status.observation_variance, aid_src_status.innovation, aid_src_status.innovation_variance)) {
+	if (measurementUpdate(Kfusion, H_YAW, aid_src_status.observation_variance, aid_src_status.innovation)) {
 
 		_time_last_heading_fuse = _time_delayed_us;