From bdd043f27f80040f142639acfa2df7cdd63d15ef Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 12 Oct 2022 16:05:49 +0200 Subject: [PATCH] ekf2: fix sideslip fusion sign --- src/modules/ekf2/EKF/python/ekf_derivation/derivation.py | 2 +- .../generated/compute_sideslip_innov_and_innov_var.h | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index ebb1289704..e6bc963a51 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -126,7 +126,7 @@ def compute_sideslip_innov_and_innov_var( sideslip_pred = predict_sideslip(state, epsilon); - innov = 0.0 - sideslip_pred + innov = sideslip_pred - 0.0 H = sf.V1(sideslip_pred).jacobian(state) innov_var = (H * P * H.T + R)[0,0] diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h index ee96db3c05..3d181c5c80 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h @@ -30,7 +30,7 @@ void ComputeSideslipInnovAndInnovVar(const matrix::Matrix& state, const matrix::Matrix& P, const Scalar R, const Scalar epsilon, Scalar* const innov = nullptr, Scalar* const innov_var = nullptr) { - // Total ops: 277 + // Total ops: 276 // Input arrays @@ -85,7 +85,7 @@ void ComputeSideslipInnovAndInnovVar(const matrix::Matrix& state, if (innov != nullptr) { Scalar& _innov = (*innov); - _innov = -_tmp15 * _tmp21; + _innov = _tmp15 * _tmp21; } if (innov_var != nullptr) {