dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: fix mag declination innovation angle wrapping

This commit is contained in:
bresch 2023-03-21 17:45:04 +01:00 committed by Daniel Agar
parent 64e90b91aa
commit 3e3307c0d0
4 changed files with 31 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/ekf2/EKF/mag_fusion.cpp
View File

@ -47,7 +47,7 @@
#include "python/ekf_derivation/generated/compute_mag_z_innov_var_and_h.h"
#include "python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h"
#include "python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h"
#include "python/ekf_derivation/generated/compute_mag_declination_innov_innov_var_and_h.h"
#include "python/ekf_derivation/generated/compute_mag_declination_pred_innov_var_and_h.h"
#include <mathlib/mathlib.h>
@ -335,10 +335,12 @@ bool Ekf::fuseDeclination(float decl_sigma)
const float R_DECL = sq(decl_sigma);
Vector24f H;
float innovation;
float decl_pred;
float innovation_variance;
sym::ComputeMagDeclinationInnovInnovVarAndH(getStateAtFusionHorizonAsVector(), P, getMagDeclination(), R_DECL, FLT_EPSILON, &innovation, &innovation_variance, &H);
sym::ComputeMagDeclinationPredInnovVarAndH(getStateAtFusionHorizonAsVector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, &H);
const float innovation = wrap_pi(decl_pred - getMagDeclination());
if (innovation_variance < R_DECL) {
// variance calculation is badly conditioned

10
src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
View File

@ -321,21 +321,19 @@ def compute_yaw_312_innov_var_and_h_alternate(
return (innov_var, H.T)
def compute_mag_declination_innov_innov_var_and_h(
def compute_mag_declination_pred_innov_var_and_h(
state: VState,
P: MState,
meas: sf.Scalar,
R: sf.Scalar,
epsilon: sf.Scalar
) -> (sf.Scalar, VState):
) -> (sf.Scalar, sf.Scalar, VState):
meas_pred = sf.atan2(state[State.iy], state[State.ix], epsilon=epsilon)
innov = meas_pred - meas
H = sf.V1(meas_pred).jacobian(state)
innov_var = (H * P * H.T + R)[0,0]
return (innov, innov_var, H.T)
return (meas_pred, innov_var, H.T)
def predict_opt_flow(state, distance, epsilon):
q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
@ -518,7 +516,7 @@ generate_px4_function(compute_yaw_321_innov_var_and_h, output_names=["innov_var"
generate_px4_function(compute_yaw_321_innov_var_and_h_alternate, output_names=["innov_var", "H"])
generate_px4_function(compute_yaw_312_innov_var_and_h, output_names=["innov_var", "H"])
generate_px4_function(compute_yaw_312_innov_var_and_h_alternate, output_names=["innov_var", "H"])
generate_px4_function(compute_mag_declination_innov_innov_var_and_h, output_names=["innov", "innov_var", "H"])
generate_px4_function(compute_mag_declination_pred_innov_var_and_h, output_names=["pred", "innov_var", "H"])
generate_px4_function(compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"])
generate_px4_function(compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"])
generate_px4_function(compute_gnss_yaw_pred_innov_var_and_h, output_names=["meas_pred", "innov_var", "H"])

24
src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_declination_innov_innov_var_and_h.h → src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_declination_pred_innov_var_and_h.h
View File

@ -13,28 +13,26 @@ namespace sym {
/**
* This function was autogenerated from a symbolic function. Do not modify by hand.
*
* Symbolic function: compute_mag_declination_innov_innov_var_and_h
* Symbolic function: compute_mag_declination_pred_innov_var_and_h
*
* Args:
* state: Matrix24_1
* P: Matrix24_24
* meas: Scalar
* R: Scalar
* epsilon: Scalar
*
* Outputs:
* innov: Scalar
* pred: Scalar
* innov_var: Scalar
* H: Matrix24_1
*/
template <typename Scalar>
void ComputeMagDeclinationInnovInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
const matrix::Matrix<Scalar, 24, 24>& P,
const Scalar meas, const Scalar R, const Scalar epsilon,
Scalar* const innov = nullptr,
Scalar* const innov_var = nullptr,
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
// Total ops: 23
void ComputeMagDeclinationPredInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
const Scalar epsilon, Scalar* const pred = nullptr,
Scalar* const innov_var = nullptr,
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
// Total ops: 22
// Input arrays
@ -47,10 +45,10 @@ void ComputeMagDeclinationInnovInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>&
const Scalar _tmp3 = _tmp0 * _tmp1;
// Output terms (3)
if (innov != nullptr) {
Scalar& _innov = (*innov);
if (pred != nullptr) {
Scalar& _pred = (*pred);
_innov = -meas + std::atan2(state(17, 0), _tmp0);
_pred = std::atan2(state(17, 0), _tmp0);
}
if (innov_var != nullptr) {

21
src/modules/ekf2/test/test_EKF_mag_declination_generated.cpp
View File

@ -35,7 +35,7 @@
#include "EKF/ekf.h"
#include "test_helper/comparison_helper.h"
#include "../EKF/python/ekf_derivation/generated/compute_mag_declination_innov_innov_var_and_h.h"
#include "../EKF/python/ekf_derivation/generated/compute_mag_declination_pred_innov_var_and_h.h"
using namespace matrix;
@ -50,15 +50,15 @@ TEST(MagDeclinationGenerated, declination90deg)
SquareMatrix24f P = createRandomCovarianceMatrix24f();
Vector24f H;
float innov;
float decl_pred;
float innov_var;
const float decl = radians(90.f);
sym::ComputeMagDeclinationInnovInnovVarAndH(state_vector, P, decl, R, FLT_EPSILON, &innov, &innov_var, &H);
sym::ComputeMagDeclinationPredInnovVarAndH(state_vector, P, R, FLT_EPSILON, &decl_pred, &innov_var, &H);
// THEN: Even at the singularity point, atan2 is still defined
EXPECT_TRUE(innov_var < 5000.f && innov_var > R) << "innov_var = " << innov_var;
EXPECT_LT(fabsf(innov), 1e-6f);
EXPECT_LT(fabsf(wrap_pi(decl_pred - decl)), 1e-6f);
}
TEST(MagDeclinationGenerated, declinationUndefined)
@ -72,15 +72,15 @@ TEST(MagDeclinationGenerated, declinationUndefined)
SquareMatrix24f P = createRandomCovarianceMatrix24f();
Vector24f H;
float innov;
float decl_pred;
float innov_var;
const float decl = radians(0.f);
sym::ComputeMagDeclinationInnovInnovVarAndH(state_vector, P, decl, R, FLT_EPSILON, &innov, &innov_var, &H);
sym::ComputeMagDeclinationPredInnovVarAndH(state_vector, P, R, FLT_EPSILON, &decl_pred, &innov_var, &H);
// THEN: the innovation variance is gigantic but finite
EXPECT_TRUE(PX4_ISFINITE(innov_var) && innov_var > R && innov_var > 1e9f) << "innov_var = " << innov_var;
EXPECT_LT(fabsf(innov), 1e-6f);
EXPECT_LT(fabsf(wrap_pi(decl_pred - decl)), 1e-6f);
}
void sympyMagDeclInnovVarHAndK(float magN, float magE, const SquareMatrix24f &P, float R_DECL,
@ -156,14 +156,15 @@ TEST(MagDeclinationGenerated, SympyVsSymforce)
Vector24f K_sympy;
Vector24f K_symforce;
float innov_sympy;
float innov_symforce;
float pred_symforce;
float innov_var_sympy;
float innov_var_symforce;
sympyMagDeclInnovVarHAndK(mag_n, mag_e, P, R_DECL, innov_var_sympy, H_sympy, K_sympy);
innov_sympy = std::atan2(mag_e, mag_n) - decl;
sym::ComputeMagDeclinationInnovInnovVarAndH(state_vector, P, decl, R_DECL, FLT_EPSILON, &innov_symforce,
innov_sympy = wrap_pi(std::atan2(mag_e, mag_n) - decl);
sym::ComputeMagDeclinationPredInnovVarAndH(state_vector, P, R_DECL, FLT_EPSILON, &pred_symforce,
&innov_var_symforce, &H_symforce);
const float innov_symforce = wrap_pi(pred_symforce - decl);
K_symforce = P * H_symforce / innov_var_symforce;
EXPECT_NEAR(innov_sympy, innov_symforce, 1e-5f);