Skip to content

Commit

Permalink
ekf2: remove old yaw 321 and 312 derivations
Browse files Browse the repository at this point in the history
bresch authored and dagar committed Mar 14, 2024
1 parent ee63f3e commit b2f1122
Showing 7 changed files with 2 additions and 436 deletions.
72 changes: 0 additions & 72 deletions src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
Original file line number Diff line number Diff line change
@@ -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 @@ def compute_gravity_z_innov_var_and_h(
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"])

This file was deleted.

This file was deleted.

This file was deleted.

This file was deleted.

2 changes: 0 additions & 2 deletions src/modules/ekf2/EKF/yaw_fusion.cpp
Original file line number Diff line number Diff line change
@@ -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 changes: 2 additions & 58 deletions src/modules/ekf2/test/test_EKF_yaw_fusion_generated.cpp
Original file line number Diff line number Diff line change
@@ -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());
}
}
}

0 comments on commit b2f1122

Please sign in to comment.