ekf-yaw-est: do not store full S_inverse

PX4 · Nov 7, 2024 · 651675f · 651675f
1 parent 7b04ea0
commit 651675f
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 11 deletions.
diff --git a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
@@ -324,11 +324,12 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index, const Vector2f &vel_NE, co
 
 	matrix::Matrix<float, 3, 2> K;
 	matrix::SquareMatrix<float, 3> P_new;
+	matrix::SquareMatrix<float, 2> S_inverse;
 
 	sym::YawEstComputeMeasurementUpdate(_ekf_gsf[model_index].P,
 					    vel_obs_var,
 					    FLT_EPSILON,
-					    &_ekf_gsf[model_index].S_inverse,
+					    &S_inverse,
 					    &_ekf_gsf[model_index].S_det_inverse,
 					    &K,
 					    &P_new);
@@ -347,15 +348,17 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index, const Vector2f &vel_NE, co
 		_ekf_gsf[model_index].P(index, index) = fmaxf(_ekf_gsf[model_index].P(index, index), min_var);
 	}
 
-	// test ratio = transpose(innovation) * inverse(innovation variance) * innovation = [1x2] * [2,2] * [2,1] = [1,1]
-	const float test_ratio = _ekf_gsf[model_index].innov * (_ekf_gsf[model_index].S_inverse * _ekf_gsf[model_index].innov);
+	// normalized innovation squared = transpose(innovation) * inverse(innovation variance) * innovation = [1x2] * [2,2] * [2,1] = [1,1]
+	_ekf_gsf[model_index].nis = _ekf_gsf[model_index].innov * (S_inverse * _ekf_gsf[model_index].innov);
 
 	// Perform a chi-square innovation consistency test and calculate a compression scale factor
 	// that limits the magnitude of innovations to 5-sigma
-	// If the test ratio is greater than 25 (5 Sigma) then reduce the length of the innovation vector to clip it at 5-Sigma
+	// If the normalized innovation squared is greater than 25 (5 Sigma) then reduce the length of the innovation vector to clip it at 5-Sigma
 	// This protects from large measurement spikes
-	const float innov_comp_scale_factor = test_ratio > 25.f ? sqrtf(25.0f / test_ratio) : 1.f;
-	_ekf_gsf[model_index].innov *= innov_comp_scale_factor;
+	if (_ekf_gsf[model_index].nis > sq(5.f)) {
+		_ekf_gsf[model_index].innov *= sqrtf(sq(5.f) / _ekf_gsf[model_index].nis);
+		_ekf_gsf[model_index].nis = sq(5.f);
+	}
 
 	// Correct the state vector
 	const Vector3f delta_state = -K * _ekf_gsf[model_index].innov;
@@ -410,10 +413,7 @@ void EKFGSF_yaw::initialiseEKFGSF(const Vector2f &vel_NE, const float vel_accura
 
 float EKFGSF_yaw::gaussianDensity(const uint8_t model_index) const
 {
-	// calculate transpose(innovation) * inv(S) * innovation
-	const float normDist = _ekf_gsf[model_index].innov.dot(_ekf_gsf[model_index].S_inverse * _ekf_gsf[model_index].innov);
-
-	return (1.f / (2.f * M_PI_F)) * sqrtf(_ekf_gsf[model_index].S_det_inverse) * expf(-0.5f * normDist);
+	return (1.f / (2.f * M_PI_F)) * sqrtf(_ekf_gsf[model_index].S_det_inverse) * expf(-0.5f * _ekf_gsf[model_index].nis);
 }
 
 bool EKFGSF_yaw::getLogData(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],

diff --git a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.h b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.h
@@ -121,7 +121,7 @@ class EKFGSF_yaw
 	struct {
 		matrix::Vector3f X{};                       // Vel North (m/s),  Vel East (m/s), yaw (rad)s
 		matrix::SquareMatrix<float, 3> P{};         // covariance matrix
-		matrix::SquareMatrix<float, 2> S_inverse{}; // inverse of the innovation covariance matrix
+		float nis{};                                // normalized innovation squared
 		float S_det_inverse{};                      // inverse of the innovation covariance matrix determinant
 		matrix::Vector2f innov{};                   // Velocity N,E innovation (m/s)
 	} _ekf_gsf[N_MODELS_EKFGSF] {};