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

ekf2: remove old yaw 321 and 312 derivations

This commit is contained in:
bresch 2024-03-13 09:57:51 +01:00 committed by Daniel Agar
parent ee63f3e664
commit b2f1122372
7 changed files with 2 additions and 436 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -408,74 +408,6 @@ def compute_yaw_innov_var_and_h(
return (innov_var, H.T)
def compute_yaw_321_innov_var_and_h(
state: VState,
P: MTangent,
R: sf.Scalar,
epsilon: sf.Scalar
) -> (sf.Scalar, VTangent):
state = vstate_to_state(state)
R_to_earth = state["quat_nominal"].to_rotation_matrix()
# Fix the singularity at pi/2 by inserting epsilon
meas_pred = sf.atan2(R_to_earth[1,0], R_to_earth[0,0], epsilon=epsilon)
H = sf.V1(meas_pred).jacobian(state)
innov_var = (H * P * H.T + R)[0,0]
return (innov_var, H.T)
def compute_yaw_321_innov_var_and_h_alternate(
state: VState,
P: MTangent,
R: sf.Scalar,
epsilon: sf.Scalar
) -> (sf.Scalar, VTangent):
state = vstate_to_state(state)
R_to_earth = state["quat_nominal"].to_rotation_matrix()
# Alternate form that has a singularity at yaw 0 instead of pi/2
meas_pred = sf.pi/2 - sf.atan2(R_to_earth[0,0], R_to_earth[1,0], epsilon=epsilon)
H = sf.V1(meas_pred).jacobian(state)
innov_var = (H * P * H.T + R)[0,0]
return (innov_var, H.T)
def compute_yaw_312_innov_var_and_h(
state: VState,
P: MTangent,
R: sf.Scalar,
epsilon: sf.Scalar
) -> (sf.Scalar, VTangent):
state = vstate_to_state(state)
R_to_earth = state["quat_nominal"].to_rotation_matrix()
# Alternate form to be used when close to pitch +-pi/2
meas_pred = sf.atan2(-R_to_earth[0,1], R_to_earth[1,1], epsilon=epsilon)
H = sf.V1(meas_pred).jacobian(state)
innov_var = (H * P * H.T + R)[0,0]
return (innov_var, H.T)
def compute_yaw_312_innov_var_and_h_alternate(
state: VState,
P: MTangent,
R: sf.Scalar,
epsilon: sf.Scalar
) -> (sf.Scalar, VTangent):
state = vstate_to_state(state)
R_to_earth = state["quat_nominal"].to_rotation_matrix()
# Alternate form to be used when close to pitch +-pi/2
meas_pred = sf.pi/2 - sf.atan2(-R_to_earth[1,1], R_to_earth[0,1], epsilon=epsilon)
H = sf.V1(meas_pred).jacobian(state)
innov_var = (H * P * H.T + R)[0,0]
return (innov_var, H.T)
def compute_mag_declination_pred_innov_var_and_h(
state: VState,
P: MTangent,
@ -697,10 +629,6 @@ if not args.disable_wind:
generate_px4_function(compute_wind_init_and_cov_from_airspeed, output_names=["wind", "P_wind"])
generate_px4_function(compute_yaw_innov_var_and_h, 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_yaw_321_innov_var_and_h, output_names=["innov_var", "H"])
generate_px4_function(compute_yaw_321_innov_var_and_h_alternate, output_names=["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"])

76
src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h
View File

@ -1,76 +0,0 @@
// -----------------------------------------------------------------------------
// This file was autogenerated by symforce from template:
// function/FUNCTION.h.jinja
// Do NOT modify by hand.
// -----------------------------------------------------------------------------
#pragma once
#include <matrix/math.hpp>
namespace sym {
/**
* This function was autogenerated from a symbolic function. Do not modify by hand.
*
* Symbolic function: compute_yaw_312_innov_var_and_h
*
* Args:
* state: Matrix24_1
* P: Matrix23_23
* R: Scalar
* epsilon: Scalar
*
* Outputs:
* innov_var: Scalar
* H: Matrix23_1
*/
template <typename Scalar>
void ComputeYaw312InnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
const Scalar epsilon, Scalar* const innov_var = nullptr,
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
// Total ops: 53
// Input arrays
// Intermediate terms (15)
const Scalar _tmp0 = 2 * state(0, 0);
const Scalar _tmp1 = -_tmp0 * state(3, 0);
const Scalar _tmp2 = 2 * state(1, 0);
const Scalar _tmp3 = _tmp2 * state(2, 0);
const Scalar _tmp4 = _tmp1 + _tmp3;
const Scalar _tmp5 = std::pow(state(3, 0), Scalar(2));
const Scalar _tmp6 = std::pow(state(1, 0), Scalar(2));
const Scalar _tmp7 = -2 * _tmp5 - 2 * _tmp6 + 1;
const Scalar _tmp8 = _tmp7 + epsilon * ((((_tmp7) > 0) - ((_tmp7) < 0)) + Scalar(0.5));
const Scalar _tmp9 = std::pow(_tmp8, Scalar(2));
const Scalar _tmp10 = _tmp4 / _tmp9;
const Scalar _tmp11 = Scalar(1.0) / (_tmp8);
const Scalar _tmp12 = _tmp9 / (std::pow(_tmp4, Scalar(2)) + _tmp9);
const Scalar _tmp13 = _tmp12 * (_tmp10 * (_tmp1 - _tmp3) -
_tmp11 * (_tmp5 - _tmp6 - std::pow(state(0, 0), Scalar(2)) +
std::pow(state(2, 0), Scalar(2))));
const Scalar _tmp14 = _tmp12 * (_tmp10 * (-_tmp2 * state(0, 0) + 2 * state(2, 0) * state(3, 0)) -
_tmp11 * (_tmp0 * state(2, 0) + _tmp2 * state(3, 0)));
// Output terms (2)
if (innov_var != nullptr) {
Scalar& _innov_var = (*innov_var);
_innov_var = R + _tmp13 * (P(0, 2) * _tmp14 + P(2, 2) * _tmp13) +
_tmp14 * (P(0, 0) * _tmp14 + P(2, 0) * _tmp13);
}
if (H != nullptr) {
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
_h.setZero();
_h(0, 0) = _tmp14;
_h(2, 0) = _tmp13;
}
} // NOLINT(readability/fn_size)
// NOLINTNEXTLINE(readability/fn_size)
} // namespace sym

76
src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h_alternate.h
View File

@ -1,76 +0,0 @@
// -----------------------------------------------------------------------------
// This file was autogenerated by symforce from template:
// function/FUNCTION.h.jinja
// Do NOT modify by hand.
// -----------------------------------------------------------------------------
#pragma once
#include <matrix/math.hpp>
namespace sym {
/**
* This function was autogenerated from a symbolic function. Do not modify by hand.
*
* Symbolic function: compute_yaw_312_innov_var_and_h_alternate
*
* Args:
* state: Matrix24_1
* P: Matrix23_23
* R: Scalar
* epsilon: Scalar
*
* Outputs:
* innov_var: Scalar
* H: Matrix23_1
*/
template <typename Scalar>
void ComputeYaw312InnovVarAndHAlternate(const matrix::Matrix<Scalar, 24, 1>& state,
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
const Scalar epsilon, Scalar* const innov_var = nullptr,
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
// Total ops: 57
// Input arrays
// Intermediate terms (15)
const Scalar _tmp0 = 2 * state(0, 0);
const Scalar _tmp1 = 2 * state(1, 0);
const Scalar _tmp2 = std::pow(state(3, 0), Scalar(2));
const Scalar _tmp3 = std::pow(state(1, 0), Scalar(2));
const Scalar _tmp4 = -2 * _tmp2 - 2 * _tmp3 + 1;
const Scalar _tmp5 = -_tmp0 * state(3, 0);
const Scalar _tmp6 = _tmp1 * state(2, 0);
const Scalar _tmp7 = _tmp5 + _tmp6;
const Scalar _tmp8 = _tmp7 + epsilon * ((((_tmp7) > 0) - ((_tmp7) < 0)) + Scalar(0.5));
const Scalar _tmp9 = std::pow(_tmp8, Scalar(2));
const Scalar _tmp10 = _tmp4 / _tmp9;
const Scalar _tmp11 = Scalar(1.0) / (_tmp8);
const Scalar _tmp12 = _tmp9 / (std::pow(_tmp4, Scalar(2)) + _tmp9);
const Scalar _tmp13 = _tmp12 * (_tmp10 * (_tmp0 * state(2, 0) + _tmp1 * state(3, 0)) -
_tmp11 * (-_tmp1 * state(0, 0) + 2 * state(2, 0) * state(3, 0)));
const Scalar _tmp14 = _tmp12 * (_tmp10 * (_tmp2 - _tmp3 - std::pow(state(0, 0), Scalar(2)) +
std::pow(state(2, 0), Scalar(2))) -
_tmp11 * (_tmp5 - _tmp6));
// Output terms (2)
if (innov_var != nullptr) {
Scalar& _innov_var = (*innov_var);
_innov_var = R - _tmp13 * (-P(0, 0) * _tmp13 - P(2, 0) * _tmp14) -
_tmp14 * (-P(0, 2) * _tmp13 - P(2, 2) * _tmp14);
}
if (H != nullptr) {
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
_h.setZero();
_h(0, 0) = -_tmp13;
_h(2, 0) = -_tmp14;
}
} // NOLINT(readability/fn_size)
// NOLINTNEXTLINE(readability/fn_size)
} // namespace sym

76
src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h
View File

@ -1,76 +0,0 @@
// -----------------------------------------------------------------------------
// This file was autogenerated by symforce from template:
// function/FUNCTION.h.jinja
// Do NOT modify by hand.
// -----------------------------------------------------------------------------
#pragma once
#include <matrix/math.hpp>
namespace sym {
/**
* This function was autogenerated from a symbolic function. Do not modify by hand.
*
* Symbolic function: compute_yaw_321_innov_var_and_h
*
* Args:
* state: Matrix24_1
* P: Matrix23_23
* R: Scalar
* epsilon: Scalar
*
* Outputs:
* innov_var: Scalar
* H: Matrix23_1
*/
template <typename Scalar>
void ComputeYaw321InnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
const Scalar epsilon, Scalar* const innov_var = nullptr,
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
// Total ops: 53
// Input arrays
// Intermediate terms (15)
const Scalar _tmp0 = 2 * state(0, 0);
const Scalar _tmp1 = _tmp0 * state(3, 0);
const Scalar _tmp2 = 2 * state(1, 0);
const Scalar _tmp3 = _tmp2 * state(2, 0);
const Scalar _tmp4 = _tmp1 + _tmp3;
const Scalar _tmp5 = std::pow(state(3, 0), Scalar(2));
const Scalar _tmp6 = std::pow(state(2, 0), Scalar(2));
const Scalar _tmp7 = -2 * _tmp5 - 2 * _tmp6 + 1;
const Scalar _tmp8 = _tmp7 + epsilon * ((((_tmp7) > 0) - ((_tmp7) < 0)) + Scalar(0.5));
const Scalar _tmp9 = std::pow(_tmp8, Scalar(2));
const Scalar _tmp10 = _tmp4 / _tmp9;
const Scalar _tmp11 = Scalar(1.0) / (_tmp8);
const Scalar _tmp12 = _tmp9 / (std::pow(_tmp4, Scalar(2)) + _tmp9);
const Scalar _tmp13 = _tmp12 * (-_tmp10 * (-_tmp1 + _tmp3) +
_tmp11 * (-_tmp5 + _tmp6 + std::pow(state(0, 0), Scalar(2)) -
std::pow(state(1, 0), Scalar(2))));
const Scalar _tmp14 = _tmp12 * (-_tmp10 * (-_tmp0 * state(2, 0) - _tmp2 * state(3, 0)) +
_tmp11 * (_tmp2 * state(0, 0) - 2 * state(2, 0) * state(3, 0)));
// Output terms (2)
if (innov_var != nullptr) {
Scalar& _innov_var = (*innov_var);
_innov_var = R + _tmp13 * (P(1, 2) * _tmp14 + P(2, 2) * _tmp13) +
_tmp14 * (P(1, 1) * _tmp14 + P(2, 1) * _tmp13);
}
if (H != nullptr) {
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
_h.setZero();
_h(1, 0) = _tmp14;
_h(2, 0) = _tmp13;
}
} // NOLINT(readability/fn_size)
// NOLINTNEXTLINE(readability/fn_size)
} // namespace sym

76
src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h_alternate.h
View File

@ -1,76 +0,0 @@
// -----------------------------------------------------------------------------
// This file was autogenerated by symforce from template:
// function/FUNCTION.h.jinja
// Do NOT modify by hand.
// -----------------------------------------------------------------------------
#pragma once
#include <matrix/math.hpp>
namespace sym {
/**
* This function was autogenerated from a symbolic function. Do not modify by hand.
*
* Symbolic function: compute_yaw_321_innov_var_and_h_alternate
*
* Args:
* state: Matrix24_1
* P: Matrix23_23
* R: Scalar
* epsilon: Scalar
*
* Outputs:
* innov_var: Scalar
* H: Matrix23_1
*/
template <typename Scalar>
void ComputeYaw321InnovVarAndHAlternate(const matrix::Matrix<Scalar, 24, 1>& state,
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
const Scalar epsilon, Scalar* const innov_var = nullptr,
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
// Total ops: 57
// Input arrays
// Intermediate terms (15)
const Scalar _tmp0 = 2 * state(2, 0);
const Scalar _tmp1 = 2 * state(1, 0);
const Scalar _tmp2 = 2 * state(0, 0) * state(3, 0);
const Scalar _tmp3 = _tmp1 * state(2, 0);
const Scalar _tmp4 = _tmp2 + _tmp3;
const Scalar _tmp5 = _tmp4 + epsilon * ((((_tmp4) > 0) - ((_tmp4) < 0)) + Scalar(0.5));
const Scalar _tmp6 = Scalar(1.0) / (_tmp5);
const Scalar _tmp7 = std::pow(state(3, 0), Scalar(2));
const Scalar _tmp8 = std::pow(state(2, 0), Scalar(2));
const Scalar _tmp9 = -2 * _tmp7 - 2 * _tmp8 + 1;
const Scalar _tmp10 = std::pow(_tmp5, Scalar(2));
const Scalar _tmp11 = _tmp9 / _tmp10;
const Scalar _tmp12 = _tmp10 / (_tmp10 + std::pow(_tmp9, Scalar(2)));
const Scalar _tmp13 = _tmp12 * (-_tmp11 * (-_tmp0 * state(3, 0) + _tmp1 * state(0, 0)) +
_tmp6 * (-_tmp0 * state(0, 0) - _tmp1 * state(3, 0)));
const Scalar _tmp14 = _tmp12 * (-_tmp11 * (-_tmp7 + _tmp8 + std::pow(state(0, 0), Scalar(2)) -
std::pow(state(1, 0), Scalar(2))) +
_tmp6 * (-_tmp2 + _tmp3));
// Output terms (2)
if (innov_var != nullptr) {
Scalar& _innov_var = (*innov_var);
_innov_var = R - _tmp13 * (-P(1, 1) * _tmp13 - P(2, 1) * _tmp14) -
_tmp14 * (-P(1, 2) * _tmp13 - P(2, 2) * _tmp14);
}
if (H != nullptr) {
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
_h.setZero();
_h(1, 0) = -_tmp13;
_h(2, 0) = -_tmp14;
}
} // NOLINT(readability/fn_size)
// NOLINTNEXTLINE(readability/fn_size)
} // namespace sym

2
src/modules/ekf2/EKF/yaw_fusion.cpp
View File

@ -33,8 +33,6 @@
#include "ekf.h"
#include <ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h>
#include <ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h>
#include <ekf_derivation/generated/compute_yaw_innov_var_and_h.h>
#include <mathlib/mathlib.h>

60
src/modules/ekf2/test/test_EKF_yaw_fusion_generated.cpp
View File

@ -35,68 +35,10 @@
#include "EKF/ekf.h"
#include "test_helper/comparison_helper.h"
#include "../EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h"
#include "../EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h_alternate.h"
#include "../EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h"
#include "../EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h_alternate.h"
#include "../EKF/python/ekf_derivation/generated/compute_yaw_innov_var_and_h.h"
using namespace matrix;
TEST(YawFusionGenerated, yawSingularity)
{
// GIVEN: an attitude that should give a singularity when transforming the
// rotation matrix to Euler yaw
StateSample state{};
state.quat_nominal = Eulerf(M_PI_F, 0.f, M_PI_F);
const float R = sq(radians(10.f));
SquareMatrixState P = createRandomCovarianceMatrix();
VectorState H_a;
VectorState H_b;
float innov_var_a;
float innov_var_b;
// WHEN: computing the innovation variance and H using two different methods
sym::ComputeYaw321InnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var_a, &H_a);
sym::ComputeYawInnovVarAndH(state.vector(), P, R, &innov_var_b, &H_b);
// THEN: Even at the singularity point, the result is still correct
EXPECT_TRUE(isEqual(H_a, H_b));
EXPECT_NEAR(innov_var_a, innov_var_b, 1e-5f);
EXPECT_TRUE(innov_var_a < 50.f && innov_var_a > R) << "innov_var = " << innov_var_a;
}
TEST(YawFusionGenerated, gimbalLock321vs312vsTangent)
{
// GIVEN: an attitude at gimbal lock position
StateSample state{};
state.quat_nominal = Eulerf(0.f, -M_PI_F / 2.f, M_PI_F);
const float R = sq(radians(10.f));
SquareMatrixState P = createRandomCovarianceMatrix();
VectorState H_321;
VectorState H_312;
VectorState H_tangent;
float innov_var_321;
float innov_var_312;
float innov_var_tangent;
sym::ComputeYaw321InnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var_321, &H_321);
sym::ComputeYaw312InnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var_312, &H_312);
sym::ComputeYawInnovVarAndH(state.vector(), P, R, &innov_var_tangent, &H_tangent);
// THEN: both computation are not equivalent, 321 is undefined but 312 and "tangent" are valid
EXPECT_FALSE(isEqual(H_321, H_312));
EXPECT_TRUE(isEqual(H_312, H_tangent));
EXPECT_GT(fabsf(innov_var_321 - innov_var_312), 1e6f);
EXPECT_NEAR(innov_var_312, innov_var_tangent, 1e-6f);
EXPECT_TRUE(innov_var_312 < 50.f && innov_var_312 > R) << "innov_var = " << innov_var_312;
}
Vector3f getRotVarNed(const Quatf &q, const SquareMatrixState &P)
{
constexpr auto S = State::quat_nominal;
@ -138,6 +80,8 @@ TEST(YawFusionGenerated, positiveVarianceAllOrientations)
<< " roll = " << degrees(roll)
<< " innov_var = " << innov_var
<< " innov_var_true = " << innov_var_true;
EXPECT_TRUE(H.isAllFinite());
}
}
}