Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-27 02:06:27 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

ekf-yaw-est: do not store full S_inverse

Browse Source
This commit is contained in:
bresch
2024-11-05 14:10:16 +01:00
committed by Daniel Agar
parent 7b04ea0dca
commit 651675fed0
2 changed files with 11 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
+10 -10
View File
@@ -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],
src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.h
+1 -1
View File
@@ -121,7 +121,7 @@ private:
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] {};