forked from Archive/PX4-Autopilot
mag_3d: compare auto-generated Jacobian against autodiff
This commit is contained in:
bresch
Daniel Agar
parent
c6f1a63659
commit
e7c4a22be8
|
|
@ -40,265 +40,29 @@
|
|||
#include "../EKF/python/ekf_derivation/generated/compute_mag_z_innov_var_and_h.h"
|
||||
|
||||
using namespace matrix;
|
||||
using D = matrix::Dual<float, 24>;
|
||||
|
||||
void sympyMagXInnovVarHxAndKx(float q0, float q1, float q2, float q3, float magN, float magE, float magD,
|
||||
const SquareMatrix24f &P, float R_MAG, float &innov_var, Vector24f &Kfusion, Vector24f &H)
|
||||
void computeHDual(const Vector24f &state_vector, int axis, Vector24f &H)
|
||||
{
|
||||
// common expressions used by sympy generated equations
|
||||
// calculate intermediate variables used for X axis innovation variance, observation Jacobians and Kalman gainss
|
||||
const float HKX0 = -magD * q2 + magE * q3 + magN * q0;
|
||||
const float HKX1 = magD * q3 + magE * q2 + magN * q1;
|
||||
const float HKX2 = magE * q1;
|
||||
const float HKX3 = magD * q0;
|
||||
const float HKX4 = magN * q2;
|
||||
const float HKX5 = magD * q1 + magE * q0 - magN * q3;
|
||||
const float HKX6 = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2);
|
||||
const float HKX7 = q0 * q3 + q1 * q2;
|
||||
const float HKX8 = q1 * q3;
|
||||
const float HKX9 = q0 * q2;
|
||||
const float HKX10 = 2 * HKX7;
|
||||
const float HKX11 = -2 * HKX8 + 2 * HKX9;
|
||||
const float HKX12 = 2 * HKX1;
|
||||
const float HKX13 = 2 * HKX0;
|
||||
const float HKX14 = -2 * HKX2 + 2 * HKX3 + 2 * HKX4;
|
||||
const float HKX15 = 2 * HKX5;
|
||||
const float HKX16 = HKX10 * P(0, 17) - HKX11 * P(0, 18) + HKX12 * P(0, 1) + HKX13 * P(0, 0) - HKX14 * P(0,
|
||||
2) + HKX15 * P(0, 3) + HKX6 * P(0, 16) + P(0, 19);
|
||||
const float HKX17 = HKX10 * P(16, 17) - HKX11 * P(16, 18) + HKX12 * P(1, 16) + HKX13 * P(0, 16) - HKX14 * P(2,
|
||||
16) + HKX15 * P(3, 16) + HKX6 * P(16, 16) + P(16, 19);
|
||||
const float HKX18 = HKX10 * P(17, 18) - HKX11 * P(18, 18) + HKX12 * P(1, 18) + HKX13 * P(0, 18) - HKX14 * P(2,
|
||||
18) + HKX15 * P(3, 18) + HKX6 * P(16, 18) + P(18, 19);
|
||||
const float HKX19 = HKX10 * P(2, 17) - HKX11 * P(2, 18) + HKX12 * P(1, 2) + HKX13 * P(0, 2) - HKX14 * P(2,
|
||||
2) + HKX15 * P(2, 3) + HKX6 * P(2, 16) + P(2, 19);
|
||||
const float HKX20 = HKX10 * P(17, 17) - HKX11 * P(17, 18) + HKX12 * P(1, 17) + HKX13 * P(0, 17) - HKX14 * P(2,
|
||||
17) + HKX15 * P(3, 17) + HKX6 * P(16, 17) + P(17, 19);
|
||||
const float HKX21 = HKX10 * P(3, 17) - HKX11 * P(3, 18) + HKX12 * P(1, 3) + HKX13 * P(0, 3) - HKX14 * P(2,
|
||||
3) + HKX15 * P(3, 3) + HKX6 * P(3, 16) + P(3, 19);
|
||||
const float HKX22 = HKX10 * P(1, 17) - HKX11 * P(1, 18) + HKX12 * P(1, 1) + HKX13 * P(0, 1) - HKX14 * P(1,
|
||||
2) + HKX15 * P(1, 3) + HKX6 * P(1, 16) + P(1, 19);
|
||||
const float HKX23 = HKX10 * P(17, 19) - HKX11 * P(18, 19) + HKX12 * P(1, 19) + HKX13 * P(0, 19) - HKX14 * P(2,
|
||||
19) + HKX15 * P(3, 19) + HKX6 * P(16, 19) + P(19, 19);
|
||||
const float HKX24 = 1.0F / (HKX10 * HKX20 - HKX11 * HKX18 + HKX12 * HKX22 + HKX13 * HKX16 - HKX14 * HKX19 + HKX15 *
|
||||
HKX21 + HKX17 * HKX6 + HKX23 + R_MAG);
|
||||
matrix::Quaternion<D> q(D(state_vector(0), 0),
|
||||
D(state_vector(1), 1),
|
||||
D(state_vector(2), 2),
|
||||
D(state_vector(3), 3));
|
||||
|
||||
innov_var = (HKX10 * HKX20 - HKX11 * HKX18 + HKX12 * HKX22 + HKX13 * HKX16 - HKX14 * HKX19 + HKX15 * HKX21 + HKX17 *
|
||||
HKX6 + HKX23 + R_MAG);
|
||||
Vector3<D> mag_field_earth(D(state_vector(16), 16), D(state_vector(17), 17), D(state_vector(18), 18));
|
||||
Vector3<D> mag_bias_body(D(state_vector(19), 19), D(state_vector(20), 20), D(state_vector(21), 21));
|
||||
|
||||
// Calculate X axis observation jacobians
|
||||
float Hfusion[24] = {};
|
||||
Hfusion[0] = 2 * HKX0;
|
||||
Hfusion[1] = 2 * HKX1;
|
||||
Hfusion[2] = 2 * HKX2 - 2 * HKX3 - 2 * HKX4;
|
||||
Hfusion[3] = 2 * HKX5;
|
||||
Hfusion[16] = HKX6;
|
||||
Hfusion[17] = 2 * HKX7;
|
||||
Hfusion[18] = 2 * HKX8 - 2 * HKX9;
|
||||
Hfusion[19] = 1;
|
||||
Vector3<D> mag_pred = Dcm<D>(q).transpose() * mag_field_earth + mag_bias_body;
|
||||
|
||||
// Calculate X axis Kalman gains
|
||||
if (true) {
|
||||
Kfusion(0) = HKX16 * HKX24;
|
||||
Kfusion(1) = HKX22 * HKX24;
|
||||
Kfusion(2) = HKX19 * HKX24;
|
||||
Kfusion(3) = HKX21 * HKX24;
|
||||
H.setZero();
|
||||
|
||||
for (unsigned row = 4; row <= 15; row++) {
|
||||
Kfusion(row) = HKX24 * (HKX10 * P(row, 17) - HKX11 * P(row, 18) + HKX12 * P(1, row) + HKX13 * P(0, row) - HKX14 * P(2,
|
||||
row) + HKX15 * P(3, row) + HKX6 * P(row, 16) + P(row, 19));
|
||||
}
|
||||
|
||||
for (unsigned row = 22; row <= 23; row++) {
|
||||
Kfusion(row) = HKX24 * (HKX10 * P(17, row) - HKX11 * P(18, row) + HKX12 * P(1, row) + HKX13 * P(0, row) - HKX14 * P(2,
|
||||
row) + HKX15 * P(3, row) + HKX6 * P(16, row) + P(19, row));
|
||||
}
|
||||
}
|
||||
|
||||
Kfusion(16) = HKX17 * HKX24;
|
||||
Kfusion(17) = HKX20 * HKX24;
|
||||
Kfusion(18) = HKX18 * HKX24;
|
||||
Kfusion(19) = HKX23 * HKX24;
|
||||
|
||||
for (unsigned row = 20; row <= 21; row++) {
|
||||
Kfusion(row) = HKX24 * (HKX10 * P(17, row) - HKX11 * P(18, row) + HKX12 * P(1, row) + HKX13 * P(0, row) - HKX14 * P(2,
|
||||
row) + HKX15 * P(3, row) + HKX6 * P(16, row) + P(19, row));
|
||||
}
|
||||
|
||||
for (int row = 0; row < 24; row++) {
|
||||
H(row) = Hfusion[row];
|
||||
for (int i = 0; i <= 23; i++) {
|
||||
H(i) = mag_pred(axis).derivative(i);
|
||||
}
|
||||
}
|
||||
|
||||
void sympyMagYInnovVarHyAndKy(float q0, float q1, float q2, float q3, float magN, float magE, float magD,
|
||||
const SquareMatrix24f &P, float R_MAG, float &innov_var, Vector24f &Kfusion, Vector24f &H)
|
||||
TEST(Mag3DFusionGenerated, symforceVsDual)
|
||||
{
|
||||
const float HKY0 = magD * q1 + magE * q0 - magN * q3;
|
||||
const float HKY1 = magD * q0 - magE * q1 + magN * q2;
|
||||
const float HKY2 = magD * q3 + magE * q2 + magN * q1;
|
||||
const float HKY3 = magD * q2;
|
||||
const float HKY4 = magE * q3;
|
||||
const float HKY5 = magN * q0;
|
||||
const float HKY6 = q1 * q2;
|
||||
const float HKY7 = q0 * q3;
|
||||
const float HKY8 = powf(q0, 2) - powf(q1, 2) + powf(q2, 2) - powf(q3, 2);
|
||||
const float HKY9 = q0 * q1 + q2 * q3;
|
||||
const float HKY10 = 2 * HKY9;
|
||||
const float HKY11 = -2 * HKY6 + 2 * HKY7;
|
||||
const float HKY12 = 2 * HKY2;
|
||||
const float HKY13 = 2 * HKY0;
|
||||
const float HKY14 = 2 * HKY1;
|
||||
const float HKY15 = -2 * HKY3 + 2 * HKY4 + 2 * HKY5;
|
||||
const float HKY16 = HKY10 * P(0, 18) - HKY11 * P(0, 16) + HKY12 * P(0, 2) + HKY13 * P(0, 0) + HKY14 * P(0,
|
||||
1) - HKY15 * P(0, 3) + HKY8 * P(0, 17) + P(0, 20);
|
||||
const float HKY17 = HKY10 * P(17, 18) - HKY11 * P(16, 17) + HKY12 * P(2, 17) + HKY13 * P(0, 17) + HKY14 * P(1,
|
||||
17) - HKY15 * P(3, 17) + HKY8 * P(17, 17) + P(17, 20);
|
||||
const float HKY18 = HKY10 * P(16, 18) - HKY11 * P(16, 16) + HKY12 * P(2, 16) + HKY13 * P(0, 16) + HKY14 * P(1,
|
||||
16) - HKY15 * P(3, 16) + HKY8 * P(16, 17) + P(16, 20);
|
||||
const float HKY19 = HKY10 * P(3, 18) - HKY11 * P(3, 16) + HKY12 * P(2, 3) + HKY13 * P(0, 3) + HKY14 * P(1,
|
||||
3) - HKY15 * P(3, 3) + HKY8 * P(3, 17) + P(3, 20);
|
||||
const float HKY20 = HKY10 * P(18, 18) - HKY11 * P(16, 18) + HKY12 * P(2, 18) + HKY13 * P(0, 18) + HKY14 * P(1,
|
||||
18) - HKY15 * P(3, 18) + HKY8 * P(17, 18) + P(18, 20);
|
||||
const float HKY21 = HKY10 * P(1, 18) - HKY11 * P(1, 16) + HKY12 * P(1, 2) + HKY13 * P(0, 1) + HKY14 * P(1,
|
||||
1) - HKY15 * P(1, 3) + HKY8 * P(1, 17) + P(1, 20);
|
||||
const float HKY22 = HKY10 * P(2, 18) - HKY11 * P(2, 16) + HKY12 * P(2, 2) + HKY13 * P(0, 2) + HKY14 * P(1,
|
||||
2) - HKY15 * P(2, 3) + HKY8 * P(2, 17) + P(2, 20);
|
||||
const float HKY23 = HKY10 * P(18, 20) - HKY11 * P(16, 20) + HKY12 * P(2, 20) + HKY13 * P(0, 20) + HKY14 * P(1,
|
||||
20) - HKY15 * P(3, 20) + HKY8 * P(17, 20) + P(20, 20);
|
||||
innov_var = (HKY10 * HKY20 - HKY11 * HKY18 + HKY12 * HKY22 + HKY13 * HKY16 + HKY14 * HKY21 - HKY15 * HKY19 + HKY17 *
|
||||
HKY8 + HKY23 + R_MAG);
|
||||
const float HKY24 = 1.0F / innov_var;
|
||||
|
||||
// Calculate Y axis observation jacobians
|
||||
float Hfusion[24] = {};
|
||||
Hfusion[0] = 2 * HKY0;
|
||||
Hfusion[1] = 2 * HKY1;
|
||||
Hfusion[2] = 2 * HKY2;
|
||||
Hfusion[3] = 2 * HKY3 - 2 * HKY4 - 2 * HKY5;
|
||||
Hfusion[16] = 2 * HKY6 - 2 * HKY7;
|
||||
Hfusion[17] = HKY8;
|
||||
Hfusion[18] = 2 * HKY9;
|
||||
Hfusion[20] = 1;
|
||||
|
||||
// Calculate Y axis Kalman gains
|
||||
if (true) {
|
||||
Kfusion(0) = HKY16 * HKY24;
|
||||
Kfusion(1) = HKY21 * HKY24;
|
||||
Kfusion(2) = HKY22 * HKY24;
|
||||
Kfusion(3) = HKY19 * HKY24;
|
||||
|
||||
for (unsigned row = 4; row <= 15; row++) {
|
||||
Kfusion(row) = HKY24 * (HKY10 * P(row, 18) - HKY11 * P(row, 16) + HKY12 * P(2, row) + HKY13 * P(0, row) + HKY14 * P(1,
|
||||
row) - HKY15 * P(3, row) + HKY8 * P(row, 17) + P(row, 20));
|
||||
}
|
||||
|
||||
for (unsigned row = 22; row <= 23; row++) {
|
||||
Kfusion(row) = HKY24 * (HKY10 * P(18, row) - HKY11 * P(16, row) + HKY12 * P(2, row) + HKY13 * P(0, row) + HKY14 * P(1,
|
||||
row) - HKY15 * P(3, row) + HKY8 * P(17, row) + P(20, row));
|
||||
}
|
||||
}
|
||||
|
||||
Kfusion(16) = HKY18 * HKY24;
|
||||
Kfusion(17) = HKY17 * HKY24;
|
||||
Kfusion(18) = HKY20 * HKY24;
|
||||
Kfusion(19) = HKY24 * (HKY10 * P(18, 19) - HKY11 * P(16, 19) + HKY12 * P(2, 19) + HKY13 * P(0, 19) + HKY14 * P(1,
|
||||
19) - HKY15 * P(3, 19) + HKY8 * P(17, 19) + P(19, 20));
|
||||
Kfusion(20) = HKY23 * HKY24;
|
||||
Kfusion(21) = HKY24 * (HKY10 * P(18, 21) - HKY11 * P(16, 21) + HKY12 * P(2, 21) + HKY13 * P(0, 21) + HKY14 * P(1,
|
||||
21) - HKY15 * P(3, 21) + HKY8 * P(17, 21) + P(20, 21));
|
||||
|
||||
// save output and repeat calculation using legacy matlab generated code
|
||||
for (int row = 0; row < 24; row++) {
|
||||
H(row) = Hfusion[row];
|
||||
}
|
||||
}
|
||||
|
||||
void sympyMagZInnovVarHzAndKz(float q0, float q1, float q2, float q3, float magN, float magE, float magD,
|
||||
const SquareMatrix24f &P, float R_MAG, float &innov_var, Vector24f &Kfusion, Vector24f &H)
|
||||
{
|
||||
const float HKZ0 = magD * q0 - magE * q1 + magN * q2;
|
||||
const float HKZ1 = magN * q3;
|
||||
const float HKZ2 = magD * q1;
|
||||
const float HKZ3 = magE * q0;
|
||||
const float HKZ4 = -magD * q2 + magE * q3 + magN * q0;
|
||||
const float HKZ5 = magD * q3 + magE * q2 + magN * q1;
|
||||
const float HKZ6 = q0 * q2 + q1 * q3;
|
||||
const float HKZ7 = q2 * q3;
|
||||
const float HKZ8 = q0 * q1;
|
||||
const float HKZ9 = powf(q0, 2) - powf(q1, 2) - powf(q2, 2) + powf(q3, 2);
|
||||
const float HKZ10 = 2 * HKZ6;
|
||||
const float HKZ11 = -2 * HKZ7 + 2 * HKZ8;
|
||||
const float HKZ12 = 2 * HKZ5;
|
||||
const float HKZ13 = 2 * HKZ0;
|
||||
const float HKZ14 = -2 * HKZ1 + 2 * HKZ2 + 2 * HKZ3;
|
||||
const float HKZ15 = 2 * HKZ4;
|
||||
const float HKZ16 = HKZ10 * P(0, 16) - HKZ11 * P(0, 17) + HKZ12 * P(0, 3) + HKZ13 * P(0, 0) - HKZ14 * P(0,
|
||||
1) + HKZ15 * P(0, 2) + HKZ9 * P(0, 18) + P(0, 21);
|
||||
const float HKZ17 = HKZ10 * P(16, 18) - HKZ11 * P(17, 18) + HKZ12 * P(3, 18) + HKZ13 * P(0, 18) - HKZ14 * P(1,
|
||||
18) + HKZ15 * P(2, 18) + HKZ9 * P(18, 18) + P(18, 21);
|
||||
const float HKZ18 = HKZ10 * P(16, 17) - HKZ11 * P(17, 17) + HKZ12 * P(3, 17) + HKZ13 * P(0, 17) - HKZ14 * P(1,
|
||||
17) + HKZ15 * P(2, 17) + HKZ9 * P(17, 18) + P(17, 21);
|
||||
const float HKZ19 = HKZ10 * P(1, 16) - HKZ11 * P(1, 17) + HKZ12 * P(1, 3) + HKZ13 * P(0, 1) - HKZ14 * P(1,
|
||||
1) + HKZ15 * P(1, 2) + HKZ9 * P(1, 18) + P(1, 21);
|
||||
const float HKZ20 = HKZ10 * P(16, 16) - HKZ11 * P(16, 17) + HKZ12 * P(3, 16) + HKZ13 * P(0, 16) - HKZ14 * P(1,
|
||||
16) + HKZ15 * P(2, 16) + HKZ9 * P(16, 18) + P(16, 21);
|
||||
const float HKZ21 = HKZ10 * P(3, 16) - HKZ11 * P(3, 17) + HKZ12 * P(3, 3) + HKZ13 * P(0, 3) - HKZ14 * P(1,
|
||||
3) + HKZ15 * P(2, 3) + HKZ9 * P(3, 18) + P(3, 21);
|
||||
const float HKZ22 = HKZ10 * P(2, 16) - HKZ11 * P(2, 17) + HKZ12 * P(2, 3) + HKZ13 * P(0, 2) - HKZ14 * P(1,
|
||||
2) + HKZ15 * P(2, 2) + HKZ9 * P(2, 18) + P(2, 21);
|
||||
const float HKZ23 = HKZ10 * P(16, 21) - HKZ11 * P(17, 21) + HKZ12 * P(3, 21) + HKZ13 * P(0, 21) - HKZ14 * P(1,
|
||||
21) + HKZ15 * P(2, 21) + HKZ9 * P(18, 21) + P(21, 21);
|
||||
innov_var = (HKZ10 * HKZ20 - HKZ11 * HKZ18 + HKZ12 * HKZ21 + HKZ13 * HKZ16 - HKZ14 * HKZ19 + HKZ15 * HKZ22 + HKZ17 *
|
||||
HKZ9 + HKZ23 + R_MAG);
|
||||
const float HKZ24 = 1.0F / innov_var;
|
||||
|
||||
// calculate Z axis observation jacobians
|
||||
float Hfusion[24] = {};
|
||||
Hfusion[0] = 2 * HKZ0;
|
||||
Hfusion[1] = 2 * HKZ1 - 2 * HKZ2 - 2 * HKZ3;
|
||||
Hfusion[2] = 2 * HKZ4;
|
||||
Hfusion[3] = 2 * HKZ5;
|
||||
Hfusion[16] = 2 * HKZ6;
|
||||
Hfusion[17] = 2 * HKZ7 - 2 * HKZ8;
|
||||
Hfusion[18] = HKZ9;
|
||||
Hfusion[21] = 1;
|
||||
|
||||
// Calculate Z axis Kalman gains
|
||||
if (true) {
|
||||
Kfusion(0) = HKZ16 * HKZ24;
|
||||
Kfusion(1) = HKZ19 * HKZ24;
|
||||
Kfusion(2) = HKZ22 * HKZ24;
|
||||
Kfusion(3) = HKZ21 * HKZ24;
|
||||
|
||||
for (unsigned row = 4; row <= 15; row++) {
|
||||
Kfusion(row) = HKZ24 * (HKZ10 * P(row, 16) - HKZ11 * P(row, 17) + HKZ12 * P(3, row) + HKZ13 * P(0, row) - HKZ14 * P(1,
|
||||
row) + HKZ15 * P(2, row) + HKZ9 * P(row, 18) + P(row, 21));
|
||||
}
|
||||
|
||||
for (unsigned row = 22; row <= 23; row++) {
|
||||
Kfusion(row) = HKZ24 * (HKZ10 * P(16, row) - HKZ11 * P(17, row) + HKZ12 * P(3, row) + HKZ13 * P(0, row) - HKZ14 * P(1,
|
||||
row) + HKZ15 * P(2, row) + HKZ9 * P(18, row) + P(21, row));
|
||||
}
|
||||
}
|
||||
|
||||
Kfusion(16) = HKZ20 * HKZ24;
|
||||
Kfusion(17) = HKZ18 * HKZ24;
|
||||
Kfusion(18) = HKZ17 * HKZ24;
|
||||
|
||||
for (unsigned row = 19; row <= 20; row++) {
|
||||
Kfusion(row) = HKZ24 * (HKZ10 * P(16, row) - HKZ11 * P(17, row) + HKZ12 * P(3, row) + HKZ13 * P(0, row) - HKZ14 * P(1,
|
||||
row) + HKZ15 * P(2, row) + HKZ9 * P(18, row) + P(row, 21));
|
||||
}
|
||||
|
||||
Kfusion(21) = HKZ23 * HKZ24;
|
||||
|
||||
// save output and repeat calculation using legacy matlab generated code
|
||||
for (int row = 0; row < 24; row++) {
|
||||
H(row) = Hfusion[row];
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Mag3DFusionGenerated, SympyVsSymforce)
|
||||
{
|
||||
// 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));
|
||||
const float q0 = q(0);
|
||||
const float q1 = q(1);
|
||||
|
|
@ -322,45 +86,25 @@ TEST(Mag3DFusionGenerated, SympyVsSymforce)
|
|||
|
||||
SquareMatrix24f P = createRandomCovarianceMatrix24f();
|
||||
|
||||
Vector24f Hfusion_sympy;
|
||||
Vector24f Kfusion_sympy;
|
||||
float mag_innov_var_sympy;
|
||||
|
||||
Vector24f H_dual;
|
||||
Vector24f Hfusion_symforce;
|
||||
Vector24f Kfusion_symforce;
|
||||
Vector3f mag_innov_var_symforce;
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if (i == 0) {
|
||||
sympyMagXInnovVarHxAndKx(q0, q1, q2, q3, magN, magE, magD, P, R_MAG, mag_innov_var_sympy, Kfusion_sympy, Hfusion_sympy);
|
||||
computeHDual(state_vector, i, H_dual);
|
||||
|
||||
if (i == 0) {
|
||||
Vector3f innov;
|
||||
sym::ComputeMagInnovInnovVarAndHx(state_vector, P, Vector3f(), R_MAG, FLT_EPSILON, &innov, &mag_innov_var_symforce,
|
||||
&Hfusion_symforce);
|
||||
|
||||
} else if (i == 1) {
|
||||
sympyMagYInnovVarHyAndKy(q0, q1, q2, q3, magN, magE, magD, P, R_MAG, mag_innov_var_sympy, Kfusion_sympy, Hfusion_sympy);
|
||||
|
||||
sym::ComputeMagYInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &(mag_innov_var_symforce(i)), &Hfusion_symforce);
|
||||
|
||||
} else {
|
||||
sympyMagZInnovVarHzAndKz(q0, q1, q2, q3, magN, magE, magD, P, R_MAG, mag_innov_var_sympy, Kfusion_sympy, Hfusion_sympy);
|
||||
|
||||
sym::ComputeMagZInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &(mag_innov_var_symforce(i)), &Hfusion_symforce);
|
||||
}
|
||||
|
||||
// K isn't generated from symbolic anymore to save flash space
|
||||
Kfusion_symforce = P * Hfusion_symforce / mag_innov_var_symforce(i);
|
||||
|
||||
DiffRatioReport report = computeDiffRatioVector24f(Hfusion_sympy, Hfusion_symforce);
|
||||
EXPECT_LT(report.max_diff_fraction, 1e-5f) << "i = " << i << "Airspeed 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 << "Airspeed Kfusion max diff fraction = " <<
|
||||
report.max_diff_fraction <<
|
||||
" location index = " << report.max_row << " sympy = " << report.max_v1 << " symforce = " << report.max_v2;
|
||||
EXPECT_NEAR(mag_innov_var_sympy, mag_innov_var_symforce(i), 1e-5f) << "i = " << i;
|
||||
EXPECT_EQ(Hfusion_symforce, H_dual);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue