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

ekf2: remove old mag 3D fusion auto-code

This commit is contained in:
bresch 2022-10-14 11:38:35 +02:00 committed by Daniel Agar
parent 77a36219c6
commit 8b9ac2d7f3
4 changed files with 0 additions and 1050 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

604
src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_fusion_generated_compare.cpp
View File

@ -1,604 +0,0 @@
#include <math.h>
#include <stdio.h>
#include <cstdlib>
#include "../../../../../matrix/matrix/math.hpp"
typedef matrix::Vector<float, 24> Vector24f;
typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
float sq(float in) {
return in * in;
}
int main()
{
// Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations
float Hfusion[24];
Vector24f H_MAG;
Vector24f Kfusion;
float mag_innov_var;
// quaternion inputs must be normalised
float q0 = 2.0f * ((float)rand() - 0.5f);
float q1 = 2.0f * ((float)rand() - 0.5f);
float q2 = 2.0f * ((float)rand() - 0.5f);
float q3 = 2.0f * ((float)rand() - 0.5f);
const float length = sqrtf(sq(q0) + sq(q1) + sq(q2) + sq(q3));
q0 /= length;
q1 /= length;
q2 /= length;
q3 /= length;
const float magN = 2.0f * ((float)rand() - 0.5f);
const float magE = 2.0f * ((float)rand() - 0.5f);
const float magD = 2.0f * ((float)rand() - 0.5f);
const float R_MAG = sq(0.05f);
const bool update_all_states = true;
// create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
SquareMatrix24f P;
for (int col=0; col<=23; col++) {
for (int row=0; row<=col; row++) {
if (row == col) {
P(row,col) = (float)rand();
} else {
P(col,row) = P(row,col) = 2.0f * ((float)rand() - 0.5f);
}
}
}
// 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);
// common expressions used by matlab generated equations
float SH_MAG[9];
SH_MAG[0] = 2.0f*magD*q3 + 2.0f*magE*q2 + 2.0f*magN*q1;
SH_MAG[1] = 2.0f*magD*q0 - 2.0f*magE*q1 + 2.0f*magN*q2;
SH_MAG[2] = 2.0f*magD*q1 + 2.0f*magE*q0 - 2.0f*magN*q3;
SH_MAG[3] = sq(q3);
SH_MAG[4] = sq(q2);
SH_MAG[5] = sq(q1);
SH_MAG[6] = sq(q0);
SH_MAG[7] = 2.0f*magN*q0;
SH_MAG[8] = 2.0f*magE*q3;
// Compare X axis equations
{
mag_innov_var = (HKX10*HKX20 - HKX11*HKX18 + HKX12*HKX22 + HKX13*HKX16 - HKX14*HKX19 + HKX15*HKX21 + HKX17*HKX6 + HKX23 + R_MAG);
float HK50 = 1.0F/mag_innov_var;
// Calculate X axis observation jacobians
memset(Hfusion, 0, sizeof(Hfusion));
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;
// Calculate X axis Kalman gains
if (update_all_states) {
Kfusion(0) = HKX16*HKX24;
Kfusion(1) = HKX22*HKX24;
Kfusion(2) = HKX19*HKX24;
Kfusion(3) = HKX21*HKX24;
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));
}
// save output and repeat calculation using legacy matlab generated code
float Hfusion_sympy[24];
Vector24f Kfusion_sympy;
for (int row=0; row<24; row++) {
Hfusion_sympy[row] = Hfusion[row];
Kfusion_sympy(row) = Kfusion(row);
}
// repeat calculation using matlab generated equations
// X axis innovation variance
mag_innov_var = (P(19,19) + R_MAG + P(1,19)*SH_MAG[0] - P(2,19)*SH_MAG[1] + P(3,19)*SH_MAG[2] - P(16,19)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + (2.0f*q0*q3 + 2.0f*q1*q2)*(P(19,17) + P(1,17)*SH_MAG[0] - P(2,17)*SH_MAG[1] + P(3,17)*SH_MAG[2] - P(16,17)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,17)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,17)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,17)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (2.0f*q0*q2 - 2.0f*q1*q3)*(P(19,18) + P(1,18)*SH_MAG[0] - P(2,18)*SH_MAG[1] + P(3,18)*SH_MAG[2] - P(16,18)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,18)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,18)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,18)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P(19,0) + P(1,0)*SH_MAG[0] - P(2,0)*SH_MAG[1] + P(3,0)*SH_MAG[2] - P(16,0)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,0)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,0)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,0)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(17,19)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,19)*(2.0f*q0*q2 - 2.0f*q1*q3) + SH_MAG[0]*(P(19,1) + P(1,1)*SH_MAG[0] - P(2,1)*SH_MAG[1] + P(3,1)*SH_MAG[2] - P(16,1)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,1)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,1)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,1)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - SH_MAG[1]*(P(19,2) + P(1,2)*SH_MAG[0] - P(2,2)*SH_MAG[1] + P(3,2)*SH_MAG[2] - P(16,2)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,2)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,2)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,2)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[2]*(P(19,3) + P(1,3)*SH_MAG[0] - P(2,3)*SH_MAG[1] + P(3,3)*SH_MAG[2] - P(16,3)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,3)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,3)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,3)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P(19,16) + P(1,16)*SH_MAG[0] - P(2,16)*SH_MAG[1] + P(3,16)*SH_MAG[2] - P(16,16)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,16)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,16)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,16)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(0,19)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2));
// Calculate X axis observation jacobians
H_MAG.setZero();
H_MAG(0) = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
H_MAG(1) = SH_MAG[0];
H_MAG(2) = -SH_MAG[1];
H_MAG(3) = SH_MAG[2];
H_MAG(16) = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6];
H_MAG(17) = 2.0f*q0*q3 + 2.0f*q1*q2;
H_MAG(18) = 2.0f*q1*q3 - 2.0f*q0*q2;
H_MAG(19) = 1.0f;
// Calculate X axis Kalman gains
float SK_MX[5];
SK_MX[0] = 1.0f / mag_innov_var;
SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6];
SK_MX[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
SK_MX[3] = 2.0f*q0*q2 - 2.0f*q1*q3;
SK_MX[4] = 2.0f*q0*q3 + 2.0f*q1*q2;
if (update_all_states) {
Kfusion(0) = SK_MX[0]*(P(0,19) + P(0,1)*SH_MAG[0] - P(0,2)*SH_MAG[1] + P(0,3)*SH_MAG[2] + P(0,0)*SK_MX[2] - P(0,16)*SK_MX[1] + P(0,17)*SK_MX[4] - P(0,18)*SK_MX[3]);
Kfusion(1) = SK_MX[0]*(P(1,19) + P(1,1)*SH_MAG[0] - P(1,2)*SH_MAG[1] + P(1,3)*SH_MAG[2] + P(1,0)*SK_MX[2] - P(1,16)*SK_MX[1] + P(1,17)*SK_MX[4] - P(1,18)*SK_MX[3]);
Kfusion(2) = SK_MX[0]*(P(2,19) + P(2,1)*SH_MAG[0] - P(2,2)*SH_MAG[1] + P(2,3)*SH_MAG[2] + P(2,0)*SK_MX[2] - P(2,16)*SK_MX[1] + P(2,17)*SK_MX[4] - P(2,18)*SK_MX[3]);
Kfusion(3) = SK_MX[0]*(P(3,19) + P(3,1)*SH_MAG[0] - P(3,2)*SH_MAG[1] + P(3,3)*SH_MAG[2] + P(3,0)*SK_MX[2] - P(3,16)*SK_MX[1] + P(3,17)*SK_MX[4] - P(3,18)*SK_MX[3]);
Kfusion(4) = SK_MX[0]*(P(4,19) + P(4,1)*SH_MAG[0] - P(4,2)*SH_MAG[1] + P(4,3)*SH_MAG[2] + P(4,0)*SK_MX[2] - P(4,16)*SK_MX[1] + P(4,17)*SK_MX[4] - P(4,18)*SK_MX[3]);
Kfusion(5) = SK_MX[0]*(P(5,19) + P(5,1)*SH_MAG[0] - P(5,2)*SH_MAG[1] + P(5,3)*SH_MAG[2] + P(5,0)*SK_MX[2] - P(5,16)*SK_MX[1] + P(5,17)*SK_MX[4] - P(5,18)*SK_MX[3]);
Kfusion(6) = SK_MX[0]*(P(6,19) + P(6,1)*SH_MAG[0] - P(6,2)*SH_MAG[1] + P(6,3)*SH_MAG[2] + P(6,0)*SK_MX[2] - P(6,16)*SK_MX[1] + P(6,17)*SK_MX[4] - P(6,18)*SK_MX[3]);
Kfusion(7) = SK_MX[0]*(P(7,19) + P(7,1)*SH_MAG[0] - P(7,2)*SH_MAG[1] + P(7,3)*SH_MAG[2] + P(7,0)*SK_MX[2] - P(7,16)*SK_MX[1] + P(7,17)*SK_MX[4] - P(7,18)*SK_MX[3]);
Kfusion(8) = SK_MX[0]*(P(8,19) + P(8,1)*SH_MAG[0] - P(8,2)*SH_MAG[1] + P(8,3)*SH_MAG[2] + P(8,0)*SK_MX[2] - P(8,16)*SK_MX[1] + P(8,17)*SK_MX[4] - P(8,18)*SK_MX[3]);
Kfusion(9) = SK_MX[0]*(P(9,19) + P(9,1)*SH_MAG[0] - P(9,2)*SH_MAG[1] + P(9,3)*SH_MAG[2] + P(9,0)*SK_MX[2] - P(9,16)*SK_MX[1] + P(9,17)*SK_MX[4] - P(9,18)*SK_MX[3]);
Kfusion(10) = SK_MX[0]*(P(10,19) + P(10,1)*SH_MAG[0] - P(10,2)*SH_MAG[1] + P(10,3)*SH_MAG[2] + P(10,0)*SK_MX[2] - P(10,16)*SK_MX[1] + P(10,17)*SK_MX[4] - P(10,18)*SK_MX[3]);
Kfusion(11) = SK_MX[0]*(P(11,19) + P(11,1)*SH_MAG[0] - P(11,2)*SH_MAG[1] + P(11,3)*SH_MAG[2] + P(11,0)*SK_MX[2] - P(11,16)*SK_MX[1] + P(11,17)*SK_MX[4] - P(11,18)*SK_MX[3]);
Kfusion(12) = SK_MX[0]*(P(12,19) + P(12,1)*SH_MAG[0] - P(12,2)*SH_MAG[1] + P(12,3)*SH_MAG[2] + P(12,0)*SK_MX[2] - P(12,16)*SK_MX[1] + P(12,17)*SK_MX[4] - P(12,18)*SK_MX[3]);
Kfusion(13) = SK_MX[0]*(P(13,19) + P(13,1)*SH_MAG[0] - P(13,2)*SH_MAG[1] + P(13,3)*SH_MAG[2] + P(13,0)*SK_MX[2] - P(13,16)*SK_MX[1] + P(13,17)*SK_MX[4] - P(13,18)*SK_MX[3]);
Kfusion(14) = SK_MX[0]*(P(14,19) + P(14,1)*SH_MAG[0] - P(14,2)*SH_MAG[1] + P(14,3)*SH_MAG[2] + P(14,0)*SK_MX[2] - P(14,16)*SK_MX[1] + P(14,17)*SK_MX[4] - P(14,18)*SK_MX[3]);
Kfusion(15) = SK_MX[0]*(P(15,19) + P(15,1)*SH_MAG[0] - P(15,2)*SH_MAG[1] + P(15,3)*SH_MAG[2] + P(15,0)*SK_MX[2] - P(15,16)*SK_MX[1] + P(15,17)*SK_MX[4] - P(15,18)*SK_MX[3]);
Kfusion(22) = SK_MX[0]*(P(22,19) + P(22,1)*SH_MAG[0] - P(22,2)*SH_MAG[1] + P(22,3)*SH_MAG[2] + P(22,0)*SK_MX[2] - P(22,16)*SK_MX[1] + P(22,17)*SK_MX[4] - P(22,18)*SK_MX[3]);
Kfusion(23) = SK_MX[0]*(P(23,19) + P(23,1)*SH_MAG[0] - P(23,2)*SH_MAG[1] + P(23,3)*SH_MAG[2] + P(23,0)*SK_MX[2] - P(23,16)*SK_MX[1] + P(23,17)*SK_MX[4] - P(23,18)*SK_MX[3]);
}
Kfusion(16) = SK_MX[0]*(P(16,19) + P(16,1)*SH_MAG[0] - P(16,2)*SH_MAG[1] + P(16,3)*SH_MAG[2] + P(16,0)*SK_MX[2] - P(16,16)*SK_MX[1] + P(16,17)*SK_MX[4] - P(16,18)*SK_MX[3]);
Kfusion(17) = SK_MX[0]*(P(17,19) + P(17,1)*SH_MAG[0] - P(17,2)*SH_MAG[1] + P(17,3)*SH_MAG[2] + P(17,0)*SK_MX[2] - P(17,16)*SK_MX[1] + P(17,17)*SK_MX[4] - P(17,18)*SK_MX[3]);
Kfusion(18) = SK_MX[0]*(P(18,19) + P(18,1)*SH_MAG[0] - P(18,2)*SH_MAG[1] + P(18,3)*SH_MAG[2] + P(18,0)*SK_MX[2] - P(18,16)*SK_MX[1] + P(18,17)*SK_MX[4] - P(18,18)*SK_MX[3]);
Kfusion(19) = SK_MX[0]*(P(19,19) + P(19,1)*SH_MAG[0] - P(19,2)*SH_MAG[1] + P(19,3)*SH_MAG[2] + P(19,0)*SK_MX[2] - P(19,16)*SK_MX[1] + P(19,17)*SK_MX[4] - P(19,18)*SK_MX[3]);
Kfusion(20) = SK_MX[0]*(P(20,19) + P(20,1)*SH_MAG[0] - P(20,2)*SH_MAG[1] + P(20,3)*SH_MAG[2] + P(20,0)*SK_MX[2] - P(20,16)*SK_MX[1] + P(20,17)*SK_MX[4] - P(20,18)*SK_MX[3]);
Kfusion(21) = SK_MX[0]*(P(21,19) + P(21,1)*SH_MAG[0] - P(21,2)*SH_MAG[1] + P(21,3)*SH_MAG[2] + P(21,0)*SK_MX[2] - P(21,16)*SK_MX[1] + P(21,17)*SK_MX[4] - P(21,18)*SK_MX[3]);
// find largest observation variance difference as a fraction of the matlab value
float max_diff_fraction = 0.0f;
int max_row;
float max_old, max_new;
for (int row=0; row<24; row++) {
float diff_fraction;
if (H_MAG(row) != 0.0f) {
diff_fraction = fabsf(Hfusion_sympy[row] - H_MAG(row)) / fabsf(H_MAG(row));
} else if (Hfusion_sympy[row] != 0.0f) {
diff_fraction = fabsf(Hfusion_sympy[row] - H_MAG(row)) / fabsf(Hfusion_sympy[row]);
} else {
diff_fraction = 0.0f;
}
if (diff_fraction > max_diff_fraction) {
max_diff_fraction = diff_fraction;
max_row = row;
max_old = H_MAG(row);
max_new = Hfusion_sympy[row];
}
}
if (max_diff_fraction > 1e-5f) {
printf("Fail: Magnetomer X axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
} else {
printf("Pass: Magnetomer X axis Hfusion max diff fraction = %e\n",max_diff_fraction);
}
// find largest Kalman gain difference as a fraction of the matlab value
max_diff_fraction = 0.0f;
for (int row=0; row<4; row++) {
float diff_fraction;
if (Kfusion(row) != 0.0f) {
diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion(row)) / fabsf(Kfusion(row));
} else if (Kfusion_sympy(row) != 0.0f) {
diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion(row)) / fabsf(Kfusion_sympy(row));
} else {
diff_fraction = 0.0f;
}
if (diff_fraction > max_diff_fraction) {
max_diff_fraction = diff_fraction;
max_row = row;
max_old = Kfusion(row);
max_new = Kfusion_sympy(row);
}
}
if (max_diff_fraction > 1e-5f) {
printf("Fail: Magnetomer X axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
} else {
printf("Pass: Magnetomer X axis Kfusion max diff fraction = %e\n",max_diff_fraction);
}
}
// Compare Y axis equations
{
// recalculate innovation variance becasue states and covariances have changed due to previous fusion
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);
const float HKY24 = 1.0F/(HKY10*HKY20 - HKY11*HKY18 + HKY12*HKY22 + HKY13*HKY16 + HKY14*HKY21 - HKY15*HKY19 + HKY17*HKY8 + HKY23 + R_MAG);
// Calculate Y axis observation jacobians
memset(Hfusion, 0, sizeof(Hfusion));
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 (update_all_states) {
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
float Hfusion_sympy[24];
Vector24f Kfusion_sympy;
for (int row=0; row<24; row++) {
Hfusion_sympy[row] = Hfusion[row];
Kfusion_sympy(row) = Kfusion(row);
}
// repeat calculation using matlab generated equations
// Y axis innovation variance
mag_innov_var = (P(20,20) + R_MAG + P(0,20)*SH_MAG[2] + P(1,20)*SH_MAG[1] + P(2,20)*SH_MAG[0] - P(17,20)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2.0f*q0*q3 - 2.0f*q1*q2)*(P(20,16) + P(0,16)*SH_MAG[2] + P(1,16)*SH_MAG[1] + P(2,16)*SH_MAG[0] - P(17,16)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,16)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,16)*(2.0f*q0*q1 + 2.0f*q2*q3) - P(3,16)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (2.0f*q0*q1 + 2.0f*q2*q3)*(P(20,18) + P(0,18)*SH_MAG[2] + P(1,18)*SH_MAG[1] + P(2,18)*SH_MAG[0] - P(17,18)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,18)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,18)*(2.0f*q0*q1 + 2.0f*q2*q3) - P(3,18)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P(20,3) + P(0,3)*SH_MAG[2] + P(1,3)*SH_MAG[1] + P(2,3)*SH_MAG[0] - P(17,3)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,3)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,3)*(2.0f*q0*q1 + 2.0f*q2*q3) - P(3,3)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - P(16,20)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,20)*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_MAG[2]*(P(20,0) + P(0,0)*SH_MAG[2] + P(1,0)*SH_MAG[1] + P(2,0)*SH_MAG[0] - P(17,0)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,0)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,0)*(2.0f*q0*q1 + 2.0f*q2*q3) - P(3,0)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[1]*(P(20,1) + P(0,1)*SH_MAG[2] + P(1,1)*SH_MAG[1] + P(2,1)*SH_MAG[0] - P(17,1)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,1)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,1)*(2.0f*q0*q1 + 2.0f*q2*q3) - P(3,1)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[0]*(P(20,2) + P(0,2)*SH_MAG[2] + P(1,2)*SH_MAG[1] + P(2,2)*SH_MAG[0] - P(17,2)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,2)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,2)*(2.0f*q0*q1 + 2.0f*q2*q3) - P(3,2)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P(20,17) + P(0,17)*SH_MAG[2] + P(1,17)*SH_MAG[1] + P(2,17)*SH_MAG[0] - P(17,17)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,17)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,17)*(2.0f*q0*q1 + 2.0f*q2*q3) - P(3,17)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - P(3,20)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2));
// Calculate Y axis observation jacobians
H_MAG.setZero();
H_MAG(0) = SH_MAG[2];
H_MAG(1) = SH_MAG[1];
H_MAG(2) = SH_MAG[0];
H_MAG(3) = 2.0f*magD*q2 - SH_MAG[8] - SH_MAG[7];
H_MAG(16) = 2.0f*q1*q2 - 2.0f*q0*q3;
H_MAG(17) = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6];
H_MAG(18) = 2.0f*q0*q1 + 2.0f*q2*q3;
H_MAG(20) = 1.0f;
// Calculate Y axis Kalman gains
float SK_MY[5];
SK_MY[0] = 1.0f / mag_innov_var;
SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6];
SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
SK_MY[3] = 2.0f*q0*q3 - 2.0f*q1*q2;
SK_MY[4] = 2.0f*q0*q1 + 2.0f*q2*q3;
if (update_all_states) {
Kfusion(0) = SK_MY[0]*(P(0,20) + P(0,0)*SH_MAG[2] + P(0,1)*SH_MAG[1] + P(0,2)*SH_MAG[0] - P(0,3)*SK_MY[2] - P(0,17)*SK_MY[1] - P(0,16)*SK_MY[3] + P(0,18)*SK_MY[4]);
Kfusion(1) = SK_MY[0]*(P(1,20) + P(1,0)*SH_MAG[2] + P(1,1)*SH_MAG[1] + P(1,2)*SH_MAG[0] - P(1,3)*SK_MY[2] - P(1,17)*SK_MY[1] - P(1,16)*SK_MY[3] + P(1,18)*SK_MY[4]);
Kfusion(2) = SK_MY[0]*(P(2,20) + P(2,0)*SH_MAG[2] + P(2,1)*SH_MAG[1] + P(2,2)*SH_MAG[0] - P(2,3)*SK_MY[2] - P(2,17)*SK_MY[1] - P(2,16)*SK_MY[3] + P(2,18)*SK_MY[4]);
Kfusion(3) = SK_MY[0]*(P(3,20) + P(3,0)*SH_MAG[2] + P(3,1)*SH_MAG[1] + P(3,2)*SH_MAG[0] - P(3,3)*SK_MY[2] - P(3,17)*SK_MY[1] - P(3,16)*SK_MY[3] + P(3,18)*SK_MY[4]);
Kfusion(4) = SK_MY[0]*(P(4,20) + P(4,0)*SH_MAG[2] + P(4,1)*SH_MAG[1] + P(4,2)*SH_MAG[0] - P(4,3)*SK_MY[2] - P(4,17)*SK_MY[1] - P(4,16)*SK_MY[3] + P(4,18)*SK_MY[4]);
Kfusion(5) = SK_MY[0]*(P(5,20) + P(5,0)*SH_MAG[2] + P(5,1)*SH_MAG[1] + P(5,2)*SH_MAG[0] - P(5,3)*SK_MY[2] - P(5,17)*SK_MY[1] - P(5,16)*SK_MY[3] + P(5,18)*SK_MY[4]);
Kfusion(6) = SK_MY[0]*(P(6,20) + P(6,0)*SH_MAG[2] + P(6,1)*SH_MAG[1] + P(6,2)*SH_MAG[0] - P(6,3)*SK_MY[2] - P(6,17)*SK_MY[1] - P(6,16)*SK_MY[3] + P(6,18)*SK_MY[4]);
Kfusion(7) = SK_MY[0]*(P(7,20) + P(7,0)*SH_MAG[2] + P(7,1)*SH_MAG[1] + P(7,2)*SH_MAG[0] - P(7,3)*SK_MY[2] - P(7,17)*SK_MY[1] - P(7,16)*SK_MY[3] + P(7,18)*SK_MY[4]);
Kfusion(8) = SK_MY[0]*(P(8,20) + P(8,0)*SH_MAG[2] + P(8,1)*SH_MAG[1] + P(8,2)*SH_MAG[0] - P(8,3)*SK_MY[2] - P(8,17)*SK_MY[1] - P(8,16)*SK_MY[3] + P(8,18)*SK_MY[4]);
Kfusion(9) = SK_MY[0]*(P(9,20) + P(9,0)*SH_MAG[2] + P(9,1)*SH_MAG[1] + P(9,2)*SH_MAG[0] - P(9,3)*SK_MY[2] - P(9,17)*SK_MY[1] - P(9,16)*SK_MY[3] + P(9,18)*SK_MY[4]);
Kfusion(10) = SK_MY[0]*(P(10,20) + P(10,0)*SH_MAG[2] + P(10,1)*SH_MAG[1] + P(10,2)*SH_MAG[0] - P(10,3)*SK_MY[2] - P(10,17)*SK_MY[1] - P(10,16)*SK_MY[3] + P(10,18)*SK_MY[4]);
Kfusion(11) = SK_MY[0]*(P(11,20) + P(11,0)*SH_MAG[2] + P(11,1)*SH_MAG[1] + P(11,2)*SH_MAG[0] - P(11,3)*SK_MY[2] - P(11,17)*SK_MY[1] - P(11,16)*SK_MY[3] + P(11,18)*SK_MY[4]);
Kfusion(12) = SK_MY[0]*(P(12,20) + P(12,0)*SH_MAG[2] + P(12,1)*SH_MAG[1] + P(12,2)*SH_MAG[0] - P(12,3)*SK_MY[2] - P(12,17)*SK_MY[1] - P(12,16)*SK_MY[3] + P(12,18)*SK_MY[4]);
Kfusion(13) = SK_MY[0]*(P(13,20) + P(13,0)*SH_MAG[2] + P(13,1)*SH_MAG[1] + P(13,2)*SH_MAG[0] - P(13,3)*SK_MY[2] - P(13,17)*SK_MY[1] - P(13,16)*SK_MY[3] + P(13,18)*SK_MY[4]);
Kfusion(14) = SK_MY[0]*(P(14,20) + P(14,0)*SH_MAG[2] + P(14,1)*SH_MAG[1] + P(14,2)*SH_MAG[0] - P(14,3)*SK_MY[2] - P(14,17)*SK_MY[1] - P(14,16)*SK_MY[3] + P(14,18)*SK_MY[4]);
Kfusion(15) = SK_MY[0]*(P(15,20) + P(15,0)*SH_MAG[2] + P(15,1)*SH_MAG[1] + P(15,2)*SH_MAG[0] - P(15,3)*SK_MY[2] - P(15,17)*SK_MY[1] - P(15,16)*SK_MY[3] + P(15,18)*SK_MY[4]);
Kfusion(22) = SK_MY[0]*(P(22,20) + P(22,0)*SH_MAG[2] + P(22,1)*SH_MAG[1] + P(22,2)*SH_MAG[0] - P(22,3)*SK_MY[2] - P(22,17)*SK_MY[1] - P(22,16)*SK_MY[3] + P(22,18)*SK_MY[4]);
Kfusion(23) = SK_MY[0]*(P(23,20) + P(23,0)*SH_MAG[2] + P(23,1)*SH_MAG[1] + P(23,2)*SH_MAG[0] - P(23,3)*SK_MY[2] - P(23,17)*SK_MY[1] - P(23,16)*SK_MY[3] + P(23,18)*SK_MY[4]);
}
Kfusion(16) = SK_MY[0]*(P(16,20) + P(16,0)*SH_MAG[2] + P(16,1)*SH_MAG[1] + P(16,2)*SH_MAG[0] - P(16,3)*SK_MY[2] - P(16,17)*SK_MY[1] - P(16,16)*SK_MY[3] + P(16,18)*SK_MY[4]);
Kfusion(17) = SK_MY[0]*(P(17,20) + P(17,0)*SH_MAG[2] + P(17,1)*SH_MAG[1] + P(17,2)*SH_MAG[0] - P(17,3)*SK_MY[2] - P(17,17)*SK_MY[1] - P(17,16)*SK_MY[3] + P(17,18)*SK_MY[4]);
Kfusion(18) = SK_MY[0]*(P(18,20) + P(18,0)*SH_MAG[2] + P(18,1)*SH_MAG[1] + P(18,2)*SH_MAG[0] - P(18,3)*SK_MY[2] - P(18,17)*SK_MY[1] - P(18,16)*SK_MY[3] + P(18,18)*SK_MY[4]);
Kfusion(19) = SK_MY[0]*(P(19,20) + P(19,0)*SH_MAG[2] + P(19,1)*SH_MAG[1] + P(19,2)*SH_MAG[0] - P(19,3)*SK_MY[2] - P(19,17)*SK_MY[1] - P(19,16)*SK_MY[3] + P(19,18)*SK_MY[4]);
Kfusion(20) = SK_MY[0]*(P(20,20) + P(20,0)*SH_MAG[2] + P(20,1)*SH_MAG[1] + P(20,2)*SH_MAG[0] - P(20,3)*SK_MY[2] - P(20,17)*SK_MY[1] - P(20,16)*SK_MY[3] + P(20,18)*SK_MY[4]);
Kfusion(21) = SK_MY[0]*(P(21,20) + P(21,0)*SH_MAG[2] + P(21,1)*SH_MAG[1] + P(21,2)*SH_MAG[0] - P(21,3)*SK_MY[2] - P(21,17)*SK_MY[1] - P(21,16)*SK_MY[3] + P(21,18)*SK_MY[4]);
// find largest observation variance difference as a fraction of the matlab value
float max_diff_fraction = 0.0f;
int max_row;
float max_old, max_new;
for (int row=0; row<24; row++) {
float diff_fraction;
if (H_MAG(row) != 0.0f) {
diff_fraction = fabsf(Hfusion_sympy[row] - H_MAG(row)) / fabsf(H_MAG(row));
} else if (Hfusion_sympy[row] != 0.0f) {
diff_fraction = fabsf(Hfusion_sympy[row] - H_MAG(row)) / fabsf(Hfusion_sympy[row]);
} else {
diff_fraction = 0.0f;
}
if (diff_fraction > max_diff_fraction) {
max_diff_fraction = diff_fraction;
max_row = row;
max_old = H_MAG(row);
max_new = Hfusion_sympy[row];
}
}
if (max_diff_fraction > 1e-5f) {
printf("Fail: Magnetomer Y axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
} else {
printf("Pass: Magnetomer Y axis Hfusion max diff fraction = %e\n",max_diff_fraction);
}
// find largest Kalman gain difference as a fraction of the matlab value
max_diff_fraction = 0.0f;
for (int row=0; row<4; row++) {
float diff_fraction;
if (Kfusion(row) != 0.0f) {
diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion(row)) / fabsf(Kfusion(row));
} else if (Kfusion_sympy(row) != 0.0f) {
diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion(row)) / fabsf(Kfusion_sympy(row));
} else {
diff_fraction = 0.0f;
}
if (diff_fraction > max_diff_fraction) {
max_diff_fraction = diff_fraction;
max_row = row;
max_old = Kfusion(row);
max_new = Kfusion_sympy(row);
}
}
if (max_diff_fraction > 1e-5f) {
printf("Fail: Magnetomer Y axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
} else {
printf("Pass: Magnetomer Y axis Kfusion max diff fraction = %e\n",max_diff_fraction);
}
}
// Compare Z axis equations
{
// recalculate innovation variance becasue states and covariances have changed due to previous fusion
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);
const float HKZ24 = 1.0F/(HKZ10*HKZ20 - HKZ11*HKZ18 + HKZ12*HKZ21 + HKZ13*HKZ16 - HKZ14*HKZ19 + HKZ15*HKZ22 + HKZ17*HKZ9 + HKZ23 + R_MAG);
// calculate Z axis observation jacobians
memset(Hfusion, 0, sizeof(Hfusion));
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 (update_all_states) {
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
float Hfusion_sympy[24];
Vector24f Kfusion_sympy;
for (int row=0; row<24; row++) {
Hfusion_sympy[row] = Hfusion[row];
Kfusion_sympy(row) = Kfusion(row);
}
// repeat calculation using matlab generated equations
// Z axis innovation variance
mag_innov_var = (P(21,21) + R_MAG + P(0,21)*SH_MAG[1] - P(1,21)*SH_MAG[2] + P(3,21)*SH_MAG[0] + P(18,21)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + (2.0f*q0*q2 + 2.0f*q1*q3)*(P(21,16) + P(0,16)*SH_MAG[1] - P(1,16)*SH_MAG[2] + P(3,16)*SH_MAG[0] + P(18,16)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,16)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,16)*(2.0f*q0*q1 - 2.0f*q2*q3) + P(2,16)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (2.0f*q0*q1 - 2.0f*q2*q3)*(P(21,17) + P(0,17)*SH_MAG[1] - P(1,17)*SH_MAG[2] + P(3,17)*SH_MAG[0] + P(18,17)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,17)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,17)*(2.0f*q0*q1 - 2.0f*q2*q3) + P(2,17)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P(21,2) + P(0,2)*SH_MAG[1] - P(1,2)*SH_MAG[2] + P(3,2)*SH_MAG[0] + P(18,2)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,2)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,2)*(2.0f*q0*q1 - 2.0f*q2*q3) + P(2,2)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(16,21)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,21)*(2.0f*q0*q1 - 2.0f*q2*q3) + SH_MAG[1]*(P(21,0) + P(0,0)*SH_MAG[1] - P(1,0)*SH_MAG[2] + P(3,0)*SH_MAG[0] + P(18,0)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,0)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,0)*(2.0f*q0*q1 - 2.0f*q2*q3) + P(2,0)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - SH_MAG[2]*(P(21,1) + P(0,1)*SH_MAG[1] - P(1,1)*SH_MAG[2] + P(3,1)*SH_MAG[0] + P(18,1)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,1)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,1)*(2.0f*q0*q1 - 2.0f*q2*q3) + P(2,1)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[0]*(P(21,3) + P(0,3)*SH_MAG[1] - P(1,3)*SH_MAG[2] + P(3,3)*SH_MAG[0] + P(18,3)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,3)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,3)*(2.0f*q0*q1 - 2.0f*q2*q3) + P(2,3)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P(21,18) + P(0,18)*SH_MAG[1] - P(1,18)*SH_MAG[2] + P(3,18)*SH_MAG[0] + P(18,18)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,18)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,18)*(2.0f*q0*q1 - 2.0f*q2*q3) + P(2,18)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(2,21)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2));
// calculate Z axis observation jacobians
H_MAG.setZero();
H_MAG(0) = SH_MAG[1];
H_MAG(1) = -SH_MAG[2];
H_MAG(2) = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
H_MAG(3) = SH_MAG[0];
H_MAG(16) = 2.0f*q0*q2 + 2.0f*q1*q3;
H_MAG(17) = 2.0f*q2*q3 - 2.0f*q0*q1;
H_MAG(18) = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
H_MAG(21) = 1.0f;
// Calculate Z axis Kalman gains
float SK_MZ[5];
SK_MZ[0] = 1.0f / mag_innov_var;
SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
SK_MZ[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
SK_MZ[3] = 2.0f*q0*q1 - 2.0f*q2*q3;
SK_MZ[4] = 2.0f*q0*q2 + 2.0f*q1*q3;
if (update_all_states) {
Kfusion(0) = SK_MZ[0]*(P(0,21) + P(0,0)*SH_MAG[1] - P(0,1)*SH_MAG[2] + P(0,3)*SH_MAG[0] + P(0,2)*SK_MZ[2] + P(0,18)*SK_MZ[1] + P(0,16)*SK_MZ[4] - P(0,17)*SK_MZ[3]);
Kfusion(1) = SK_MZ[0]*(P(1,21) + P(1,0)*SH_MAG[1] - P(1,1)*SH_MAG[2] + P(1,3)*SH_MAG[0] + P(1,2)*SK_MZ[2] + P(1,18)*SK_MZ[1] + P(1,16)*SK_MZ[4] - P(1,17)*SK_MZ[3]);
Kfusion(2) = SK_MZ[0]*(P(2,21) + P(2,0)*SH_MAG[1] - P(2,1)*SH_MAG[2] + P(2,3)*SH_MAG[0] + P(2,2)*SK_MZ[2] + P(2,18)*SK_MZ[1] + P(2,16)*SK_MZ[4] - P(2,17)*SK_MZ[3]);
Kfusion(3) = SK_MZ[0]*(P(3,21) + P(3,0)*SH_MAG[1] - P(3,1)*SH_MAG[2] + P(3,3)*SH_MAG[0] + P(3,2)*SK_MZ[2] + P(3,18)*SK_MZ[1] + P(3,16)*SK_MZ[4] - P(3,17)*SK_MZ[3]);
Kfusion(4) = SK_MZ[0]*(P(4,21) + P(4,0)*SH_MAG[1] - P(4,1)*SH_MAG[2] + P(4,3)*SH_MAG[0] + P(4,2)*SK_MZ[2] + P(4,18)*SK_MZ[1] + P(4,16)*SK_MZ[4] - P(4,17)*SK_MZ[3]);
Kfusion(5) = SK_MZ[0]*(P(5,21) + P(5,0)*SH_MAG[1] - P(5,1)*SH_MAG[2] + P(5,3)*SH_MAG[0] + P(5,2)*SK_MZ[2] + P(5,18)*SK_MZ[1] + P(5,16)*SK_MZ[4] - P(5,17)*SK_MZ[3]);
Kfusion(6) = SK_MZ[0]*(P(6,21) + P(6,0)*SH_MAG[1] - P(6,1)*SH_MAG[2] + P(6,3)*SH_MAG[0] + P(6,2)*SK_MZ[2] + P(6,18)*SK_MZ[1] + P(6,16)*SK_MZ[4] - P(6,17)*SK_MZ[3]);
Kfusion(7) = SK_MZ[0]*(P(7,21) + P(7,0)*SH_MAG[1] - P(7,1)*SH_MAG[2] + P(7,3)*SH_MAG[0] + P(7,2)*SK_MZ[2] + P(7,18)*SK_MZ[1] + P(7,16)*SK_MZ[4] - P(7,17)*SK_MZ[3]);
Kfusion(8) = SK_MZ[0]*(P(8,21) + P(8,0)*SH_MAG[1] - P(8,1)*SH_MAG[2] + P(8,3)*SH_MAG[0] + P(8,2)*SK_MZ[2] + P(8,18)*SK_MZ[1] + P(8,16)*SK_MZ[4] - P(8,17)*SK_MZ[3]);
Kfusion(9) = SK_MZ[0]*(P(9,21) + P(9,0)*SH_MAG[1] - P(9,1)*SH_MAG[2] + P(9,3)*SH_MAG[0] + P(9,2)*SK_MZ[2] + P(9,18)*SK_MZ[1] + P(9,16)*SK_MZ[4] - P(9,17)*SK_MZ[3]);
Kfusion(10) = SK_MZ[0]*(P(10,21) + P(10,0)*SH_MAG[1] - P(10,1)*SH_MAG[2] + P(10,3)*SH_MAG[0] + P(10,2)*SK_MZ[2] + P(10,18)*SK_MZ[1] + P(10,16)*SK_MZ[4] - P(10,17)*SK_MZ[3]);
Kfusion(11) = SK_MZ[0]*(P(11,21) + P(11,0)*SH_MAG[1] - P(11,1)*SH_MAG[2] + P(11,3)*SH_MAG[0] + P(11,2)*SK_MZ[2] + P(11,18)*SK_MZ[1] + P(11,16)*SK_MZ[4] - P(11,17)*SK_MZ[3]);
Kfusion(12) = SK_MZ[0]*(P(12,21) + P(12,0)*SH_MAG[1] - P(12,1)*SH_MAG[2] + P(12,3)*SH_MAG[0] + P(12,2)*SK_MZ[2] + P(12,18)*SK_MZ[1] + P(12,16)*SK_MZ[4] - P(12,17)*SK_MZ[3]);
Kfusion(13) = SK_MZ[0]*(P(13,21) + P(13,0)*SH_MAG[1] - P(13,1)*SH_MAG[2] + P(13,3)*SH_MAG[0] + P(13,2)*SK_MZ[2] + P(13,18)*SK_MZ[1] + P(13,16)*SK_MZ[4] - P(13,17)*SK_MZ[3]);
Kfusion(14) = SK_MZ[0]*(P(14,21) + P(14,0)*SH_MAG[1] - P(14,1)*SH_MAG[2] + P(14,3)*SH_MAG[0] + P(14,2)*SK_MZ[2] + P(14,18)*SK_MZ[1] + P(14,16)*SK_MZ[4] - P(14,17)*SK_MZ[3]);
Kfusion(15) = SK_MZ[0]*(P(15,21) + P(15,0)*SH_MAG[1] - P(15,1)*SH_MAG[2] + P(15,3)*SH_MAG[0] + P(15,2)*SK_MZ[2] + P(15,18)*SK_MZ[1] + P(15,16)*SK_MZ[4] - P(15,17)*SK_MZ[3]);
Kfusion(22) = SK_MZ[0]*(P(22,21) + P(22,0)*SH_MAG[1] - P(22,1)*SH_MAG[2] + P(22,3)*SH_MAG[0] + P(22,2)*SK_MZ[2] + P(22,18)*SK_MZ[1] + P(22,16)*SK_MZ[4] - P(22,17)*SK_MZ[3]);
Kfusion(23) = SK_MZ[0]*(P(23,21) + P(23,0)*SH_MAG[1] - P(23,1)*SH_MAG[2] + P(23,3)*SH_MAG[0] + P(23,2)*SK_MZ[2] + P(23,18)*SK_MZ[1] + P(23,16)*SK_MZ[4] - P(23,17)*SK_MZ[3]);
}
Kfusion(16) = SK_MZ[0]*(P(16,21) + P(16,0)*SH_MAG[1] - P(16,1)*SH_MAG[2] + P(16,3)*SH_MAG[0] + P(16,2)*SK_MZ[2] + P(16,18)*SK_MZ[1] + P(16,16)*SK_MZ[4] - P(16,17)*SK_MZ[3]);
Kfusion(17) = SK_MZ[0]*(P(17,21) + P(17,0)*SH_MAG[1] - P(17,1)*SH_MAG[2] + P(17,3)*SH_MAG[0] + P(17,2)*SK_MZ[2] + P(17,18)*SK_MZ[1] + P(17,16)*SK_MZ[4] - P(17,17)*SK_MZ[3]);
Kfusion(18) = SK_MZ[0]*(P(18,21) + P(18,0)*SH_MAG[1] - P(18,1)*SH_MAG[2] + P(18,3)*SH_MAG[0] + P(18,2)*SK_MZ[2] + P(18,18)*SK_MZ[1] + P(18,16)*SK_MZ[4] - P(18,17)*SK_MZ[3]);
Kfusion(19) = SK_MZ[0]*(P(19,21) + P(19,0)*SH_MAG[1] - P(19,1)*SH_MAG[2] + P(19,3)*SH_MAG[0] + P(19,2)*SK_MZ[2] + P(19,18)*SK_MZ[1] + P(19,16)*SK_MZ[4] - P(19,17)*SK_MZ[3]);
Kfusion(20) = SK_MZ[0]*(P(20,21) + P(20,0)*SH_MAG[1] - P(20,1)*SH_MAG[2] + P(20,3)*SH_MAG[0] + P(20,2)*SK_MZ[2] + P(20,18)*SK_MZ[1] + P(20,16)*SK_MZ[4] - P(20,17)*SK_MZ[3]);
Kfusion(21) = SK_MZ[0]*(P(21,21) + P(21,0)*SH_MAG[1] - P(21,1)*SH_MAG[2] + P(21,3)*SH_MAG[0] + P(21,2)*SK_MZ[2] + P(21,18)*SK_MZ[1] + P(21,16)*SK_MZ[4] - P(21,17)*SK_MZ[3]);
// find largest observation variance difference as a fraction of the matlab value
float max_diff_fraction = 0.0f;
int max_row;
float max_old, max_new;
for (int row=0; row<24; row++) {
float diff_fraction;
if (H_MAG(row) != 0.0f) {
diff_fraction = fabsf(Hfusion_sympy[row] - H_MAG(row)) / fabsf(H_MAG(row));
} else if (Hfusion_sympy[row] != 0.0f) {
diff_fraction = fabsf(Hfusion_sympy[row] - H_MAG(row)) / fabsf(Hfusion_sympy[row]);
} else {
diff_fraction = 0.0f;
}
if (diff_fraction > max_diff_fraction) {
max_diff_fraction = diff_fraction;
max_row = row;
max_old = H_MAG(row);
max_new = Hfusion_sympy[row];
}
}
if (max_diff_fraction > 1e-5f) {
printf("Fail: Magnetomer Z axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
} else {
printf("Pass: Magnetomer Z axis Hfusion max diff fraction = %e\n",max_diff_fraction);
}
// find largest Kalman gain difference as a fraction of the matlab value
max_diff_fraction = 0.0f;
for (int row=0; row<4; row++) {
float diff_fraction;
if (Kfusion(row) != 0.0f) {
diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion(row)) / fabsf(Kfusion(row));
} else if (Kfusion_sympy(row) != 0.0f) {
diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion(row)) / fabsf(Kfusion_sympy(row));
} else {
diff_fraction = 0.0f;
}
if (diff_fraction > max_diff_fraction) {
max_diff_fraction = diff_fraction;
max_row = row;
max_old = Kfusion(row);
max_new = Kfusion_sympy(row);
}
}
if (max_diff_fraction > 1e-5f) {
printf("Fail: Magnetomer Z axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
} else {
printf("Pass: Magnetomer Z axis Kfusion max diff fraction = %e\n",max_diff_fraction);
}
}
return 0;
}

210
src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated.cpp
View File

@ -1,210 +0,0 @@
// Axis 0 equations
// Sub Expressions
const float HKX0 = -magD*q2 + magE*q3 + magN*q0;
const float HKX1 = magD*q3 + magE*q2 + magN*q1;
const float HKX2 = magD*q0 - magE*q1 + magN*q2;
const float HKX3 = magD*q1 + magE*q0 - magN*q3;
const float HKX4 = (q0)*(q0) + (q1)*(q1) - (q2)*(q2) - (q3)*(q3);
const float HKX5 = q0*q3 + q1*q2;
const float HKX6 = q0*q2 - q1*q3;
const float HKX7 = 2*HKX5;
const float HKX8 = 2*HKX6;
const float HKX9 = 2*HKX1;
const float HKX10 = 2*HKX0;
const float HKX11 = 2*HKX2;
const float HKX12 = 2*HKX3;
const float HKX13 = HKX10*P(0,0) - HKX11*P(0,2) + HKX12*P(0,3) + HKX4*P(0,16) + HKX7*P(0,17) - HKX8*P(0,18) + HKX9*P(0,1) + P(0,19);
const float HKX14 = HKX10*P(0,16) - HKX11*P(2,16) + HKX12*P(3,16) + HKX4*P(16,16) + HKX7*P(16,17) - HKX8*P(16,18) + HKX9*P(1,16) + P(16,19);
const float HKX15 = HKX10*P(0,18) - HKX11*P(2,18) + HKX12*P(3,18) + HKX4*P(16,18) + HKX7*P(17,18) - HKX8*P(18,18) + HKX9*P(1,18) + P(18,19);
const float HKX16 = HKX10*P(0,2) - HKX11*P(2,2) + HKX12*P(2,3) + HKX4*P(2,16) + HKX7*P(2,17) - HKX8*P(2,18) + HKX9*P(1,2) + P(2,19);
const float HKX17 = HKX10*P(0,17) - HKX11*P(2,17) + HKX12*P(3,17) + HKX4*P(16,17) + HKX7*P(17,17) - HKX8*P(17,18) + HKX9*P(1,17) + P(17,19);
const float HKX18 = HKX10*P(0,3) - HKX11*P(2,3) + HKX12*P(3,3) + HKX4*P(3,16) + HKX7*P(3,17) - HKX8*P(3,18) + HKX9*P(1,3) + P(3,19);
const float HKX19 = HKX10*P(0,1) - HKX11*P(1,2) + HKX12*P(1,3) + HKX4*P(1,16) + HKX7*P(1,17) - HKX8*P(1,18) + HKX9*P(1,1) + P(1,19);
const float HKX20 = HKX10*P(0,19) - HKX11*P(2,19) + HKX12*P(3,19) + HKX4*P(16,19) + HKX7*P(17,19) - HKX8*P(18,19) + HKX9*P(1,19) + P(19,19);
const float HKX21 = 1.0F/(HKX10*HKX13 - HKX11*HKX16 + HKX12*HKX18 + HKX14*HKX4 - HKX15*HKX8 + HKX17*HKX7 + HKX19*HKX9 + HKX20 + R_MAG);
// Observation Jacobians
Hfusion.at<0>() = 2*HKX0;
Hfusion.at<1>() = 2*HKX1;
Hfusion.at<2>() = -2*HKX2;
Hfusion.at<3>() = 2*HKX3;
Hfusion.at<16>() = HKX4;
Hfusion.at<17>() = 2*HKX5;
Hfusion.at<18>() = -2*HKX6;
Hfusion.at<19>() = 1;
// Kalman gains
Kfusion(0) = HKX13*HKX21;
Kfusion(1) = HKX19*HKX21;
Kfusion(2) = HKX16*HKX21;
Kfusion(3) = HKX18*HKX21;
Kfusion(4) = HKX21*(HKX10*P(0,4) - HKX11*P(2,4) + HKX12*P(3,4) + HKX4*P(4,16) + HKX7*P(4,17) - HKX8*P(4,18) + HKX9*P(1,4) + P(4,19));
Kfusion(5) = HKX21*(HKX10*P(0,5) - HKX11*P(2,5) + HKX12*P(3,5) + HKX4*P(5,16) + HKX7*P(5,17) - HKX8*P(5,18) + HKX9*P(1,5) + P(5,19));
Kfusion(6) = HKX21*(HKX10*P(0,6) - HKX11*P(2,6) + HKX12*P(3,6) + HKX4*P(6,16) + HKX7*P(6,17) - HKX8*P(6,18) + HKX9*P(1,6) + P(6,19));
Kfusion(7) = HKX21*(HKX10*P(0,7) - HKX11*P(2,7) + HKX12*P(3,7) + HKX4*P(7,16) + HKX7*P(7,17) - HKX8*P(7,18) + HKX9*P(1,7) + P(7,19));
Kfusion(8) = HKX21*(HKX10*P(0,8) - HKX11*P(2,8) + HKX12*P(3,8) + HKX4*P(8,16) + HKX7*P(8,17) - HKX8*P(8,18) + HKX9*P(1,8) + P(8,19));
Kfusion(9) = HKX21*(HKX10*P(0,9) - HKX11*P(2,9) + HKX12*P(3,9) + HKX4*P(9,16) + HKX7*P(9,17) - HKX8*P(9,18) + HKX9*P(1,9) + P(9,19));
Kfusion(10) = HKX21*(HKX10*P(0,10) - HKX11*P(2,10) + HKX12*P(3,10) + HKX4*P(10,16) + HKX7*P(10,17) - HKX8*P(10,18) + HKX9*P(1,10) + P(10,19));
Kfusion(11) = HKX21*(HKX10*P(0,11) - HKX11*P(2,11) + HKX12*P(3,11) + HKX4*P(11,16) + HKX7*P(11,17) - HKX8*P(11,18) + HKX9*P(1,11) + P(11,19));
Kfusion(12) = HKX21*(HKX10*P(0,12) - HKX11*P(2,12) + HKX12*P(3,12) + HKX4*P(12,16) + HKX7*P(12,17) - HKX8*P(12,18) + HKX9*P(1,12) + P(12,19));
Kfusion(13) = HKX21*(HKX10*P(0,13) - HKX11*P(2,13) + HKX12*P(3,13) + HKX4*P(13,16) + HKX7*P(13,17) - HKX8*P(13,18) + HKX9*P(1,13) + P(13,19));
Kfusion(14) = HKX21*(HKX10*P(0,14) - HKX11*P(2,14) + HKX12*P(3,14) + HKX4*P(14,16) + HKX7*P(14,17) - HKX8*P(14,18) + HKX9*P(1,14) + P(14,19));
Kfusion(15) = HKX21*(HKX10*P(0,15) - HKX11*P(2,15) + HKX12*P(3,15) + HKX4*P(15,16) + HKX7*P(15,17) - HKX8*P(15,18) + HKX9*P(1,15) + P(15,19));
Kfusion(16) = HKX14*HKX21;
Kfusion(17) = HKX17*HKX21;
Kfusion(18) = HKX15*HKX21;
Kfusion(19) = HKX20*HKX21;
Kfusion(20) = HKX21*(HKX10*P(0,20) - HKX11*P(2,20) + HKX12*P(3,20) + HKX4*P(16,20) + HKX7*P(17,20) - HKX8*P(18,20) + HKX9*P(1,20) + P(19,20));
Kfusion(21) = HKX21*(HKX10*P(0,21) - HKX11*P(2,21) + HKX12*P(3,21) + HKX4*P(16,21) + HKX7*P(17,21) - HKX8*P(18,21) + HKX9*P(1,21) + P(19,21));
Kfusion(22) = HKX21*(HKX10*P(0,22) - HKX11*P(2,22) + HKX12*P(3,22) + HKX4*P(16,22) + HKX7*P(17,22) - HKX8*P(18,22) + HKX9*P(1,22) + P(19,22));
Kfusion(23) = HKX21*(HKX10*P(0,23) - HKX11*P(2,23) + HKX12*P(3,23) + HKX4*P(16,23) + HKX7*P(17,23) - HKX8*P(18,23) + HKX9*P(1,23) + P(19,23));
// Predicted observation
// Innovation variance
// Axis 1 equations
// Sub Expressions
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 + magE*q3 + magN*q0;
const float HKY4 = q0*q3 - q1*q2;
const float HKY5 = (q0)*(q0) - (q1)*(q1) + (q2)*(q2) - (q3)*(q3);
const float HKY6 = q0*q1 + q2*q3;
const float HKY7 = 2*HKY6;
const float HKY8 = 2*HKY4;
const float HKY9 = 2*HKY2;
const float HKY10 = 2*HKY0;
const float HKY11 = 2*HKY1;
const float HKY12 = 2*HKY3;
const float HKY13 = HKY10*P(0,0) + HKY11*P(0,1) - HKY12*P(0,3) + HKY5*P(0,17) + HKY7*P(0,18) - HKY8*P(0,16) + HKY9*P(0,2) + P(0,20);
const float HKY14 = HKY10*P(0,17) + HKY11*P(1,17) - HKY12*P(3,17) + HKY5*P(17,17) + HKY7*P(17,18) - HKY8*P(16,17) + HKY9*P(2,17) + P(17,20);
const float HKY15 = HKY10*P(0,16) + HKY11*P(1,16) - HKY12*P(3,16) + HKY5*P(16,17) + HKY7*P(16,18) - HKY8*P(16,16) + HKY9*P(2,16) + P(16,20);
const float HKY16 = HKY10*P(0,3) + HKY11*P(1,3) - HKY12*P(3,3) + HKY5*P(3,17) + HKY7*P(3,18) - HKY8*P(3,16) + HKY9*P(2,3) + P(3,20);
const float HKY17 = HKY10*P(0,18) + HKY11*P(1,18) - HKY12*P(3,18) + HKY5*P(17,18) + HKY7*P(18,18) - HKY8*P(16,18) + HKY9*P(2,18) + P(18,20);
const float HKY18 = HKY10*P(0,1) + HKY11*P(1,1) - HKY12*P(1,3) + HKY5*P(1,17) + HKY7*P(1,18) - HKY8*P(1,16) + HKY9*P(1,2) + P(1,20);
const float HKY19 = HKY10*P(0,2) + HKY11*P(1,2) - HKY12*P(2,3) + HKY5*P(2,17) + HKY7*P(2,18) - HKY8*P(2,16) + HKY9*P(2,2) + P(2,20);
const float HKY20 = HKY10*P(0,20) + HKY11*P(1,20) - HKY12*P(3,20) + HKY5*P(17,20) + HKY7*P(18,20) - HKY8*P(16,20) + HKY9*P(2,20) + P(20,20);
const float HKY21 = 1.0F/(HKY10*HKY13 + HKY11*HKY18 - HKY12*HKY16 + HKY14*HKY5 - HKY15*HKY8 + HKY17*HKY7 + HKY19*HKY9 + HKY20 + R_MAG);
// Observation Jacobians
Hfusion.at<0>() = 2*HKY0;
Hfusion.at<1>() = 2*HKY1;
Hfusion.at<2>() = 2*HKY2;
Hfusion.at<3>() = -2*HKY3;
Hfusion.at<16>() = -2*HKY4;
Hfusion.at<17>() = HKY5;
Hfusion.at<18>() = 2*HKY6;
Hfusion.at<20>() = 1;
// Kalman gains
Kfusion(0) = HKY13*HKY21;
Kfusion(1) = HKY18*HKY21;
Kfusion(2) = HKY19*HKY21;
Kfusion(3) = HKY16*HKY21;
Kfusion(4) = HKY21*(HKY10*P(0,4) + HKY11*P(1,4) - HKY12*P(3,4) + HKY5*P(4,17) + HKY7*P(4,18) - HKY8*P(4,16) + HKY9*P(2,4) + P(4,20));
Kfusion(5) = HKY21*(HKY10*P(0,5) + HKY11*P(1,5) - HKY12*P(3,5) + HKY5*P(5,17) + HKY7*P(5,18) - HKY8*P(5,16) + HKY9*P(2,5) + P(5,20));
Kfusion(6) = HKY21*(HKY10*P(0,6) + HKY11*P(1,6) - HKY12*P(3,6) + HKY5*P(6,17) + HKY7*P(6,18) - HKY8*P(6,16) + HKY9*P(2,6) + P(6,20));
Kfusion(7) = HKY21*(HKY10*P(0,7) + HKY11*P(1,7) - HKY12*P(3,7) + HKY5*P(7,17) + HKY7*P(7,18) - HKY8*P(7,16) + HKY9*P(2,7) + P(7,20));
Kfusion(8) = HKY21*(HKY10*P(0,8) + HKY11*P(1,8) - HKY12*P(3,8) + HKY5*P(8,17) + HKY7*P(8,18) - HKY8*P(8,16) + HKY9*P(2,8) + P(8,20));
Kfusion(9) = HKY21*(HKY10*P(0,9) + HKY11*P(1,9) - HKY12*P(3,9) + HKY5*P(9,17) + HKY7*P(9,18) - HKY8*P(9,16) + HKY9*P(2,9) + P(9,20));
Kfusion(10) = HKY21*(HKY10*P(0,10) + HKY11*P(1,10) - HKY12*P(3,10) + HKY5*P(10,17) + HKY7*P(10,18) - HKY8*P(10,16) + HKY9*P(2,10) + P(10,20));
Kfusion(11) = HKY21*(HKY10*P(0,11) + HKY11*P(1,11) - HKY12*P(3,11) + HKY5*P(11,17) + HKY7*P(11,18) - HKY8*P(11,16) + HKY9*P(2,11) + P(11,20));
Kfusion(12) = HKY21*(HKY10*P(0,12) + HKY11*P(1,12) - HKY12*P(3,12) + HKY5*P(12,17) + HKY7*P(12,18) - HKY8*P(12,16) + HKY9*P(2,12) + P(12,20));
Kfusion(13) = HKY21*(HKY10*P(0,13) + HKY11*P(1,13) - HKY12*P(3,13) + HKY5*P(13,17) + HKY7*P(13,18) - HKY8*P(13,16) + HKY9*P(2,13) + P(13,20));
Kfusion(14) = HKY21*(HKY10*P(0,14) + HKY11*P(1,14) - HKY12*P(3,14) + HKY5*P(14,17) + HKY7*P(14,18) - HKY8*P(14,16) + HKY9*P(2,14) + P(14,20));
Kfusion(15) = HKY21*(HKY10*P(0,15) + HKY11*P(1,15) - HKY12*P(3,15) + HKY5*P(15,17) + HKY7*P(15,18) - HKY8*P(15,16) + HKY9*P(2,15) + P(15,20));
Kfusion(16) = HKY15*HKY21;
Kfusion(17) = HKY14*HKY21;
Kfusion(18) = HKY17*HKY21;
Kfusion(19) = HKY21*(HKY10*P(0,19) + HKY11*P(1,19) - HKY12*P(3,19) + HKY5*P(17,19) + HKY7*P(18,19) - HKY8*P(16,19) + HKY9*P(2,19) + P(19,20));
Kfusion(20) = HKY20*HKY21;
Kfusion(21) = HKY21*(HKY10*P(0,21) + HKY11*P(1,21) - HKY12*P(3,21) + HKY5*P(17,21) + HKY7*P(18,21) - HKY8*P(16,21) + HKY9*P(2,21) + P(20,21));
Kfusion(22) = HKY21*(HKY10*P(0,22) + HKY11*P(1,22) - HKY12*P(3,22) + HKY5*P(17,22) + HKY7*P(18,22) - HKY8*P(16,22) + HKY9*P(2,22) + P(20,22));
Kfusion(23) = HKY21*(HKY10*P(0,23) + HKY11*P(1,23) - HKY12*P(3,23) + HKY5*P(17,23) + HKY7*P(18,23) - HKY8*P(16,23) + HKY9*P(2,23) + P(20,23));
// Predicted observation
// Innovation variance
// Axis 2 equations
// Sub Expressions
const float HKZ0 = magD*q0 - magE*q1 + magN*q2;
const float HKZ1 = magD*q1 + magE*q0 - magN*q3;
const float HKZ2 = -magD*q2 + magE*q3 + magN*q0;
const float HKZ3 = magD*q3 + magE*q2 + magN*q1;
const float HKZ4 = q0*q2 + q1*q3;
const float HKZ5 = q0*q1 - q2*q3;
const float HKZ6 = (q0)*(q0) - (q1)*(q1) - (q2)*(q2) + (q3)*(q3);
const float HKZ7 = 2*HKZ4;
const float HKZ8 = 2*HKZ5;
const float HKZ9 = 2*HKZ3;
const float HKZ10 = 2*HKZ0;
const float HKZ11 = 2*HKZ1;
const float HKZ12 = 2*HKZ2;
const float HKZ13 = HKZ10*P(0,0) - HKZ11*P(0,1) + HKZ12*P(0,2) + HKZ6*P(0,18) + HKZ7*P(0,16) - HKZ8*P(0,17) + HKZ9*P(0,3) + P(0,21);
const float HKZ14 = HKZ10*P(0,18) - HKZ11*P(1,18) + HKZ12*P(2,18) + HKZ6*P(18,18) + HKZ7*P(16,18) - HKZ8*P(17,18) + HKZ9*P(3,18) + P(18,21);
const float HKZ15 = HKZ10*P(0,17) - HKZ11*P(1,17) + HKZ12*P(2,17) + HKZ6*P(17,18) + HKZ7*P(16,17) - HKZ8*P(17,17) + HKZ9*P(3,17) + P(17,21);
const float HKZ16 = HKZ10*P(0,1) - HKZ11*P(1,1) + HKZ12*P(1,2) + HKZ6*P(1,18) + HKZ7*P(1,16) - HKZ8*P(1,17) + HKZ9*P(1,3) + P(1,21);
const float HKZ17 = HKZ10*P(0,16) - HKZ11*P(1,16) + HKZ12*P(2,16) + HKZ6*P(16,18) + HKZ7*P(16,16) - HKZ8*P(16,17) + HKZ9*P(3,16) + P(16,21);
const float HKZ18 = HKZ10*P(0,3) - HKZ11*P(1,3) + HKZ12*P(2,3) + HKZ6*P(3,18) + HKZ7*P(3,16) - HKZ8*P(3,17) + HKZ9*P(3,3) + P(3,21);
const float HKZ19 = HKZ10*P(0,2) - HKZ11*P(1,2) + HKZ12*P(2,2) + HKZ6*P(2,18) + HKZ7*P(2,16) - HKZ8*P(2,17) + HKZ9*P(2,3) + P(2,21);
const float HKZ20 = HKZ10*P(0,21) - HKZ11*P(1,21) + HKZ12*P(2,21) + HKZ6*P(18,21) + HKZ7*P(16,21) - HKZ8*P(17,21) + HKZ9*P(3,21) + P(21,21);
const float HKZ21 = 1.0F/(HKZ10*HKZ13 - HKZ11*HKZ16 + HKZ12*HKZ19 + HKZ14*HKZ6 - HKZ15*HKZ8 + HKZ17*HKZ7 + HKZ18*HKZ9 + HKZ20 + R_MAG);
// Observation Jacobians
Hfusion.at<0>() = 2*HKZ0;
Hfusion.at<1>() = -2*HKZ1;
Hfusion.at<2>() = 2*HKZ2;
Hfusion.at<3>() = 2*HKZ3;
Hfusion.at<16>() = 2*HKZ4;
Hfusion.at<17>() = -2*HKZ5;
Hfusion.at<18>() = HKZ6;
Hfusion.at<21>() = 1;
// Kalman gains
Kfusion(0) = HKZ13*HKZ21;
Kfusion(1) = HKZ16*HKZ21;
Kfusion(2) = HKZ19*HKZ21;
Kfusion(3) = HKZ18*HKZ21;
Kfusion(4) = HKZ21*(HKZ10*P(0,4) - HKZ11*P(1,4) + HKZ12*P(2,4) + HKZ6*P(4,18) + HKZ7*P(4,16) - HKZ8*P(4,17) + HKZ9*P(3,4) + P(4,21));
Kfusion(5) = HKZ21*(HKZ10*P(0,5) - HKZ11*P(1,5) + HKZ12*P(2,5) + HKZ6*P(5,18) + HKZ7*P(5,16) - HKZ8*P(5,17) + HKZ9*P(3,5) + P(5,21));
Kfusion(6) = HKZ21*(HKZ10*P(0,6) - HKZ11*P(1,6) + HKZ12*P(2,6) + HKZ6*P(6,18) + HKZ7*P(6,16) - HKZ8*P(6,17) + HKZ9*P(3,6) + P(6,21));
Kfusion(7) = HKZ21*(HKZ10*P(0,7) - HKZ11*P(1,7) + HKZ12*P(2,7) + HKZ6*P(7,18) + HKZ7*P(7,16) - HKZ8*P(7,17) + HKZ9*P(3,7) + P(7,21));
Kfusion(8) = HKZ21*(HKZ10*P(0,8) - HKZ11*P(1,8) + HKZ12*P(2,8) + HKZ6*P(8,18) + HKZ7*P(8,16) - HKZ8*P(8,17) + HKZ9*P(3,8) + P(8,21));
Kfusion(9) = HKZ21*(HKZ10*P(0,9) - HKZ11*P(1,9) + HKZ12*P(2,9) + HKZ6*P(9,18) + HKZ7*P(9,16) - HKZ8*P(9,17) + HKZ9*P(3,9) + P(9,21));
Kfusion(10) = HKZ21*(HKZ10*P(0,10) - HKZ11*P(1,10) + HKZ12*P(2,10) + HKZ6*P(10,18) + HKZ7*P(10,16) - HKZ8*P(10,17) + HKZ9*P(3,10) + P(10,21));
Kfusion(11) = HKZ21*(HKZ10*P(0,11) - HKZ11*P(1,11) + HKZ12*P(2,11) + HKZ6*P(11,18) + HKZ7*P(11,16) - HKZ8*P(11,17) + HKZ9*P(3,11) + P(11,21));
Kfusion(12) = HKZ21*(HKZ10*P(0,12) - HKZ11*P(1,12) + HKZ12*P(2,12) + HKZ6*P(12,18) + HKZ7*P(12,16) - HKZ8*P(12,17) + HKZ9*P(3,12) + P(12,21));
Kfusion(13) = HKZ21*(HKZ10*P(0,13) - HKZ11*P(1,13) + HKZ12*P(2,13) + HKZ6*P(13,18) + HKZ7*P(13,16) - HKZ8*P(13,17) + HKZ9*P(3,13) + P(13,21));
Kfusion(14) = HKZ21*(HKZ10*P(0,14) - HKZ11*P(1,14) + HKZ12*P(2,14) + HKZ6*P(14,18) + HKZ7*P(14,16) - HKZ8*P(14,17) + HKZ9*P(3,14) + P(14,21));
Kfusion(15) = HKZ21*(HKZ10*P(0,15) - HKZ11*P(1,15) + HKZ12*P(2,15) + HKZ6*P(15,18) + HKZ7*P(15,16) - HKZ8*P(15,17) + HKZ9*P(3,15) + P(15,21));
Kfusion(16) = HKZ17*HKZ21;
Kfusion(17) = HKZ15*HKZ21;
Kfusion(18) = HKZ14*HKZ21;
Kfusion(19) = HKZ21*(HKZ10*P(0,19) - HKZ11*P(1,19) + HKZ12*P(2,19) + HKZ6*P(18,19) + HKZ7*P(16,19) - HKZ8*P(17,19) + HKZ9*P(3,19) + P(19,21));
Kfusion(20) = HKZ21*(HKZ10*P(0,20) - HKZ11*P(1,20) + HKZ12*P(2,20) + HKZ6*P(18,20) + HKZ7*P(16,20) - HKZ8*P(17,20) + HKZ9*P(3,20) + P(20,21));
Kfusion(21) = HKZ20*HKZ21;
Kfusion(22) = HKZ21*(HKZ10*P(0,22) - HKZ11*P(1,22) + HKZ12*P(2,22) + HKZ6*P(18,22) + HKZ7*P(16,22) - HKZ8*P(17,22) + HKZ9*P(3,22) + P(21,22));
Kfusion(23) = HKZ21*(HKZ10*P(0,23) - HKZ11*P(1,23) + HKZ12*P(2,23) + HKZ6*P(18,23) + HKZ7*P(16,23) - HKZ8*P(17,23) + HKZ9*P(3,23) + P(21,23));
// Predicted observation
// Innovation variance

193
src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated_alt.cpp
View File

@ -1,193 +0,0 @@
// Sub Expressions
const float HK0 = -magD*q2 + magE*q3 + magN*q0;
const float HK1 = 2*HK0;
const float HK2 = magD*q3 + magE*q2 + magN*q1;
const float HK3 = 2*HK2;
const float HK4 = magD*q0 - magE*q1 + magN*q2;
const float HK5 = magD*q1 + magE*q0 - magN*q3;
const float HK6 = 2*HK5;
const float HK7 = (q1)*(q1);
const float HK8 = (q2)*(q2);
const float HK9 = -HK8;
const float HK10 = (q0)*(q0);
const float HK11 = (q3)*(q3);
const float HK12 = HK10 - HK11;
const float HK13 = HK12 + HK7 + HK9;
const float HK14 = q0*q3;
const float HK15 = HK14 + q1*q2;
const float HK16 = q0*q2;
const float HK17 = HK16 - q1*q3;
const float HK18 = 2*HK15;
const float HK19 = 2*HK17;
const float HK20 = 2*HK2;
const float HK21 = 2*HK0;
const float HK22 = 2*HK4;
const float HK23 = HK22*P(0,2);
const float HK24 = 2*HK5;
const float HK25 = HK24*P(0,3);
const float HK26 = HK13*P(0,16) + HK18*P(0,17) - HK19*P(0,18) + HK20*P(0,1) + HK21*P(0,0) - HK23 + HK25 + P(0,19);
const float HK27 = HK13*P(16,16) + HK18*P(16,17) - HK19*P(16,18) + HK20*P(1,16) + HK21*P(0,16) - HK22*P(2,16) + HK24*P(3,16) + P(16,19);
const float HK28 = HK13*P(16,18) + HK18*P(17,18) - HK19*P(18,18) + HK20*P(1,18) + HK21*P(0,18) - HK22*P(2,18) + HK24*P(3,18) + P(18,19);
const float HK29 = HK20*P(1,2);
const float HK30 = HK21*P(0,2);
const float HK31 = HK13*P(2,16) + HK18*P(2,17) - HK19*P(2,18) - HK22*P(2,2) + HK24*P(2,3) + HK29 + HK30 + P(2,19);
const float HK32 = HK13*P(16,17) + HK18*P(17,17) - HK19*P(17,18) + HK20*P(1,17) + HK21*P(0,17) - HK22*P(2,17) + HK24*P(3,17) + P(17,19);
const float HK33 = HK20*P(1,3);
const float HK34 = HK21*P(0,3);
const float HK35 = HK13*P(3,16) + HK18*P(3,17) - HK19*P(3,18) - HK22*P(2,3) + HK24*P(3,3) + HK33 + HK34 + P(3,19);
const float HK36 = HK22*P(1,2);
const float HK37 = HK24*P(1,3);
const float HK38 = HK13*P(1,16) + HK18*P(1,17) - HK19*P(1,18) + HK20*P(1,1) + HK21*P(0,1) - HK36 + HK37 + P(1,19);
const float HK39 = HK13*P(16,19) + HK18*P(17,19) - HK19*P(18,19) + HK20*P(1,19) + HK21*P(0,19) - HK22*P(2,19) + HK24*P(3,19) + P(19,19);
const float HK40 = 1.0F/(HK13*HK27 + HK18*HK32 - HK19*HK28 + HK20*HK38 + HK21*HK26 - HK22*HK31 + HK24*HK35 + HK39 + R_MAG);
const float HK41 = 2*HK4;
const float HK42 = HK14 - q1*q2;
const float HK43 = -HK7;
const float HK44 = HK12 + HK43 + HK8;
const float HK45 = q0*q1;
const float HK46 = HK45 + q2*q3;
const float HK47 = 2*HK46;
const float HK48 = 2*HK42;
const float HK49 = HK22*P(0,1);
const float HK50 = HK20*P(0,2) + HK24*P(0,0) - HK34 + HK44*P(0,17) + HK47*P(0,18) - HK48*P(0,16) + HK49 + P(0,20);
const float HK51 = HK20*P(2,17) - HK21*P(3,17) + HK22*P(1,17) + HK24*P(0,17) + HK44*P(17,17) + HK47*P(17,18) - HK48*P(16,17) + P(17,20);
const float HK52 = HK20*P(2,16) - HK21*P(3,16) + HK22*P(1,16) + HK24*P(0,16) + HK44*P(16,17) + HK47*P(16,18) - HK48*P(16,16) + P(16,20);
const float HK53 = HK20*P(2,3);
const float HK54 = -HK21*P(3,3) + HK22*P(1,3) + HK25 + HK44*P(3,17) + HK47*P(3,18) - HK48*P(3,16) + HK53 + P(3,20);
const float HK55 = HK20*P(2,18) - HK21*P(3,18) + HK22*P(1,18) + HK24*P(0,18) + HK44*P(17,18) + HK47*P(18,18) - HK48*P(16,18) + P(18,20);
const float HK56 = HK24*P(0,1);
const float HK57 = -HK21*P(1,3) + HK22*P(1,1) + HK29 + HK44*P(1,17) + HK47*P(1,18) - HK48*P(1,16) + HK56 + P(1,20);
const float HK58 = HK21*P(2,3);
const float HK59 = HK20*P(2,2) + HK24*P(0,2) + HK36 + HK44*P(2,17) + HK47*P(2,18) - HK48*P(2,16) - HK58 + P(2,20);
const float HK60 = HK20*P(2,20) - HK21*P(3,20) + HK22*P(1,20) + HK24*P(0,20) + HK44*P(17,20) + HK47*P(18,20) - HK48*P(16,20) + P(20,20);
const float HK61 = 1.0F/(HK20*HK59 - HK21*HK54 + HK22*HK57 + HK24*HK50 + HK44*HK51 + HK47*HK55 - HK48*HK52 + HK60 + R_MAG);
const float HK62 = HK16 + q1*q3;
const float HK63 = HK45 - q2*q3;
const float HK64 = HK10 + HK11 + HK43 + HK9;
const float HK65 = 2*HK62;
const float HK66 = 2*HK63;
const float HK67 = HK20*P(0,3) + HK22*P(0,0) + HK30 - HK56 + HK64*P(0,18) + HK65*P(0,16) - HK66*P(0,17) + P(0,21);
const float HK68 = HK20*P(3,18) + HK21*P(2,18) + HK22*P(0,18) - HK24*P(1,18) + HK64*P(18,18) + HK65*P(16,18) - HK66*P(17,18) + P(18,21);
const float HK69 = HK20*P(3,17) + HK21*P(2,17) + HK22*P(0,17) - HK24*P(1,17) + HK64*P(17,18) + HK65*P(16,17) - HK66*P(17,17) + P(17,21);
const float HK70 = HK21*P(1,2) - HK24*P(1,1) + HK33 + HK49 + HK64*P(1,18) + HK65*P(1,16) - HK66*P(1,17) + P(1,21);
const float HK71 = HK20*P(3,16) + HK21*P(2,16) + HK22*P(0,16) - HK24*P(1,16) + HK64*P(16,18) + HK65*P(16,16) - HK66*P(16,17) + P(16,21);
const float HK72 = HK20*P(3,3) + HK22*P(0,3) - HK37 + HK58 + HK64*P(3,18) + HK65*P(3,16) - HK66*P(3,17) + P(3,21);
const float HK73 = HK21*P(2,2) + HK23 - HK24*P(1,2) + HK53 + HK64*P(2,18) + HK65*P(2,16) - HK66*P(2,17) + P(2,21);
const float HK74 = HK20*P(3,21) + HK21*P(2,21) + HK22*P(0,21) - HK24*P(1,21) + HK64*P(18,21) + HK65*P(16,21) - HK66*P(17,21) + P(21,21);
const float HK75 = 1.0F/(HK20*HK72 + HK21*HK73 + HK22*HK67 - HK24*HK70 + HK64*HK68 + HK65*HK71 - HK66*HK69 + HK74 + R_MAG);
// Observation Jacobians - axis 0
Hfusion.at<0>() = HK1;
Hfusion.at<1>() = HK3;
Hfusion.at<2>() = -2*HK4;
Hfusion.at<3>() = HK6;
Hfusion.at<16>() = HK13;
Hfusion.at<17>() = 2*HK15;
Hfusion.at<18>() = -2*HK17;
Hfusion.at<19>() = 1;
// Kalman gains - axis 0
Kfusion(0) = HK26*HK40;
Kfusion(1) = HK38*HK40;
Kfusion(2) = HK31*HK40;
Kfusion(3) = HK35*HK40;
Kfusion(4) = HK40*(HK13*P(4,16) + HK18*P(4,17) - HK19*P(4,18) + HK20*P(1,4) + HK21*P(0,4) - HK22*P(2,4) + HK24*P(3,4) + P(4,19));
Kfusion(5) = HK40*(HK13*P(5,16) + HK18*P(5,17) - HK19*P(5,18) + HK20*P(1,5) + HK21*P(0,5) - HK22*P(2,5) + HK24*P(3,5) + P(5,19));
Kfusion(6) = HK40*(HK13*P(6,16) + HK18*P(6,17) - HK19*P(6,18) + HK20*P(1,6) + HK21*P(0,6) - HK22*P(2,6) + HK24*P(3,6) + P(6,19));
Kfusion(7) = HK40*(HK13*P(7,16) + HK18*P(7,17) - HK19*P(7,18) + HK20*P(1,7) + HK21*P(0,7) - HK22*P(2,7) + HK24*P(3,7) + P(7,19));
Kfusion(8) = HK40*(HK13*P(8,16) + HK18*P(8,17) - HK19*P(8,18) + HK20*P(1,8) + HK21*P(0,8) - HK22*P(2,8) + HK24*P(3,8) + P(8,19));
Kfusion(9) = HK40*(HK13*P(9,16) + HK18*P(9,17) - HK19*P(9,18) + HK20*P(1,9) + HK21*P(0,9) - HK22*P(2,9) + HK24*P(3,9) + P(9,19));
Kfusion(10) = HK40*(HK13*P(10,16) + HK18*P(10,17) - HK19*P(10,18) + HK20*P(1,10) + HK21*P(0,10) - HK22*P(2,10) + HK24*P(3,10) + P(10,19));
Kfusion(11) = HK40*(HK13*P(11,16) + HK18*P(11,17) - HK19*P(11,18) + HK20*P(1,11) + HK21*P(0,11) - HK22*P(2,11) + HK24*P(3,11) + P(11,19));
Kfusion(12) = HK40*(HK13*P(12,16) + HK18*P(12,17) - HK19*P(12,18) + HK20*P(1,12) + HK21*P(0,12) - HK22*P(2,12) + HK24*P(3,12) + P(12,19));
Kfusion(13) = HK40*(HK13*P(13,16) + HK18*P(13,17) - HK19*P(13,18) + HK20*P(1,13) + HK21*P(0,13) - HK22*P(2,13) + HK24*P(3,13) + P(13,19));
Kfusion(14) = HK40*(HK13*P(14,16) + HK18*P(14,17) - HK19*P(14,18) + HK20*P(1,14) + HK21*P(0,14) - HK22*P(2,14) + HK24*P(3,14) + P(14,19));
Kfusion(15) = HK40*(HK13*P(15,16) + HK18*P(15,17) - HK19*P(15,18) + HK20*P(1,15) + HK21*P(0,15) - HK22*P(2,15) + HK24*P(3,15) + P(15,19));
Kfusion(16) = HK27*HK40;
Kfusion(17) = HK32*HK40;
Kfusion(18) = HK28*HK40;
Kfusion(19) = HK39*HK40;
Kfusion(20) = HK40*(HK13*P(16,20) + HK18*P(17,20) - HK19*P(18,20) + HK20*P(1,20) + HK21*P(0,20) - HK22*P(2,20) + HK24*P(3,20) + P(19,20));
Kfusion(21) = HK40*(HK13*P(16,21) + HK18*P(17,21) - HK19*P(18,21) + HK20*P(1,21) + HK21*P(0,21) - HK22*P(2,21) + HK24*P(3,21) + P(19,21));
Kfusion(22) = HK40*(HK13*P(16,22) + HK18*P(17,22) - HK19*P(18,22) + HK20*P(1,22) + HK21*P(0,22) - HK22*P(2,22) + HK24*P(3,22) + P(19,22));
Kfusion(23) = HK40*(HK13*P(16,23) + HK18*P(17,23) - HK19*P(18,23) + HK20*P(1,23) + HK21*P(0,23) - HK22*P(2,23) + HK24*P(3,23) + P(19,23));
// Observation Jacobians - axis 1
Hfusion.at<0>() = HK6;
Hfusion.at<1>() = HK41;
Hfusion.at<2>() = HK3;
Hfusion.at<3>() = -2*HK0;
Hfusion.at<16>() = -2*HK42;
Hfusion.at<17>() = HK44;
Hfusion.at<18>() = 2*HK46;
Hfusion.at<20>() = 1;
// Kalman gains - axis 1
Kfusion(0) = HK50*HK61;
Kfusion(1) = HK57*HK61;
Kfusion(2) = HK59*HK61;
Kfusion(3) = HK54*HK61;
Kfusion(4) = HK61*(HK20*P(2,4) - HK21*P(3,4) + HK22*P(1,4) + HK24*P(0,4) + HK44*P(4,17) + HK47*P(4,18) - HK48*P(4,16) + P(4,20));
Kfusion(5) = HK61*(HK20*P(2,5) - HK21*P(3,5) + HK22*P(1,5) + HK24*P(0,5) + HK44*P(5,17) + HK47*P(5,18) - HK48*P(5,16) + P(5,20));
Kfusion(6) = HK61*(HK20*P(2,6) - HK21*P(3,6) + HK22*P(1,6) + HK24*P(0,6) + HK44*P(6,17) + HK47*P(6,18) - HK48*P(6,16) + P(6,20));
Kfusion(7) = HK61*(HK20*P(2,7) - HK21*P(3,7) + HK22*P(1,7) + HK24*P(0,7) + HK44*P(7,17) + HK47*P(7,18) - HK48*P(7,16) + P(7,20));
Kfusion(8) = HK61*(HK20*P(2,8) - HK21*P(3,8) + HK22*P(1,8) + HK24*P(0,8) + HK44*P(8,17) + HK47*P(8,18) - HK48*P(8,16) + P(8,20));
Kfusion(9) = HK61*(HK20*P(2,9) - HK21*P(3,9) + HK22*P(1,9) + HK24*P(0,9) + HK44*P(9,17) + HK47*P(9,18) - HK48*P(9,16) + P(9,20));
Kfusion(10) = HK61*(HK20*P(2,10) - HK21*P(3,10) + HK22*P(1,10) + HK24*P(0,10) + HK44*P(10,17) + HK47*P(10,18) - HK48*P(10,16) + P(10,20));
Kfusion(11) = HK61*(HK20*P(2,11) - HK21*P(3,11) + HK22*P(1,11) + HK24*P(0,11) + HK44*P(11,17) + HK47*P(11,18) - HK48*P(11,16) + P(11,20));
Kfusion(12) = HK61*(HK20*P(2,12) - HK21*P(3,12) + HK22*P(1,12) + HK24*P(0,12) + HK44*P(12,17) + HK47*P(12,18) - HK48*P(12,16) + P(12,20));
Kfusion(13) = HK61*(HK20*P(2,13) - HK21*P(3,13) + HK22*P(1,13) + HK24*P(0,13) + HK44*P(13,17) + HK47*P(13,18) - HK48*P(13,16) + P(13,20));
Kfusion(14) = HK61*(HK20*P(2,14) - HK21*P(3,14) + HK22*P(1,14) + HK24*P(0,14) + HK44*P(14,17) + HK47*P(14,18) - HK48*P(14,16) + P(14,20));
Kfusion(15) = HK61*(HK20*P(2,15) - HK21*P(3,15) + HK22*P(1,15) + HK24*P(0,15) + HK44*P(15,17) + HK47*P(15,18) - HK48*P(15,16) + P(15,20));
Kfusion(16) = HK52*HK61;
Kfusion(17) = HK51*HK61;
Kfusion(18) = HK55*HK61;
Kfusion(19) = HK61*(HK20*P(2,19) - HK21*P(3,19) + HK22*P(1,19) + HK24*P(0,19) + HK44*P(17,19) + HK47*P(18,19) - HK48*P(16,19) + P(19,20));
Kfusion(20) = HK60*HK61;
Kfusion(21) = HK61*(HK20*P(2,21) - HK21*P(3,21) + HK22*P(1,21) + HK24*P(0,21) + HK44*P(17,21) + HK47*P(18,21) - HK48*P(16,21) + P(20,21));
Kfusion(22) = HK61*(HK20*P(2,22) - HK21*P(3,22) + HK22*P(1,22) + HK24*P(0,22) + HK44*P(17,22) + HK47*P(18,22) - HK48*P(16,22) + P(20,22));
Kfusion(23) = HK61*(HK20*P(2,23) - HK21*P(3,23) + HK22*P(1,23) + HK24*P(0,23) + HK44*P(17,23) + HK47*P(18,23) - HK48*P(16,23) + P(20,23));
// Observation Jacobians - axis 2
Hfusion.at<0>() = HK41;
Hfusion.at<1>() = -2*HK5;
Hfusion.at<2>() = HK1;
Hfusion.at<3>() = HK3;
Hfusion.at<16>() = 2*HK62;
Hfusion.at<17>() = -2*HK63;
Hfusion.at<18>() = HK64;
Hfusion.at<21>() = 1;
// Kalman gains - axis 2
Kfusion(0) = HK67*HK75;
Kfusion(1) = HK70*HK75;
Kfusion(2) = HK73*HK75;
Kfusion(3) = HK72*HK75;
Kfusion(4) = HK75*(HK20*P(3,4) + HK21*P(2,4) + HK22*P(0,4) - HK24*P(1,4) + HK64*P(4,18) + HK65*P(4,16) - HK66*P(4,17) + P(4,21));
Kfusion(5) = HK75*(HK20*P(3,5) + HK21*P(2,5) + HK22*P(0,5) - HK24*P(1,5) + HK64*P(5,18) + HK65*P(5,16) - HK66*P(5,17) + P(5,21));
Kfusion(6) = HK75*(HK20*P(3,6) + HK21*P(2,6) + HK22*P(0,6) - HK24*P(1,6) + HK64*P(6,18) + HK65*P(6,16) - HK66*P(6,17) + P(6,21));
Kfusion(7) = HK75*(HK20*P(3,7) + HK21*P(2,7) + HK22*P(0,7) - HK24*P(1,7) + HK64*P(7,18) + HK65*P(7,16) - HK66*P(7,17) + P(7,21));
Kfusion(8) = HK75*(HK20*P(3,8) + HK21*P(2,8) + HK22*P(0,8) - HK24*P(1,8) + HK64*P(8,18) + HK65*P(8,16) - HK66*P(8,17) + P(8,21));
Kfusion(9) = HK75*(HK20*P(3,9) + HK21*P(2,9) + HK22*P(0,9) - HK24*P(1,9) + HK64*P(9,18) + HK65*P(9,16) - HK66*P(9,17) + P(9,21));
Kfusion(10) = HK75*(HK20*P(3,10) + HK21*P(2,10) + HK22*P(0,10) - HK24*P(1,10) + HK64*P(10,18) + HK65*P(10,16) - HK66*P(10,17) + P(10,21));
Kfusion(11) = HK75*(HK20*P(3,11) + HK21*P(2,11) + HK22*P(0,11) - HK24*P(1,11) + HK64*P(11,18) + HK65*P(11,16) - HK66*P(11,17) + P(11,21));
Kfusion(12) = HK75*(HK20*P(3,12) + HK21*P(2,12) + HK22*P(0,12) - HK24*P(1,12) + HK64*P(12,18) + HK65*P(12,16) - HK66*P(12,17) + P(12,21));
Kfusion(13) = HK75*(HK20*P(3,13) + HK21*P(2,13) + HK22*P(0,13) - HK24*P(1,13) + HK64*P(13,18) + HK65*P(13,16) - HK66*P(13,17) + P(13,21));
Kfusion(14) = HK75*(HK20*P(3,14) + HK21*P(2,14) + HK22*P(0,14) - HK24*P(1,14) + HK64*P(14,18) + HK65*P(14,16) - HK66*P(14,17) + P(14,21));
Kfusion(15) = HK75*(HK20*P(3,15) + HK21*P(2,15) + HK22*P(0,15) - HK24*P(1,15) + HK64*P(15,18) + HK65*P(15,16) - HK66*P(15,17) + P(15,21));
Kfusion(16) = HK71*HK75;
Kfusion(17) = HK69*HK75;
Kfusion(18) = HK68*HK75;
Kfusion(19) = HK75*(HK20*P(3,19) + HK21*P(2,19) + HK22*P(0,19) - HK24*P(1,19) + HK64*P(18,19) + HK65*P(16,19) - HK66*P(17,19) + P(19,21));
Kfusion(20) = HK75*(HK20*P(3,20) + HK21*P(2,20) + HK22*P(0,20) - HK24*P(1,20) + HK64*P(18,20) + HK65*P(16,20) - HK66*P(17,20) + P(20,21));
Kfusion(21) = HK74*HK75;
Kfusion(22) = HK75*(HK20*P(3,22) + HK21*P(2,22) + HK22*P(0,22) - HK24*P(1,22) + HK64*P(18,22) + HK65*P(16,22) - HK66*P(17,22) + P(21,22));
Kfusion(23) = HK75*(HK20*P(3,23) + HK21*P(2,23) + HK22*P(0,23) - HK24*P(1,23) + HK64*P(18,23) + HK65*P(16,23) - HK66*P(17,23) + P(21,23));

43
src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_innov_var_generated.cpp
View File

@ -1,43 +0,0 @@
// Sub Expressions
const float IV0 = q0*q1;
const float IV1 = q2*q3;
const float IV2 = 2*IV0 + 2*IV1;
const float IV3 = 2*q0*q3 - 2*q1*q2;
const float IV4 = 2*magD*q3 + 2*magE*q2 + 2*magN*q1;
const float IV5 = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;
const float IV6 = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;
const float IV7 = -2*magD*q2 + 2*magE*q3 + 2*magN*q0;
const float IV8 = (q2)*(q2);
const float IV9 = (q3)*(q3);
const float IV10 = (q0)*(q0) - (q1)*(q1);
const float IV11 = IV10 + IV8 - IV9;
const float IV12 = IV7*P(2,3);
const float IV13 = IV5*P(0,1);
const float IV14 = IV6*P(0,1);
const float IV15 = IV4*P(2,3);
const float IV16 = 2*q0*q2 + 2*q1*q3;
const float IV17 = 2*IV0 - 2*IV1;
const float IV18 = IV10 - IV8 + IV9;
// Observation Jacobians - axis 0
Hfusion.at<0>() = R_MAG;
Hfusion.at<1>() = IV11*P(17,20) + IV11*(IV11*P(17,17) + IV2*P(17,18) - IV3*P(16,17) + IV4*P(2,17) + IV5*P(0,17) + IV6*P(1,17) - IV7*P(3,17) + P(17,20)) + IV2*P(18,20) + IV2*(IV11*P(17,18) + IV2*P(18,18) - IV3*P(16,18) + IV4*P(2,18) + IV5*P(0,18) + IV6*P(1,18) - IV7*P(3,18) + P(18,20)) - IV3*P(16,20) - IV3*(IV11*P(16,17) + IV2*P(16,18) - IV3*P(16,16) + IV4*P(2,16) + IV5*P(0,16) + IV6*P(1,16) - IV7*P(3,16) + P(16,20)) + IV4*P(2,20) + IV4*(IV11*P(2,17) - IV12 + IV2*P(2,18) - IV3*P(2,16) + IV4*P(2,2) + IV5*P(0,2) + IV6*P(1,2) + P(2,20)) + IV5*P(0,20) + IV5*(IV11*P(0,17) + IV14 + IV2*P(0,18) - IV3*P(0,16) + IV4*P(0,2) + IV5*P(0,0) - IV7*P(0,3) + P(0,20)) + IV6*P(1,20) + IV6*(IV11*P(1,17) + IV13 + IV2*P(1,18) - IV3*P(1,16) + IV4*P(1,2) + IV6*P(1,1) - IV7*P(1,3) + P(1,20)) - IV7*P(3,20) - IV7*(IV11*P(3,17) + IV15 + IV2*P(3,18) - IV3*P(3,16) + IV5*P(0,3) + IV6*P(1,3) - IV7*P(3,3) + P(3,20)) + P(20,20) + R_MAG;
Hfusion.at<2>() = IV16*P(16,21) + IV16*(IV16*P(16,16) - IV17*P(16,17) + IV18*P(16,18) + IV4*P(3,16) - IV5*P(1,16) + IV6*P(0,16) + IV7*P(2,16) + P(16,21)) - IV17*P(17,21) - IV17*(IV16*P(16,17) - IV17*P(17,17) + IV18*P(17,18) + IV4*P(3,17) - IV5*P(1,17) + IV6*P(0,17) + IV7*P(2,17) + P(17,21)) + IV18*P(18,21) + IV18*(IV16*P(16,18) - IV17*P(17,18) + IV18*P(18,18) + IV4*P(3,18) - IV5*P(1,18) + IV6*P(0,18) + IV7*P(2,18) + P(18,21)) + IV4*P(3,21) + IV4*(IV12 + IV16*P(3,16) - IV17*P(3,17) + IV18*P(3,18) + IV4*P(3,3) - IV5*P(1,3) + IV6*P(0,3) + P(3,21)) - IV5*P(1,21) - IV5*(IV14 + IV16*P(1,16) - IV17*P(1,17) + IV18*P(1,18) + IV4*P(1,3) - IV5*P(1,1) + IV7*P(1,2) + P(1,21)) + IV6*P(0,21) + IV6*(-IV13 + IV16*P(0,16) - IV17*P(0,17) + IV18*P(0,18) + IV4*P(0,3) + IV6*P(0,0) + IV7*P(0,2) + P(0,21)) + IV7*P(2,21) + IV7*(IV15 + IV16*P(2,16) - IV17*P(2,17) + IV18*P(2,18) - IV5*P(1,2) + IV6*P(0,2) + IV7*P(2,2) + P(2,21)) + P(21,21) + R_MAG;
// Kalman gains - axis 0
// Observation Jacobians - axis 1
// Kalman gains - axis 1
// Observation Jacobians - axis 2
// Kalman gains - axis 2