fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 16:08:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf-yaw-est: do not store full S_inverse

Browse Source
This commit is contained in:
bresch
2024-11-07 11:37:56 -05: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::Matrix<float, 3, 2> K;
matrix::SquareMatrix<float, 3> P_new; matrix::SquareMatrix<float, 3> P_new;
matrix::SquareMatrix<float, 2> S_inverse;
sym::YawEstComputeMeasurementUpdate(_ekf_gsf[model_index].P, sym::YawEstComputeMeasurementUpdate(_ekf_gsf[model_index].P,
vel_obs_var, vel_obs_var,
FLT_EPSILON, FLT_EPSILON,
&_ekf_gsf[model_index].S_inverse, &S_inverse,
&_ekf_gsf[model_index].S_det_inverse, &_ekf_gsf[model_index].S_det_inverse,
&K, &K,
&P_new); &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); _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] // normalized innovation squared = 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); _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 // Perform a chi-square innovation consistency test and calculate a compression scale factor
// that limits the magnitude of innovations to 5-sigma // 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 // This protects from large measurement spikes
const float innov_comp_scale_factor = test_ratio > 25.f ? sqrtf(25.0f / test_ratio) : 1.f; if (_ekf_gsf[model_index].nis > sq(5.f)) {
_ekf_gsf[model_index].innov *= innov_comp_scale_factor; _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 // Correct the state vector
const Vector3f delta_state = -K * _ekf_gsf[model_index].innov; 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 float EKFGSF_yaw::gaussianDensity(const uint8_t model_index) const
{ {
// calculate transpose(innovation) * inv(S) * innovation return (1.f / (2.f * M_PI_F)) * sqrtf(_ekf_gsf[model_index].S_det_inverse) * expf(-0.5f * _ekf_gsf[model_index].nis);
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);
} }
bool EKFGSF_yaw::getLogData(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], 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 { struct {
matrix::Vector3f X{}; // Vel North (m/s), Vel East (m/s), yaw (rad)s matrix::Vector3f X{}; // Vel North (m/s), Vel East (m/s), yaw (rad)s
matrix::SquareMatrix<float, 3> P{}; // covariance matrix 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 float S_det_inverse{}; // inverse of the innovation covariance matrix determinant
matrix::Vector2f innov{}; // Velocity N,E innovation (m/s) matrix::Vector2f innov{}; // Velocity N,E innovation (m/s)
} _ekf_gsf[N_MODELS_EKFGSF] {}; } _ekf_gsf[N_MODELS_EKFGSF] {};