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

ekf2: fix sideslip fusion sign

Browse Source
This commit is contained in:
bresch
2022-10-12 16:05:49 +02:00
committed by Mathieu Bresciani
parent 53865118fb
commit bdd043f27f
2 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
+1 -1
View File
@@ -126,7 +126,7 @@ def compute_sideslip_innov_and_innov_var(
sideslip_pred = predict_sideslip(state, epsilon); sideslip_pred = predict_sideslip(state, epsilon);
innov = 0.0 - sideslip_pred innov = sideslip_pred - 0.0
H = sf.V1(sideslip_pred).jacobian(state) H = sf.V1(sideslip_pred).jacobian(state)
innov_var = (H * P * H.T + R)[0,0] innov_var = (H * P * H.T + R)[0,0]
src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h
+2 -2
View File
@@ -30,7 +30,7 @@ void ComputeSideslipInnovAndInnovVar(const matrix::Matrix<Scalar, 24, 1>& state,
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R, const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
const Scalar epsilon, Scalar* const innov = nullptr, const Scalar epsilon, Scalar* const innov = nullptr,
Scalar* const innov_var = nullptr) { Scalar* const innov_var = nullptr) {
// Total ops: 277 // Total ops: 276
// Input arrays // Input arrays
@@ -85,7 +85,7 @@ void ComputeSideslipInnovAndInnovVar(const matrix::Matrix<Scalar, 24, 1>& state,
if (innov != nullptr) { if (innov != nullptr) {
Scalar& _innov = (*innov); Scalar& _innov = (*innov);
_innov = -_tmp15 * _tmp21; _innov = _tmp15 * _tmp21;
} }
if (innov_var != nullptr) { if (innov_var != nullptr) {