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

ekf2: new yaw derivation

Browse Source 
Instead of euler angles, compute measurement jacobian using a small
global perturbation around the vertical axis
This commit is contained in:
bresch
2024-03-14 11:33:45 -04:00
committed by Daniel Agar
parent 6373d8d243
commit e3f67d5c1a
4 changed files with 123 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/test/test_EKF_yaw_fusion_generated.cpp
+36 -17
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;
}
}
}