Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Robustify yaw emergency estimator against GNSS glitches #23894

Open
wants to merge 3 commits into
base: main
Choose a base branch
from
Open

Robustify yaw emergency estimator against GNSS glitches #23894

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
61574ca
ekf-yaw-est: do not reset when all weights collapse
bresch Nov 4, 2024
6592a0a
ekf-yaw-est: wrap_pi when updating yaw
bresch Nov 5, 2024
55001a4
ekf-yaw-est: do not store full S_inverse
bresch Nov 5, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
39 changes: 16 additions & 23 deletions src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -132,27 +132,19 @@ void EKFGSF_yaw::fuseVelocity(const Vector2f &vel_NE, const float vel_accuracy,
float total_weight = 0.0f;
// calculate weighting for each model assuming a normal distribution
const float min_weight = 1e-5f;
uint8_t n_weight_clips = 0;

for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
_model_weights(model_index) = gaussianDensity(model_index) * _model_weights(model_index);

if (_model_weights(model_index) < min_weight) {
n_weight_clips++;
_model_weights(model_index) = min_weight;
}

total_weight += _model_weights(model_index);
}

// normalise the weighting function
if (n_weight_clips < N_MODELS_EKFGSF) {
_model_weights /= total_weight;

} else {
// all weights have collapsed due to excessive innovation variances so reset filters
reset();
}
_model_weights /= total_weight;
}

// Calculate a composite yaw vector as a weighted average of the states for each model.
Expand Down Expand Up @@ -332,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);
Expand All @@ -355,21 +348,24 @@ 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;

// Correct the state vector and capture the change in yaw angle
const float oldYaw = _ekf_gsf[model_index].X(2);
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);
}

_ekf_gsf[model_index].X -= (K * _ekf_gsf[model_index].innov) * innov_comp_scale_factor;
// Correct the state vector
const Vector3f delta_state = -K * _ekf_gsf[model_index].innov;
const float yawDelta = delta_state(2);

const float yawDelta = _ekf_gsf[model_index].X(2) - oldYaw;
_ekf_gsf[model_index].X.xy() += delta_state.xy();
_ekf_gsf[model_index].X(2) = wrap_pi(_ekf_gsf[model_index].X(2) + yawDelta);

// apply the change in yaw angle to the AHRS
// take advantage of sparseness in the yaw rotation matrix
Expand Down Expand Up @@ -417,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],
Expand Down
2 changes: 1 addition & 1 deletion src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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] {};
Expand Down
Loading