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

ekf2: new yaw derivation 

Instead of euler angles, compute measurement jacobian using a small
global perturbation around the vertical axis
This commit is contained in:
bresch 2024-03-12 17:26:51 +01:00 committed by Daniel Agar
parent 6373d8d243
commit e3f67d5c1a
4 changed files with 123 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -390,6 +390,24 @@ def compute_mag_z_innov_var_and_h(
return (innov_var, H.T)
def compute_yaw_innov_var_and_h(
state: VState,
P: MTangent,
R: sf.Scalar
) -> (sf.Scalar, VTangent):
state = vstate_to_state(state)
q = sf.Quaternion.from_storage(state["quat_nominal"].to_storage())
r = sf.Quaternion.symbolic('r')
delta_q = q * r.conj() # create a quaternion error of the measurement at the origin
delta_meas_pred = 2 * delta_q.z # Use small angle approximation to obtain a simpler jacobian
H = sf.V1(delta_meas_pred).jacobian(state)
H = H.subs({r.w: q.w, r.x: q.x, r.y: q.y, r.z: q.z}) # assume innovation is small
innov_var = (H * P * H.T + R)[0,0]
return (innov_var, H.T)
def compute_yaw_321_innov_var_and_h(
state: VState,
P: MTangent,
@ -678,6 +696,7 @@ if not args.disable_wind:
generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"])
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"])

65
src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_innov_var_and_h.h Normal file
View File

@ -0,0 +1,65 @@
// -----------------------------------------------------------------------------
// 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_innov_var_and_h
*
* Args:
* state: Matrix24_1
* P: Matrix23_23
* R: Scalar
*
* Outputs:
* innov_var: Scalar
* H: Matrix23_1
*/
template <typename Scalar>
void ComputeYawInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
Scalar* const innov_var = nullptr,
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
// Total ops: 36
// Input arrays
// Intermediate terms (5)
const Scalar _tmp0 = 2 * state(2, 0);
const Scalar _tmp1 = 2 * state(1, 0);
const Scalar _tmp2 = -_tmp0 * state(0, 0) + _tmp1 * state(3, 0);
const Scalar _tmp3 = _tmp0 * state(3, 0) + _tmp1 * state(0, 0);
const Scalar _tmp4 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(1, 0), Scalar(2)) -
std::pow(state(2, 0), Scalar(2)) + std::pow(state(3, 0), Scalar(2));
// Output terms (2)
if (innov_var != nullptr) {
Scalar& _innov_var = (*innov_var);
_innov_var = R + _tmp2 * (P(0, 0) * _tmp2 + P(1, 0) * _tmp3 + P(2, 0) * _tmp4) +
_tmp3 * (P(0, 1) * _tmp2 + P(1, 1) * _tmp3 + P(2, 1) * _tmp4) +
_tmp4 * (P(0, 2) * _tmp2 + P(1, 2) * _tmp3 + P(2, 2) * _tmp4);
}
if (H != nullptr) {
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
_h.setZero();
_h(0, 0) = _tmp2;
_h(1, 0) = _tmp3;
_h(2, 0) = _tmp4;
}
} // NOLINT(readability/fn_size)
// NOLINTNEXTLINE(readability/fn_size)
} // namespace sym

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

@ -35,6 +35,7 @@
#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>
@ -42,7 +43,7 @@
bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status)
{
VectorState H_YAW;
computeYawInnovVarAndH(aid_src_status.observation_variance, aid_src_status.innovation_variance, H_YAW);
sym::ComputeYawInnovVarAndH(_state.vector(), P, aid_src_status.observation_variance, &aid_src_status.innovation_variance, &H_YAW);
return fuseYaw(aid_src_status, H_YAW);
}
@ -135,10 +136,5 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H
void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const
{
if (shouldUse321RotationSequence(_R_to_earth)) {
sym::ComputeYaw321InnovVarAndH(_state.vector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW);
} else {
sym::ComputeYaw312InnovVarAndH(_state.vector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW);
}
sym::ComputeYawInnovVarAndH(_state.vector(), P, variance, &innovation_variance, &H_YAW);
}

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

@ -39,10 +39,11 @@
#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, singularityYawEquivalence)
TEST(YawFusionGenerated, yawSingularity)
{
// GIVEN: an attitude that should give a singularity when transforming the
// rotation matrix to Euler yaw
@ -57,18 +58,18 @@ TEST(YawFusionGenerated, singularityYawEquivalence)
float innov_var_a;
float innov_var_b;
// WHEN: computing the innovation variance and H using two different
// alternate forms (one is singular at pi/2 and the other one at 0)
// 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::ComputeYaw321InnovVarAndHAlternate(state.vector(), P, R, FLT_EPSILON, &innov_var_b, &H_b);
sym::ComputeYawInnovVarAndH(state.vector(), P, R, &innov_var_b, &H_b);
// THEN: Even at the singularity point, the result is still correct, thanks to epsilon
// 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, gimbalLock321vs312)
TEST(YawFusionGenerated, gimbalLock321vs312vsTangent)
{
// GIVEN: an attitude at gimbal lock position
StateSample state{};
@ -79,18 +80,31 @@ TEST(YawFusionGenerated, gimbalLock321vs312)
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 is valid
// 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;
matrix::SquareMatrix3f rot_cov_body = P.slice<S.dof, S.dof>(S.idx, S.idx);
auto R_to_earth = Dcmf(q);
return matrix::SquareMatrix<float, State::quat_nominal.dof>(R_to_earth * rot_cov_body * R_to_earth.T()).diag();
}
TEST(YawFusionGenerated, positiveVarianceAllOrientations)
{
const float R = sq(radians(10.f));
@ -99,19 +113,14 @@ TEST(YawFusionGenerated, positiveVarianceAllOrientations)
VectorState H;
float innov_var;
// GIVEN: all orientations (90 deg steps)
for (float yaw = 0.f; yaw < 2.f * M_PI_F; yaw += M_PI_F / 2.f) {
for (float pitch = 0.f; pitch < 2.f * M_PI_F; pitch += M_PI_F / 2.f) {
for (float roll = 0.f; roll < 2.f * M_PI_F; roll += M_PI_F / 2.f) {
// GIVEN: all orientations
for (float yaw = 0.f; yaw < 2.f * M_PI_F; yaw += M_PI_F / 4.f) {
for (float pitch = 0.f; pitch < 2.f * M_PI_F; pitch += M_PI_F / 4.f) {
for (float roll = 0.f; roll < 2.f * M_PI_F; roll += M_PI_F / 4.f) {
StateSample state{};
state.quat_nominal = Eulerf(roll, pitch, yaw);
if (shouldUse321RotationSequence(Dcmf(state.quat_nominal))) {
sym::ComputeYaw321InnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var, &H);
} else {
sym::ComputeYaw312InnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var, &H);
}
sym::ComputeYawInnovVarAndH(state.vector(), P, R, &innov_var, &H);
// THEN: the innovation variance must be positive and finite
EXPECT_TRUE(innov_var < 100.f && innov_var > R)
@ -119,6 +128,16 @@ TEST(YawFusionGenerated, positiveVarianceAllOrientations)
<< " pitch = " << degrees(pitch)
<< " roll = " << degrees(roll)
<< " innov_var = " << innov_var;
// AND: it should be the same as the "true" innovation variance obtained by summing
// the Z rotation variance in NED and the measurement variance
const float innov_var_true = getRotVarNed(state.quat_nominal, P)(2) + R;
EXPECT_NEAR(innov_var, innov_var_true, 1e-5f)
<< "yaw = " << degrees(yaw)
<< " pitch = " << degrees(pitch)
<< " roll = " << degrees(roll)
<< " innov_var = " << innov_var
<< " innov_var_true = " << innov_var_true;
}
}
}