mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-15 07:03:34 +08:00
opt_flow: compare auto-generated Jacobian against autodiff
This commit is contained in:
bresch
@@ -39,220 +39,43 @@
|
||||
#include "../EKF/python/ekf_derivation/generated/compute_flow_y_innov_var_and_h.h"
|
||||
|
||||
using namespace matrix;
|
||||
using D = matrix::Dual<float, 7>;
|
||||
|
||||
void sympyFlowInnovVarHAndK(float q0, float q1, float q2, float q3, float vn, float ve, float vd, float range,
|
||||
const SquareMatrix24f &P, float R_LOS, Vector2f &innov_var,
|
||||
Vector24f &Kfusion_x, Vector24f &Kfusion_y, Vector24f &H_x, Vector24f &H_y)
|
||||
void computeHDual(const Vector24f &state_vector, float range, int axis, Vector24f &H)
|
||||
{
|
||||
const Dcmf Tbs;
|
||||
matrix::Quaternion<D> q(D(state_vector(0), 0),
|
||||
D(state_vector(1), 1),
|
||||
D(state_vector(2), 2),
|
||||
D(state_vector(3), 3));
|
||||
|
||||
const float HK0 = -Tbs(1, 0) * q2 + Tbs(1, 1) * q1 + Tbs(1, 2) * q0;
|
||||
const float HK1 = Tbs(1, 0) * q3 + Tbs(1, 1) * q0 - Tbs(1, 2) * q1;
|
||||
const float HK2 = Tbs(1, 0) * q0 - Tbs(1, 1) * q3 + Tbs(1, 2) * q2;
|
||||
const float HK3 = HK0 * vd + HK1 * ve + HK2 * vn;
|
||||
const float HK4 = 1.0F / range;
|
||||
const float HK5 = 2 * HK4;
|
||||
const float HK6 = Tbs(1, 0) * q1 + Tbs(1, 1) * q2 + Tbs(1, 2) * q3;
|
||||
const float HK7 = -HK0 * ve + HK1 * vd + HK6 * vn;
|
||||
const float HK8 = HK0 * vn - HK2 * vd + HK6 * ve;
|
||||
const float HK9 = -HK1 * vn + HK2 * ve + HK6 * vd;
|
||||
const float HK10 = q0 * q2;
|
||||
const float HK11 = q1 * q3;
|
||||
const float HK12 = HK10 + HK11;
|
||||
const float HK13 = 2 * Tbs(1, 2);
|
||||
const float HK14 = q0 * q3;
|
||||
const float HK15 = q1 * q2;
|
||||
const float HK16 = HK14 - HK15;
|
||||
const float HK17 = 2 * Tbs(1, 1);
|
||||
const float HK18 = ecl::powf(q1, 2);
|
||||
const float HK19 = ecl::powf(q2, 2);
|
||||
const float HK20 = -HK19;
|
||||
const float HK21 = ecl::powf(q0, 2);
|
||||
const float HK22 = ecl::powf(q3, 2);
|
||||
const float HK23 = HK21 - HK22;
|
||||
const float HK24 = HK18 + HK20 + HK23;
|
||||
const float HK25 = HK12 * HK13 - HK16 * HK17 + HK24 * Tbs(1, 0);
|
||||
const float HK26 = HK14 + HK15;
|
||||
const float HK27 = 2 * Tbs(1, 0);
|
||||
const float HK28 = q0 * q1;
|
||||
const float HK29 = q2 * q3;
|
||||
const float HK30 = HK28 - HK29;
|
||||
const float HK31 = -HK18;
|
||||
const float HK32 = HK19 + HK23 + HK31;
|
||||
const float HK33 = -HK13 * HK30 + HK26 * HK27 + HK32 * Tbs(1, 1);
|
||||
const float HK34 = HK28 + HK29;
|
||||
const float HK35 = HK10 - HK11;
|
||||
const float HK36 = HK20 + HK21 + HK22 + HK31;
|
||||
const float HK37 = HK17 * HK34 - HK27 * HK35 + HK36 * Tbs(1, 2);
|
||||
const float HK38 = 2 * HK3;
|
||||
const float HK39 = 2 * HK7;
|
||||
const float HK40 = 2 * HK8;
|
||||
const float HK41 = 2 * HK9;
|
||||
const float HK42 = HK25 * P(0, 4) + HK33 * P(0, 5) + HK37 * P(0, 6) + HK38 * P(0, 0) + HK39 * P(0, 1) + HK40 * P(0,
|
||||
2) + HK41 * P(0, 3);
|
||||
const float HK43 = ecl::powf(range, -2);
|
||||
const float HK44 = HK25 * P(4, 6) + HK33 * P(5, 6) + HK37 * P(6, 6) + HK38 * P(0, 6) + HK39 * P(1, 6) + HK40 * P(2,
|
||||
6) + HK41 * P(3, 6);
|
||||
const float HK45 = HK25 * P(4, 5) + HK33 * P(5, 5) + HK37 * P(5, 6) + HK38 * P(0, 5) + HK39 * P(1, 5) + HK40 * P(2,
|
||||
5) + HK41 * P(3, 5);
|
||||
const float HK46 = HK25 * P(4, 4) + HK33 * P(4, 5) + HK37 * P(4, 6) + HK38 * P(0, 4) + HK39 * P(1, 4) + HK40 * P(2,
|
||||
4) + HK41 * P(3, 4);
|
||||
const float HK47 = HK25 * P(2, 4) + HK33 * P(2, 5) + HK37 * P(2, 6) + HK38 * P(0, 2) + HK39 * P(1, 2) + HK40 * P(2,
|
||||
2) + HK41 * P(2, 3);
|
||||
const float HK48 = HK25 * P(3, 4) + HK33 * P(3, 5) + HK37 * P(3, 6) + HK38 * P(0, 3) + HK39 * P(1, 3) + HK40 * P(2,
|
||||
3) + HK41 * P(3, 3);
|
||||
const float HK49 = HK25 * P(1, 4) + HK33 * P(1, 5) + HK37 * P(1, 6) + HK38 * P(0, 1) + HK39 * P(1, 1) + HK40 * P(1,
|
||||
2) + HK41 * P(1, 3);
|
||||
// calculate the velocity of the sensor in the earth frame
|
||||
Vector3<D> vel_earth(D(state_vector(4), 4), D(state_vector(5), 5), D(state_vector(6), 6));
|
||||
|
||||
const float HK51 = Tbs(0, 1) * q1;
|
||||
const float HK52 = Tbs(0, 2) * q0;
|
||||
const float HK53 = Tbs(0, 0) * q2;
|
||||
const float HK54 = HK51 + HK52 - HK53;
|
||||
const float HK55 = Tbs(0, 0) * q3;
|
||||
const float HK56 = Tbs(0, 1) * q0;
|
||||
const float HK57 = Tbs(0, 2) * q1;
|
||||
const float HK58 = HK55 + HK56 - HK57;
|
||||
const float HK59 = Tbs(0, 0) * q0;
|
||||
const float HK60 = Tbs(0, 2) * q2;
|
||||
const float HK61 = Tbs(0, 1) * q3;
|
||||
const float HK62 = HK59 + HK60 - HK61;
|
||||
const float HK63 = HK54 * vd + HK58 * ve + HK62 * vn;
|
||||
const float HK64 = Tbs(0, 0) * q1 + Tbs(0, 1) * q2 + Tbs(0, 2) * q3;
|
||||
const float HK65 = HK58 * vd + HK64 * vn;
|
||||
const float HK66 = -HK54 * ve + HK65;
|
||||
const float HK67 = HK54 * vn + HK64 * ve;
|
||||
const float HK68 = -HK62 * vd + HK67;
|
||||
const float HK69 = HK62 * ve + HK64 * vd;
|
||||
const float HK70 = -HK58 * vn + HK69;
|
||||
const float HK71 = 2 * Tbs(0, 1);
|
||||
const float HK72 = 2 * Tbs(0, 2);
|
||||
const float HK73 = HK12 * HK72 + HK24 * Tbs(0, 0);
|
||||
const float HK74 = -HK16 * HK71 + HK73;
|
||||
const float HK75 = 2 * Tbs(0, 0);
|
||||
const float HK76 = HK26 * HK75 + HK32 * Tbs(0, 1);
|
||||
const float HK77 = -HK30 * HK72 + HK76;
|
||||
const float HK78 = HK34 * HK71 + HK36 * Tbs(0, 2);
|
||||
const float HK79 = -HK35 * HK75 + HK78;
|
||||
const float HK80 = 2 * HK63;
|
||||
const float HK81 = 2 * HK65 + 2 * ve * (-HK51 - HK52 + HK53);
|
||||
const float HK82 = 2 * HK67 + 2 * vd * (-HK59 - HK60 + HK61);
|
||||
const float HK83 = 2 * HK69 + 2 * vn * (-HK55 - HK56 + HK57);
|
||||
const float HK84 = HK71 * (-HK14 + HK15) + HK73;
|
||||
const float HK85 = HK72 * (-HK28 + HK29) + HK76;
|
||||
const float HK86 = HK75 * (-HK10 + HK11) + HK78;
|
||||
const float HK87 = HK80 * P(0, 0) + HK81 * P(0, 1) + HK82 * P(0, 2) + HK83 * P(0, 3) + HK84 * P(0, 4) + HK85 * P(0,
|
||||
5) + HK86 * P(0, 6);
|
||||
const float HK88 = HK80 * P(0, 6) + HK81 * P(1, 6) + HK82 * P(2, 6) + HK83 * P(3, 6) + HK84 * P(4, 6) + HK85 * P(5,
|
||||
6) + HK86 * P(6, 6);
|
||||
const float HK89 = HK80 * P(0, 5) + HK81 * P(1, 5) + HK82 * P(2, 5) + HK83 * P(3, 5) + HK84 * P(4, 5) + HK85 * P(5,
|
||||
5) + HK86 * P(5, 6);
|
||||
const float HK90 = HK80 * P(0, 4) + HK81 * P(1, 4) + HK82 * P(2, 4) + HK83 * P(3, 4) + HK84 * P(4, 4) + HK85 * P(4,
|
||||
5) + HK86 * P(4, 6);
|
||||
const float HK91 = HK80 * P(0, 2) + HK81 * P(1, 2) + HK82 * P(2, 2) + HK83 * P(2, 3) + HK84 * P(2, 4) + HK85 * P(2,
|
||||
5) + HK86 * P(2, 6);
|
||||
const float HK92 = 2 * HK43;
|
||||
const float HK93 = HK80 * P(0, 3) + HK81 * P(1, 3) + HK82 * P(2, 3) + HK83 * P(3, 3) + HK84 * P(3, 4) + HK85 * P(3,
|
||||
5) + HK86 * P(3, 6);
|
||||
const float HK94 = HK80 * P(0, 1) + HK81 * P(1, 1) + HK82 * P(1, 2) + HK83 * P(1, 3) + HK84 * P(1, 4) + HK85 * P(1,
|
||||
5) + HK86 * P(1, 6);
|
||||
// rotate into body frame
|
||||
Vector3<D> vel_body = Dcm<D>(q).transpose() * vel_earth;
|
||||
matrix::Vector2<D> predicted_flow;
|
||||
predicted_flow(0) = vel_body(1) / range;
|
||||
predicted_flow(1) = -vel_body(0) / range;
|
||||
|
||||
// X-axis
|
||||
innov_var(0) = (HK25 * HK43 * HK46 + HK33 * HK43 * HK45 + HK37 * HK43 * HK44 + HK38 * HK42 * HK43 + HK39 * HK43 * HK49 +
|
||||
HK40 * HK43 * HK47 + HK41 * HK43 * HK48 + R_LOS);
|
||||
H.setZero();
|
||||
|
||||
{
|
||||
const float HK50 = HK4 / innov_var(0);
|
||||
|
||||
// Observation Jacobians - axis 0
|
||||
SparseVector24f<0, 1, 2, 3, 4, 5, 6> Hfusion;
|
||||
Hfusion.at<0>() = HK3 * HK5;
|
||||
Hfusion.at<1>() = HK5 * HK7;
|
||||
Hfusion.at<2>() = HK5 * HK8;
|
||||
Hfusion.at<3>() = HK5 * HK9;
|
||||
Hfusion.at<4>() = HK25 * HK4;
|
||||
Hfusion.at<5>() = HK33 * HK4;
|
||||
Hfusion.at<6>() = HK37 * HK4;
|
||||
|
||||
// Kalman gains - axis 0
|
||||
Vector24f Kfusion;
|
||||
Kfusion(0) = HK42 * HK50;
|
||||
Kfusion(1) = HK49 * HK50;
|
||||
Kfusion(2) = HK47 * HK50;
|
||||
Kfusion(3) = HK48 * HK50;
|
||||
Kfusion(4) = HK46 * HK50;
|
||||
Kfusion(5) = HK45 * HK50;
|
||||
Kfusion(6) = HK44 * HK50;
|
||||
|
||||
for (unsigned row = 7; row <= 23; row++) {
|
||||
Kfusion(row) = HK50 * (HK25 * P(4, row) + HK33 * P(5, row) + HK37 * P(6, row) + HK38 * P(0, row) + HK39 * P(1,
|
||||
row) + HK40 * P(2, row) + HK41 * P(3, row));
|
||||
}
|
||||
|
||||
// copy to arrays used for comparison
|
||||
for (int row = 0; row < 7; row++) {
|
||||
H_x(row) = Hfusion.atCompressedIndex(row);
|
||||
}
|
||||
|
||||
for (int row = 0; row < 24; row++) {
|
||||
Kfusion_x(row) = Kfusion(row);
|
||||
}
|
||||
}
|
||||
|
||||
// Y-axis
|
||||
innov_var(1) = (HK43 * HK74 * HK90 + HK43 * HK77 * HK89 + HK43 * HK79 * HK88 + HK43 * HK80 * HK87 + HK66 * HK92 * HK94 +
|
||||
HK68 * HK91 * HK92 + HK70 * HK92 * HK93 + R_LOS);
|
||||
|
||||
{
|
||||
const float HK95 = HK4 / innov_var(1);
|
||||
|
||||
// Observation Jacobians - axis 1
|
||||
SparseVector24f<0, 1, 2, 3, 4, 5, 6> Hfusion;
|
||||
Hfusion.at<0>() = -HK5 * HK63;
|
||||
Hfusion.at<1>() = -HK5 * HK66;
|
||||
Hfusion.at<2>() = -HK5 * HK68;
|
||||
Hfusion.at<3>() = -HK5 * HK70;
|
||||
Hfusion.at<4>() = -HK4 * HK74;
|
||||
Hfusion.at<5>() = -HK4 * HK77;
|
||||
Hfusion.at<6>() = -HK4 * HK79;
|
||||
|
||||
// Kalman gains - axis 1
|
||||
Vector24f Kfusion;
|
||||
Kfusion(0) = -HK87 * HK95;
|
||||
Kfusion(1) = -HK94 * HK95;
|
||||
Kfusion(2) = -HK91 * HK95;
|
||||
Kfusion(3) = -HK93 * HK95;
|
||||
Kfusion(4) = -HK90 * HK95;
|
||||
Kfusion(5) = -HK89 * HK95;
|
||||
Kfusion(6) = -HK88 * HK95;
|
||||
|
||||
for (unsigned row = 7; row <= 23; row++) {
|
||||
Kfusion(row) = -HK95 * (HK80 * P(0, row) + HK81 * P(1, row) + HK82 * P(2, row) + HK83 * P(3, row) + HK84 * P(4,
|
||||
row) + HK85 * P(5, row) + HK86 * P(6, row));
|
||||
}
|
||||
|
||||
// copy to arrays used for comparison
|
||||
for (int row = 0; row < 7; row++) {
|
||||
H_y(row) = Hfusion.atCompressedIndex(row);
|
||||
}
|
||||
|
||||
for (int row = 0; row < 24; row++) {
|
||||
Kfusion_y(row) = Kfusion(row);
|
||||
}
|
||||
for (int i = 0; i <= 6; i++) {
|
||||
H(i) = predicted_flow(axis).derivative(i);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(OptFlowFusionGenerated, SympyVsSymforce)
|
||||
TEST(OptFlowFusionGenerated, symforceVsDual)
|
||||
{
|
||||
// Compare calculation of observation Jacobians and Kalman gains for sympy and symforce generated equations
|
||||
const Quatf q(Eulerf(-M_PI_F / 2.f, M_PI_F / 3.f, M_PI_F * 4.f / 5.f));
|
||||
// Compare calculation of observation Jacobians
|
||||
const Quatf q(Eulerf(M_PI_F / 4.f, -M_PI_F / 6.f, M_PI_F));
|
||||
const float q0 = q(0);
|
||||
const float q1 = q(1);
|
||||
const float q2 = q(2);
|
||||
const float q3 = q(3);
|
||||
|
||||
const float vn = 10.0f * 2.0f * ((float)randf() - 0.5f);
|
||||
const float ve = 10.0f * 2.0f * ((float)randf() - 0.5f);
|
||||
const float vd = 2.0f * ((float)randf() - 0.5f);
|
||||
const float vn = 5.f;
|
||||
const float ve = 0.f;
|
||||
const float vd = -1.5f;
|
||||
|
||||
const float range = 5.0f;
|
||||
|
||||
@@ -267,46 +90,26 @@ TEST(OptFlowFusionGenerated, SympyVsSymforce)
|
||||
|
||||
const float R_LOS = sq(0.15f);
|
||||
|
||||
SquareMatrix24f P = createRandomCovarianceMatrix24f();
|
||||
SquareMatrix24f P;
|
||||
|
||||
Vector24f Hfusion_sympy_x;
|
||||
Vector24f Hfusion_sympy_y;
|
||||
Vector24f Kfusion_sympy_x;
|
||||
Vector24f Kfusion_sympy_y;
|
||||
Vector2f innov_var_sympy;
|
||||
|
||||
Vector24f Hfusion_symforce;
|
||||
Vector24f Kfusion_symforce;
|
||||
Vector2f innov_var_symforce;
|
||||
|
||||
sympyFlowInnovVarHAndK(q0, q1, q2, q3, vn, ve, vd, range, P, R_LOS, innov_var_sympy,
|
||||
Kfusion_sympy_x, Kfusion_sympy_y, Hfusion_sympy_x, Hfusion_sympy_y);
|
||||
Vector24f H;
|
||||
Vector24f H_dual;
|
||||
Vector2f innov_var;
|
||||
|
||||
for (int i = 0; i < 2; i++) {
|
||||
Vector24f &Hfusion_sympy = Hfusion_sympy_x;
|
||||
Vector24f &Kfusion_sympy = Kfusion_sympy_x;
|
||||
computeHDual(state_vector, range, i, H_dual);
|
||||
|
||||
if (i == 0) {
|
||||
sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var_symforce, &Hfusion_symforce);
|
||||
sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H);
|
||||
|
||||
} else {
|
||||
Hfusion_sympy = Hfusion_sympy_y;
|
||||
Kfusion_sympy = Kfusion_sympy_y;
|
||||
sym::ComputeFlowYInnovVarAndH(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var_symforce(1), &Hfusion_symforce);
|
||||
sym::ComputeFlowYInnovVarAndH(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var(1), &H);
|
||||
}
|
||||
|
||||
// K isn't generated from symbolic anymore to save flash space
|
||||
Kfusion_symforce = P * Hfusion_symforce / innov_var_symforce(i);
|
||||
// Both derivations are equivalent
|
||||
EXPECT_EQ(H, H_dual);
|
||||
|
||||
DiffRatioReport report = computeDiffRatioVector24f(Hfusion_sympy, Hfusion_symforce);
|
||||
EXPECT_LT(report.max_diff_fraction, 1e-5f) << "i = " << i << "Hfusion max diff fraction = " <<
|
||||
report.max_diff_fraction <<
|
||||
" location index = " << report.max_row << " sympy = " << report.max_v1 << " symforce = " << report.max_v2;
|
||||
|
||||
report = computeDiffRatioVector24f(Kfusion_sympy, Kfusion_symforce);
|
||||
EXPECT_LT(report.max_diff_fraction, 1e-5f) << "i = " << i << "Kfusion max diff fraction = " <<
|
||||
report.max_diff_fraction <<
|
||||
" location index = " << report.max_row << " sympy = " << report.max_v1 << " symforce = " << report.max_v2;
|
||||
EXPECT_NEAR(innov_var_sympy(i), innov_var_symforce(i), 1e-5f) << "i = " << i;
|
||||
// Since the state variance is 0, the observation variance is the innovation variance
|
||||
EXPECT_FLOAT_EQ(innov_var(i), R_LOS);
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user