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

ekf2: fix mag declination innovation angle wrapping

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
+4 -6
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"])
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
+11 -13
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) {