fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: purge old ekf2 derivation

Browse Source
This commit is contained in:
bresch 2023-01-06 15:05:23 +01:00 committed by Daniel Agar
parent d1b95a21e4
commit 49f8bcfc69
9 changed files with 0 additions and 1946 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/modules/ekf2/EKF/python/ekf_derivation/__init__.py
View File

@ -1,7 +0,0 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Sat Mar 14 13:02:26 2020
@author: roman
"""

65
src/modules/ekf2/EKF/python/ekf_derivation/code_gen.py
View File

@ -1,65 +0,0 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Sat Mar 14 12:47:24 2020
@author: roman
"""
from sympy import ccode
from sympy.codegen.ast import float32, real
class CodeGenerator:
def __init__(self, file_name):
self.file_name = file_name
self.file = open(self.file_name, 'w')
def print_string(self, string):
self.file.write("// " + string + "\n")
def get_ccode(self, expression):
custom_functions = {
"Pow": [
(lambda b, e: e == 2, lambda b, e: f"({b})*({b})"),
(lambda b, e: e == -1, lambda b, e: f"1.0F/({b})"),
(lambda b, e: e == -2, lambda b, e: f"1.0F/(({b})*({b}))"),
(lambda b, e: e == 0.5, lambda b, e: f"sqrtf({b})"),
(lambda b, e: e == -0.5, lambda b, e: f"1.0F/sqrtf({b})"),
(lambda b, e: True, "ecl::powf"),
]
}
return ccode(expression, type_aliases={real:float32}, user_functions=custom_functions)
def write_subexpressions(self,subexpressions):
write_string = ""
for item in subexpressions:
write_string = write_string + "const float " + str(item[0]) + " = " + self.get_ccode(item[1]) + ";\n"
write_string = write_string + "\n\n"
self.file.write(write_string)
def write_matrix(self, matrix, variable_name, is_symmetric=False, pre_bracket="(", post_bracket=")"):
write_string = ""
if matrix.shape[0] * matrix.shape[1] == 1:
if matrix[0] != 0:
write_string = write_string + variable_name + " = " + self.get_ccode(matrix[0]) + ";\n"
elif matrix.shape[0] == 1 or matrix.shape[1] == 1:
for i in range(0,len(matrix)):
if matrix[i] == 0:
continue
write_string = write_string + variable_name + pre_bracket + str(i) + post_bracket + " = " + self.get_ccode(matrix[i]) + ";\n"
else:
for j in range(0, matrix.shape[1]):
for i in range(0, matrix.shape[0]):
if j >= i or not is_symmetric:
if matrix[i,j] == 0:
continue
write_string = write_string + variable_name + pre_bracket + str(i) + "," + str(j) + post_bracket + " = " + self.get_ccode(matrix[i,j]) + ";\n"
write_string = write_string + "\n\n"
self.file.write(write_string)
def close(self):
self.file.close()

476
src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_fusion_generated_compare.cpp
View File

@ -1,476 +0,0 @@
#include <math.h>
#include <stdio.h>
#include <cstdlib>
#include "../../../../../matrix/matrix/math.hpp"
#include "util.h"
typedef matrix::Vector<float, 24> Vector24f;
typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
template<int ... Idxs>
using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
int main()
{
// Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations
SparseVector24f<0,1,2,3,4,5,6,22,23> Hfusion;
Vector24f H_ACC;
Vector24f Kfusion;
float drag_innov_var;
Vector24f Hfusion_sympy;
Vector24f Kfusion_sympy;
Vector24f Hfusion_matlab;
Vector24f Kfusion_matlab;
float SH_ACC[4] = {}; // Variable used to optimise calculations of measurement jacobian
float SK_ACC[9] = {}; // Variable used to optimise calculations of the Kalman gain vector
const float R_ACC = sq(2.5f); // observation noise variance in specific force drag (m/sec**2)**2
// 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;
// get latest velocity in earth frame
const float vn = 8.0f;
const float ve = 6.0f;
const float vd = 1.0f;
// get latest wind velocity in earth frame
const float vwn = 4.0f;
const float vwe = 3.0f;
const float BC_inv_x = 1.0f / 35.0f;
const float BC_inv_y = 1.0f / 25.0f;
const float airSpd = 5.0f;
const float rho = 1.225f;
// 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);
}
}
}
// Compare X axis equations
{
// Estimate the derivative of specific force wrt airspeed along the X axis
// Limit lower value to prevent arithmetic exceptions
const float Kaccx = fmaxf(1e-1f, rho * BC_inv_x * airSpd);
// intermediate variables
const float HK0 = vn - vwn;
const float HK1 = ve - vwe;
const float HK2 = HK0*q0 + HK1*q3 - q2*vd;
const float HK3 = 2*Kaccx;
const float HK4 = HK0*q1 + HK1*q2 + q3*vd;
const float HK5 = HK0*q2 - HK1*q1 + q0*vd;
const float HK6 = -HK0*q3 + HK1*q0 + q1*vd;
const float HK7 = ecl::powf(q0, 2) + ecl::powf(q1, 2) - ecl::powf(q2, 2) - ecl::powf(q3, 2);
const float HK8 = HK7*Kaccx;
const float HK9 = q0*q3 + q1*q2;
const float HK10 = HK3*HK9;
const float HK11 = q0*q2 - q1*q3;
const float HK12 = 2*HK9;
const float HK13 = 2*HK11;
const float HK14 = 2*HK4;
const float HK15 = 2*HK2;
const float HK16 = 2*HK5;
const float HK17 = 2*HK6;
const float HK18 = -HK12*P(0,23) + HK12*P(0,5) - HK13*P(0,6) + HK14*P(0,1) + HK15*P(0,0) - HK16*P(0,2) + HK17*P(0,3) - HK7*P(0,22) + HK7*P(0,4);
const float HK19 = HK12*P(5,23);
const float HK20 = -HK12*P(23,23) - HK13*P(6,23) + HK14*P(1,23) + HK15*P(0,23) - HK16*P(2,23) + HK17*P(3,23) + HK19 - HK7*P(22,23) + HK7*P(4,23);
const float HK21 = ecl::powf(Kaccx, 2);
const float HK22 = HK12*HK21;
const float HK23 = HK12*P(5,5) - HK13*P(5,6) + HK14*P(1,5) + HK15*P(0,5) - HK16*P(2,5) + HK17*P(3,5) - HK19 + HK7*P(4,5) - HK7*P(5,22);
const float HK24 = HK12*P(5,6) - HK12*P(6,23) - HK13*P(6,6) + HK14*P(1,6) + HK15*P(0,6) - HK16*P(2,6) + HK17*P(3,6) + HK7*P(4,6) - HK7*P(6,22);
const float HK25 = HK7*P(4,22);
const float HK26 = -HK12*P(4,23) + HK12*P(4,5) - HK13*P(4,6) + HK14*P(1,4) + HK15*P(0,4) - HK16*P(2,4) + HK17*P(3,4) - HK25 + HK7*P(4,4);
const float HK27 = HK21*HK7;
const float HK28 = -HK12*P(22,23) + HK12*P(5,22) - HK13*P(6,22) + HK14*P(1,22) + HK15*P(0,22) - HK16*P(2,22) + HK17*P(3,22) + HK25 - HK7*P(22,22);
const float HK29 = -HK12*P(1,23) + HK12*P(1,5) - HK13*P(1,6) + HK14*P(1,1) + HK15*P(0,1) - HK16*P(1,2) + HK17*P(1,3) - HK7*P(1,22) + HK7*P(1,4);
const float HK30 = -HK12*P(2,23) + HK12*P(2,5) - HK13*P(2,6) + HK14*P(1,2) + HK15*P(0,2) - HK16*P(2,2) + HK17*P(2,3) - HK7*P(2,22) + HK7*P(2,4);
const float HK31 = -HK12*P(3,23) + HK12*P(3,5) - HK13*P(3,6) + HK14*P(1,3) + HK15*P(0,3) - HK16*P(2,3) + HK17*P(3,3) - HK7*P(3,22) + HK7*P(3,4);
const float HK32 = Kaccx/(-HK13*HK21*HK24 + HK14*HK21*HK29 + HK15*HK18*HK21 - HK16*HK21*HK30 + HK17*HK21*HK31 - HK20*HK22 + HK22*HK23 + HK26*HK27 - HK27*HK28 + R_ACC);
// Observation Jacobians
Hfusion.at<0>() = -HK2*HK3;
Hfusion.at<1>() = -HK3*HK4;
Hfusion.at<2>() = HK3*HK5;
Hfusion.at<3>() = -HK3*HK6;
Hfusion.at<4>() = -HK8;
Hfusion.at<5>() = -HK10;
Hfusion.at<6>() = HK11*HK3;
Hfusion.at<22>() = HK8;
Hfusion.at<23>() = HK10;
// Kalman gains
Kfusion(0) = -HK18*HK32;
Kfusion(1) = -HK29*HK32;
Kfusion(2) = -HK30*HK32;
Kfusion(3) = -HK31*HK32;
Kfusion(4) = -HK26*HK32;
Kfusion(5) = -HK23*HK32;
Kfusion(6) = -HK24*HK32;
Kfusion(7) = -HK32*(HK12*P(5,7) - HK12*P(7,23) - HK13*P(6,7) + HK14*P(1,7) + HK15*P(0,7) - HK16*P(2,7) + HK17*P(3,7) + HK7*P(4,7) - HK7*P(7,22));
Kfusion(8) = -HK32*(HK12*P(5,8) - HK12*P(8,23) - HK13*P(6,8) + HK14*P(1,8) + HK15*P(0,8) - HK16*P(2,8) + HK17*P(3,8) + HK7*P(4,8) - HK7*P(8,22));
Kfusion(9) = -HK32*(HK12*P(5,9) - HK12*P(9,23) - HK13*P(6,9) + HK14*P(1,9) + HK15*P(0,9) - HK16*P(2,9) + HK17*P(3,9) + HK7*P(4,9) - HK7*P(9,22));
Kfusion(10) = -HK32*(-HK12*P(10,23) + HK12*P(5,10) - HK13*P(6,10) + HK14*P(1,10) + HK15*P(0,10) - HK16*P(2,10) + HK17*P(3,10) - HK7*P(10,22) + HK7*P(4,10));
Kfusion(11) = -HK32*(-HK12*P(11,23) + HK12*P(5,11) - HK13*P(6,11) + HK14*P(1,11) + HK15*P(0,11) - HK16*P(2,11) + HK17*P(3,11) - HK7*P(11,22) + HK7*P(4,11));
Kfusion(12) = -HK32*(-HK12*P(12,23) + HK12*P(5,12) - HK13*P(6,12) + HK14*P(1,12) + HK15*P(0,12) - HK16*P(2,12) + HK17*P(3,12) - HK7*P(12,22) + HK7*P(4,12));
Kfusion(13) = -HK32*(-HK12*P(13,23) + HK12*P(5,13) - HK13*P(6,13) + HK14*P(1,13) + HK15*P(0,13) - HK16*P(2,13) + HK17*P(3,13) - HK7*P(13,22) + HK7*P(4,13));
Kfusion(14) = -HK32*(-HK12*P(14,23) + HK12*P(5,14) - HK13*P(6,14) + HK14*P(1,14) + HK15*P(0,14) - HK16*P(2,14) + HK17*P(3,14) - HK7*P(14,22) + HK7*P(4,14));
Kfusion(15) = -HK32*(-HK12*P(15,23) + HK12*P(5,15) - HK13*P(6,15) + HK14*P(1,15) + HK15*P(0,15) - HK16*P(2,15) + HK17*P(3,15) - HK7*P(15,22) + HK7*P(4,15));
Kfusion(16) = -HK32*(-HK12*P(16,23) + HK12*P(5,16) - HK13*P(6,16) + HK14*P(1,16) + HK15*P(0,16) - HK16*P(2,16) + HK17*P(3,16) - HK7*P(16,22) + HK7*P(4,16));
Kfusion(17) = -HK32*(-HK12*P(17,23) + HK12*P(5,17) - HK13*P(6,17) + HK14*P(1,17) + HK15*P(0,17) - HK16*P(2,17) + HK17*P(3,17) - HK7*P(17,22) + HK7*P(4,17));
Kfusion(18) = -HK32*(-HK12*P(18,23) + HK12*P(5,18) - HK13*P(6,18) + HK14*P(1,18) + HK15*P(0,18) - HK16*P(2,18) + HK17*P(3,18) - HK7*P(18,22) + HK7*P(4,18));
Kfusion(19) = -HK32*(-HK12*P(19,23) + HK12*P(5,19) - HK13*P(6,19) + HK14*P(1,19) + HK15*P(0,19) - HK16*P(2,19) + HK17*P(3,19) - HK7*P(19,22) + HK7*P(4,19));
Kfusion(20) = -HK32*(-HK12*P(20,23) + HK12*P(5,20) - HK13*P(6,20) + HK14*P(1,20) + HK15*P(0,20) - HK16*P(2,20) + HK17*P(3,20) - HK7*P(20,22) + HK7*P(4,20));
Kfusion(21) = -HK32*(-HK12*P(21,23) + HK12*P(5,21) - HK13*P(6,21) + HK14*P(1,21) + HK15*P(0,21) - HK16*P(2,21) + HK17*P(3,21) - HK7*P(21,22) + HK7*P(4,21));
Kfusion(22) = -HK28*HK32;
Kfusion(23) = -HK20*HK32;
// save output
Hfusion_sympy(0) = Hfusion.at<0>();
Hfusion_sympy(1) = Hfusion.at<1>();
Hfusion_sympy(2) = Hfusion.at<2>();
Hfusion_sympy(3) = Hfusion.at<3>();
Hfusion_sympy(4) = Hfusion.at<4>();
Hfusion_sympy(5) = Hfusion.at<5>();
Hfusion_sympy(6) = Hfusion.at<6>();
Hfusion_sympy(22) = Hfusion.at<22>();
Hfusion_sympy(23) = Hfusion.at<23>();
Kfusion_sympy = Kfusion;
// repeat calculation using matlab generated equations
const float Kacc = Kaccx;
// Estimate the derivative of specific force wrt airspeed along the X axis
SH_ACC[0] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
SH_ACC[1] = vn - vwn;
SH_ACC[2] = ve - vwe;
SH_ACC[3] = 2.0f*q0*q3 + 2.0f*q1*q2;
H_ACC.setZero();
H_ACC(0) = -Kacc*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd);
H_ACC(1) = -Kacc*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd);
H_ACC(2) = Kacc*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd);
H_ACC(3) = -Kacc*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd);
H_ACC(4) = -Kacc*SH_ACC[0];
H_ACC(5) = -Kacc*SH_ACC[3];
H_ACC(6) = Kacc*(2.0f*q0*q2 - 2.0f*q1*q3);
H_ACC(22) = Kacc*SH_ACC[0];
H_ACC(23) = Kacc*SH_ACC[3];
drag_innov_var = (R_ACC + Kacc*SH_ACC[0]*(Kacc*P(4,4)*SH_ACC[0] + Kacc*P(5,4)*SH_ACC[3] - Kacc*P(22,4)*SH_ACC[0] - Kacc*P(23,4)*SH_ACC[3] - Kacc*P(6,4)*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P(0,4)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P(1,4)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(2,4)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(3,4)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) + Kacc*SH_ACC[3]*(Kacc*P(4,5)*SH_ACC[0] + Kacc*P(5,5)*SH_ACC[3] - Kacc*P(22,5)*SH_ACC[0] - Kacc*P(23,5)*SH_ACC[3] - Kacc*P(6,5)*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P(0,5)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P(1,5)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(2,5)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(3,5)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) - Kacc*SH_ACC[0]*(Kacc*P(4,22)*SH_ACC[0] + Kacc*P(5,22)*SH_ACC[3] - Kacc*P(22,22)*SH_ACC[0] - Kacc*P(23,22)*SH_ACC[3] - Kacc*P(6,22)*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P(0,22)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P(1,22)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(2,22)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(3,22)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) - Kacc*SH_ACC[3]*(Kacc*P(4,23)*SH_ACC[0] + Kacc*P(5,23)*SH_ACC[3] - Kacc*P(22,23)*SH_ACC[0] - Kacc*P(23,23)*SH_ACC[3] - Kacc*P(6,23)*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P(0,23)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P(1,23)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(2,23)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(3,23)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) - Kacc*(2.0f*q0*q2 - 2.0f*q1*q3)*(Kacc*P(4,6)*SH_ACC[0] + Kacc*P(5,6)*SH_ACC[3] - Kacc*P(22,6)*SH_ACC[0] - Kacc*P(23,6)*SH_ACC[3] - Kacc*P(6,6)*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P(0,6)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P(1,6)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(2,6)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(3,6)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) + Kacc*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)*(Kacc*P(4,0)*SH_ACC[0] + Kacc*P(5,0)*SH_ACC[3] - Kacc*P(22,0)*SH_ACC[0] - Kacc*P(23,0)*SH_ACC[3] - Kacc*P(6,0)*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P(0,0)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P(1,0)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(2,0)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(3,0)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) + Kacc*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd)*(Kacc*P(4,1)*SH_ACC[0] + Kacc*P(5,1)*SH_ACC[3] - Kacc*P(22,1)*SH_ACC[0] - Kacc*P(23,1)*SH_ACC[3] - Kacc*P(6,1)*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P(0,1)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P(1,1)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(2,1)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(3,1)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) - Kacc*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd)*(Kacc*P(4,2)*SH_ACC[0] + Kacc*P(5,2)*SH_ACC[3] - Kacc*P(22,2)*SH_ACC[0] - Kacc*P(23,2)*SH_ACC[3] - Kacc*P(6,2)*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P(0,2)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P(1,2)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(2,2)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(3,2)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) + Kacc*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)*(Kacc*P(4,3)*SH_ACC[0] + Kacc*P(5,3)*SH_ACC[3] - Kacc*P(22,3)*SH_ACC[0] - Kacc*P(23,3)*SH_ACC[3] - Kacc*P(6,3)*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P(0,3)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P(1,3)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(2,3)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(3,3)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)));
SK_ACC[0] = 1.0f/drag_innov_var;
SK_ACC[1] = 2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd;
SK_ACC[2] = 2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd;
SK_ACC[3] = 2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd;
SK_ACC[4] = 2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd;
SK_ACC[5] = 2.0f*q0*q2 - 2.0f*q1*q3;
SK_ACC[6] = SH_ACC[3];
Kfusion(0) = -SK_ACC[0]*(Kacc*P(0,4)*SH_ACC[0] - Kacc*P(0,22)*SH_ACC[0] + Kacc*P(0,0)*SK_ACC[3] - Kacc*P(0,2)*SK_ACC[2] + Kacc*P(0,3)*SK_ACC[1] + Kacc*P(0,1)*SK_ACC[4] + Kacc*P(0,5)*SK_ACC[6] - Kacc*P(0,6)*SK_ACC[5] - Kacc*P(0,23)*SK_ACC[6]);
Kfusion(1) = -SK_ACC[0]*(Kacc*P(1,4)*SH_ACC[0] - Kacc*P(1,22)*SH_ACC[0] + Kacc*P(1,0)*SK_ACC[3] - Kacc*P(1,2)*SK_ACC[2] + Kacc*P(1,3)*SK_ACC[1] + Kacc*P(1,1)*SK_ACC[4] + Kacc*P(1,5)*SK_ACC[6] - Kacc*P(1,6)*SK_ACC[5] - Kacc*P(1,23)*SK_ACC[6]);
Kfusion(2) = -SK_ACC[0]*(Kacc*P(2,4)*SH_ACC[0] - Kacc*P(2,22)*SH_ACC[0] + Kacc*P(2,0)*SK_ACC[3] - Kacc*P(2,2)*SK_ACC[2] + Kacc*P(2,3)*SK_ACC[1] + Kacc*P(2,1)*SK_ACC[4] + Kacc*P(2,5)*SK_ACC[6] - Kacc*P(2,6)*SK_ACC[5] - Kacc*P(2,23)*SK_ACC[6]);
Kfusion(3) = -SK_ACC[0]*(Kacc*P(3,4)*SH_ACC[0] - Kacc*P(3,22)*SH_ACC[0] + Kacc*P(3,0)*SK_ACC[3] - Kacc*P(3,2)*SK_ACC[2] + Kacc*P(3,3)*SK_ACC[1] + Kacc*P(3,1)*SK_ACC[4] + Kacc*P(3,5)*SK_ACC[6] - Kacc*P(3,6)*SK_ACC[5] - Kacc*P(3,23)*SK_ACC[6]);
Kfusion(4) = -SK_ACC[0]*(Kacc*P(4,4)*SH_ACC[0] - Kacc*P(4,22)*SH_ACC[0] + Kacc*P(4,0)*SK_ACC[3] - Kacc*P(4,2)*SK_ACC[2] + Kacc*P(4,3)*SK_ACC[1] + Kacc*P(4,1)*SK_ACC[4] + Kacc*P(4,5)*SK_ACC[6] - Kacc*P(4,6)*SK_ACC[5] - Kacc*P(4,23)*SK_ACC[6]);
Kfusion(5) = -SK_ACC[0]*(Kacc*P(5,4)*SH_ACC[0] - Kacc*P(5,22)*SH_ACC[0] + Kacc*P(5,0)*SK_ACC[3] - Kacc*P(5,2)*SK_ACC[2] + Kacc*P(5,3)*SK_ACC[1] + Kacc*P(5,1)*SK_ACC[4] + Kacc*P(5,5)*SK_ACC[6] - Kacc*P(5,6)*SK_ACC[5] - Kacc*P(5,23)*SK_ACC[6]);
Kfusion(6) = -SK_ACC[0]*(Kacc*P(6,4)*SH_ACC[0] - Kacc*P(6,22)*SH_ACC[0] + Kacc*P(6,0)*SK_ACC[3] - Kacc*P(6,2)*SK_ACC[2] + Kacc*P(6,3)*SK_ACC[1] + Kacc*P(6,1)*SK_ACC[4] + Kacc*P(6,5)*SK_ACC[6] - Kacc*P(6,6)*SK_ACC[5] - Kacc*P(6,23)*SK_ACC[6]);
Kfusion(7) = -SK_ACC[0]*(Kacc*P(7,4)*SH_ACC[0] - Kacc*P(7,22)*SH_ACC[0] + Kacc*P(7,0)*SK_ACC[3] - Kacc*P(7,2)*SK_ACC[2] + Kacc*P(7,3)*SK_ACC[1] + Kacc*P(7,1)*SK_ACC[4] + Kacc*P(7,5)*SK_ACC[6] - Kacc*P(7,6)*SK_ACC[5] - Kacc*P(7,23)*SK_ACC[6]);
Kfusion(8) = -SK_ACC[0]*(Kacc*P(8,4)*SH_ACC[0] - Kacc*P(8,22)*SH_ACC[0] + Kacc*P(8,0)*SK_ACC[3] - Kacc*P(8,2)*SK_ACC[2] + Kacc*P(8,3)*SK_ACC[1] + Kacc*P(8,1)*SK_ACC[4] + Kacc*P(8,5)*SK_ACC[6] - Kacc*P(8,6)*SK_ACC[5] - Kacc*P(8,23)*SK_ACC[6]);
Kfusion(9) = -SK_ACC[0]*(Kacc*P(9,4)*SH_ACC[0] - Kacc*P(9,22)*SH_ACC[0] + Kacc*P(9,0)*SK_ACC[3] - Kacc*P(9,2)*SK_ACC[2] + Kacc*P(9,3)*SK_ACC[1] + Kacc*P(9,1)*SK_ACC[4] + Kacc*P(9,5)*SK_ACC[6] - Kacc*P(9,6)*SK_ACC[5] - Kacc*P(9,23)*SK_ACC[6]);
Kfusion(10) = -SK_ACC[0]*(Kacc*P(10,4)*SH_ACC[0] - Kacc*P(10,22)*SH_ACC[0] + Kacc*P(10,0)*SK_ACC[3] - Kacc*P(10,2)*SK_ACC[2] + Kacc*P(10,3)*SK_ACC[1] + Kacc*P(10,1)*SK_ACC[4] + Kacc*P(10,5)*SK_ACC[6] - Kacc*P(10,6)*SK_ACC[5] - Kacc*P(10,23)*SK_ACC[6]);
Kfusion(11) = -SK_ACC[0]*(Kacc*P(11,4)*SH_ACC[0] - Kacc*P(11,22)*SH_ACC[0] + Kacc*P(11,0)*SK_ACC[3] - Kacc*P(11,2)*SK_ACC[2] + Kacc*P(11,3)*SK_ACC[1] + Kacc*P(11,1)*SK_ACC[4] + Kacc*P(11,5)*SK_ACC[6] - Kacc*P(11,6)*SK_ACC[5] - Kacc*P(11,23)*SK_ACC[6]);
Kfusion(12) = -SK_ACC[0]*(Kacc*P(12,4)*SH_ACC[0] - Kacc*P(12,22)*SH_ACC[0] + Kacc*P(12,0)*SK_ACC[3] - Kacc*P(12,2)*SK_ACC[2] + Kacc*P(12,3)*SK_ACC[1] + Kacc*P(12,1)*SK_ACC[4] + Kacc*P(12,5)*SK_ACC[6] - Kacc*P(12,6)*SK_ACC[5] - Kacc*P(12,23)*SK_ACC[6]);
Kfusion(13) = -SK_ACC[0]*(Kacc*P(13,4)*SH_ACC[0] - Kacc*P(13,22)*SH_ACC[0] + Kacc*P(13,0)*SK_ACC[3] - Kacc*P(13,2)*SK_ACC[2] + Kacc*P(13,3)*SK_ACC[1] + Kacc*P(13,1)*SK_ACC[4] + Kacc*P(13,5)*SK_ACC[6] - Kacc*P(13,6)*SK_ACC[5] - Kacc*P(13,23)*SK_ACC[6]);
Kfusion(14) = -SK_ACC[0]*(Kacc*P(14,4)*SH_ACC[0] - Kacc*P(14,22)*SH_ACC[0] + Kacc*P(14,0)*SK_ACC[3] - Kacc*P(14,2)*SK_ACC[2] + Kacc*P(14,3)*SK_ACC[1] + Kacc*P(14,1)*SK_ACC[4] + Kacc*P(14,5)*SK_ACC[6] - Kacc*P(14,6)*SK_ACC[5] - Kacc*P(14,23)*SK_ACC[6]);
Kfusion(15) = -SK_ACC[0]*(Kacc*P(15,4)*SH_ACC[0] - Kacc*P(15,22)*SH_ACC[0] + Kacc*P(15,0)*SK_ACC[3] - Kacc*P(15,2)*SK_ACC[2] + Kacc*P(15,3)*SK_ACC[1] + Kacc*P(15,1)*SK_ACC[4] + Kacc*P(15,5)*SK_ACC[6] - Kacc*P(15,6)*SK_ACC[5] - Kacc*P(15,23)*SK_ACC[6]);
Kfusion(16) = -SK_ACC[0]*(Kacc*P(16,4)*SH_ACC[0] - Kacc*P(16,22)*SH_ACC[0] + Kacc*P(16,0)*SK_ACC[3] - Kacc*P(16,2)*SK_ACC[2] + Kacc*P(16,3)*SK_ACC[1] + Kacc*P(16,1)*SK_ACC[4] + Kacc*P(16,5)*SK_ACC[6] - Kacc*P(16,6)*SK_ACC[5] - Kacc*P(16,23)*SK_ACC[6]);
Kfusion(17) = -SK_ACC[0]*(Kacc*P(17,4)*SH_ACC[0] - Kacc*P(17,22)*SH_ACC[0] + Kacc*P(17,0)*SK_ACC[3] - Kacc*P(17,2)*SK_ACC[2] + Kacc*P(17,3)*SK_ACC[1] + Kacc*P(17,1)*SK_ACC[4] + Kacc*P(17,5)*SK_ACC[6] - Kacc*P(17,6)*SK_ACC[5] - Kacc*P(17,23)*SK_ACC[6]);
Kfusion(18) = -SK_ACC[0]*(Kacc*P(18,4)*SH_ACC[0] - Kacc*P(18,22)*SH_ACC[0] + Kacc*P(18,0)*SK_ACC[3] - Kacc*P(18,2)*SK_ACC[2] + Kacc*P(18,3)*SK_ACC[1] + Kacc*P(18,1)*SK_ACC[4] + Kacc*P(18,5)*SK_ACC[6] - Kacc*P(18,6)*SK_ACC[5] - Kacc*P(18,23)*SK_ACC[6]);
Kfusion(19) = -SK_ACC[0]*(Kacc*P(19,4)*SH_ACC[0] - Kacc*P(19,22)*SH_ACC[0] + Kacc*P(19,0)*SK_ACC[3] - Kacc*P(19,2)*SK_ACC[2] + Kacc*P(19,3)*SK_ACC[1] + Kacc*P(19,1)*SK_ACC[4] + Kacc*P(19,5)*SK_ACC[6] - Kacc*P(19,6)*SK_ACC[5] - Kacc*P(19,23)*SK_ACC[6]);
Kfusion(20) = -SK_ACC[0]*(Kacc*P(20,4)*SH_ACC[0] - Kacc*P(20,22)*SH_ACC[0] + Kacc*P(20,0)*SK_ACC[3] - Kacc*P(20,2)*SK_ACC[2] + Kacc*P(20,3)*SK_ACC[1] + Kacc*P(20,1)*SK_ACC[4] + Kacc*P(20,5)*SK_ACC[6] - Kacc*P(20,6)*SK_ACC[5] - Kacc*P(20,23)*SK_ACC[6]);
Kfusion(21) = -SK_ACC[0]*(Kacc*P(21,4)*SH_ACC[0] - Kacc*P(21,22)*SH_ACC[0] + Kacc*P(21,0)*SK_ACC[3] - Kacc*P(21,2)*SK_ACC[2] + Kacc*P(21,3)*SK_ACC[1] + Kacc*P(21,1)*SK_ACC[4] + Kacc*P(21,5)*SK_ACC[6] - Kacc*P(21,6)*SK_ACC[5] - Kacc*P(21,23)*SK_ACC[6]);
Kfusion(22) = -SK_ACC[0]*(Kacc*P(22,4)*SH_ACC[0] - Kacc*P(22,22)*SH_ACC[0] + Kacc*P(22,0)*SK_ACC[3] - Kacc*P(22,2)*SK_ACC[2] + Kacc*P(22,3)*SK_ACC[1] + Kacc*P(22,1)*SK_ACC[4] + Kacc*P(22,5)*SK_ACC[6] - Kacc*P(22,6)*SK_ACC[5] - Kacc*P(22,23)*SK_ACC[6]);
Kfusion(23) = -SK_ACC[0]*(Kacc*P(23,4)*SH_ACC[0] - Kacc*P(23,22)*SH_ACC[0] + Kacc*P(23,0)*SK_ACC[3] - Kacc*P(23,2)*SK_ACC[2] + Kacc*P(23,3)*SK_ACC[1] + Kacc*P(23,1)*SK_ACC[4] + Kacc*P(23,5)*SK_ACC[6] - Kacc*P(23,6)*SK_ACC[5] - Kacc*P(23,23)*SK_ACC[6]);
Hfusion_matlab = H_ACC;
Kfusion_matlab = Kfusion;
// 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 (Hfusion_matlab(row) != 0.0f) {
diff_fraction = fabsf(Hfusion_sympy(row) - Hfusion_matlab(row)) / fabsf(Hfusion_matlab(row));
} else if (Hfusion_sympy(row) != 0.0f) {
diff_fraction = fabsf(Hfusion_sympy(row) - Hfusion_matlab(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 = Hfusion_matlab(row);
max_new = Hfusion_sympy(row);
}
}
if (max_diff_fraction > 1e-5f) {
printf("Fail: Specific Force 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: Specific Force X axis Hfusion max diff fraction = %e\n",max_diff_fraction);
}
// find largest Kalman gain difference as a fraction of the matlab value
// find largest Kalman gain difference as a fraction of the matlab value
max_diff_fraction = 0.0f;
for (int row=0; row<24; row++) {
float diff_fraction;
if (Kfusion_matlab(row) != 0.0f) {
diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion_matlab(row)) / fabsf(Kfusion_matlab(row));
} else if (Kfusion_sympy(row) != 0.0f) {
diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion_matlab(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_matlab(row);
max_new = Kfusion_sympy(row);
}
}
if (max_diff_fraction > 1e-5f) {
printf("Fail: Specific Force 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: Specific Force X axis Kfusion max diff fraction = %e\n",max_diff_fraction);
}
}
// Compare Y axis equations
{
const float airSpd = 10.0f * (float)rand() + 5.0f;
// Estimate the derivative of specific force wrt airspeed along the X axis
// Limit lower value to prevent arithmetic exceptions
const float Kaccy = fmaxf(1e-1f, rho * BC_inv_y * airSpd);
// Sub Expressions
const float HK0 = ve - vwe;
const float HK1 = vn - vwn;
const float HK2 = HK0*q0 - HK1*q3 + q1*vd;
const float HK3 = 2*Kaccy;
const float HK4 = -HK0*q1 + HK1*q2 + q0*vd;
const float HK5 = HK0*q2 + HK1*q1 + q3*vd;
const float HK6 = HK0*q3 + HK1*q0 - q2*vd;
const float HK7 = q0*q3 - q1*q2;
const float HK8 = HK3*HK7;
const float HK9 = ecl::powf(q0, 2) - ecl::powf(q1, 2) + ecl::powf(q2, 2) - ecl::powf(q3, 2);
const float HK10 = HK9*Kaccy;
const float HK11 = q0*q1 + q2*q3;
const float HK12 = 2*HK11;
const float HK13 = 2*HK7;
const float HK14 = 2*HK5;
const float HK15 = 2*HK2;
const float HK16 = 2*HK4;
const float HK17 = 2*HK6;
const float HK18 = HK12*P(0,6) + HK13*P(0,22) - HK13*P(0,4) + HK14*P(0,2) + HK15*P(0,0) + HK16*P(0,1) - HK17*P(0,3) - HK9*P(0,23) + HK9*P(0,5);
const float HK19 = ecl::powf(Kaccy, 2);
const float HK20 = HK12*P(6,6) - HK13*P(4,6) + HK13*P(6,22) + HK14*P(2,6) + HK15*P(0,6) + HK16*P(1,6) - HK17*P(3,6) + HK9*P(5,6) - HK9*P(6,23);
const float HK21 = HK13*P(4,22);
const float HK22 = HK12*P(6,22) + HK13*P(22,22) + HK14*P(2,22) + HK15*P(0,22) + HK16*P(1,22) - HK17*P(3,22) - HK21 - HK9*P(22,23) + HK9*P(5,22);
const float HK23 = HK13*HK19;
const float HK24 = HK12*P(4,6) - HK13*P(4,4) + HK14*P(2,4) + HK15*P(0,4) + HK16*P(1,4) - HK17*P(3,4) + HK21 - HK9*P(4,23) + HK9*P(4,5);
const float HK25 = HK9*P(5,23);
const float HK26 = HK12*P(5,6) - HK13*P(4,5) + HK13*P(5,22) + HK14*P(2,5) + HK15*P(0,5) + HK16*P(1,5) - HK17*P(3,5) - HK25 + HK9*P(5,5);
const float HK27 = HK19*HK9;
const float HK28 = HK12*P(6,23) + HK13*P(22,23) - HK13*P(4,23) + HK14*P(2,23) + HK15*P(0,23) + HK16*P(1,23) - HK17*P(3,23) + HK25 - HK9*P(23,23);
const float HK29 = HK12*P(2,6) + HK13*P(2,22) - HK13*P(2,4) + HK14*P(2,2) + HK15*P(0,2) + HK16*P(1,2) - HK17*P(2,3) - HK9*P(2,23) + HK9*P(2,5);
const float HK30 = HK12*P(1,6) + HK13*P(1,22) - HK13*P(1,4) + HK14*P(1,2) + HK15*P(0,1) + HK16*P(1,1) - HK17*P(1,3) - HK9*P(1,23) + HK9*P(1,5);
const float HK31 = HK12*P(3,6) + HK13*P(3,22) - HK13*P(3,4) + HK14*P(2,3) + HK15*P(0,3) + HK16*P(1,3) - HK17*P(3,3) - HK9*P(3,23) + HK9*P(3,5);
const float HK32 = Kaccy/(HK12*HK19*HK20 + HK14*HK19*HK29 + HK15*HK18*HK19 + HK16*HK19*HK30 - HK17*HK19*HK31 + HK22*HK23 - HK23*HK24 + HK26*HK27 - HK27*HK28 + R_ACC);
// Observation Jacobians
Hfusion.at<0>() = -HK2*HK3;
Hfusion.at<1>() = -HK3*HK4;
Hfusion.at<2>() = -HK3*HK5;
Hfusion.at<3>() = HK3*HK6;
Hfusion.at<4>() = HK8;
Hfusion.at<5>() = -HK10;
Hfusion.at<6>() = -HK11*HK3;
Hfusion.at<22>() = -HK8;
Hfusion.at<23>() = HK10;
// Kalman gains
Kfusion(0) = -HK18*HK32;
Kfusion(1) = -HK30*HK32;
Kfusion(2) = -HK29*HK32;
Kfusion(3) = -HK31*HK32;
Kfusion(4) = -HK24*HK32;
Kfusion(5) = -HK26*HK32;
Kfusion(6) = -HK20*HK32;
Kfusion(7) = -HK32*(HK12*P(6,7) - HK13*P(4,7) + HK13*P(7,22) + HK14*P(2,7) + HK15*P(0,7) + HK16*P(1,7) - HK17*P(3,7) + HK9*P(5,7) - HK9*P(7,23));
Kfusion(8) = -HK32*(HK12*P(6,8) - HK13*P(4,8) + HK13*P(8,22) + HK14*P(2,8) + HK15*P(0,8) + HK16*P(1,8) - HK17*P(3,8) + HK9*P(5,8) - HK9*P(8,23));
Kfusion(9) = -HK32*(HK12*P(6,9) - HK13*P(4,9) + HK13*P(9,22) + HK14*P(2,9) + HK15*P(0,9) + HK16*P(1,9) - HK17*P(3,9) + HK9*P(5,9) - HK9*P(9,23));
Kfusion(10) = -HK32*(HK12*P(6,10) + HK13*P(10,22) - HK13*P(4,10) + HK14*P(2,10) + HK15*P(0,10) + HK16*P(1,10) - HK17*P(3,10) - HK9*P(10,23) + HK9*P(5,10));
Kfusion(11) = -HK32*(HK12*P(6,11) + HK13*P(11,22) - HK13*P(4,11) + HK14*P(2,11) + HK15*P(0,11) + HK16*P(1,11) - HK17*P(3,11) - HK9*P(11,23) + HK9*P(5,11));
Kfusion(12) = -HK32*(HK12*P(6,12) + HK13*P(12,22) - HK13*P(4,12) + HK14*P(2,12) + HK15*P(0,12) + HK16*P(1,12) - HK17*P(3,12) - HK9*P(12,23) + HK9*P(5,12));
Kfusion(13) = -HK32*(HK12*P(6,13) + HK13*P(13,22) - HK13*P(4,13) + HK14*P(2,13) + HK15*P(0,13) + HK16*P(1,13) - HK17*P(3,13) - HK9*P(13,23) + HK9*P(5,13));
Kfusion(14) = -HK32*(HK12*P(6,14) + HK13*P(14,22) - HK13*P(4,14) + HK14*P(2,14) + HK15*P(0,14) + HK16*P(1,14) - HK17*P(3,14) - HK9*P(14,23) + HK9*P(5,14));
Kfusion(15) = -HK32*(HK12*P(6,15) + HK13*P(15,22) - HK13*P(4,15) + HK14*P(2,15) + HK15*P(0,15) + HK16*P(1,15) - HK17*P(3,15) - HK9*P(15,23) + HK9*P(5,15));
Kfusion(16) = -HK32*(HK12*P(6,16) + HK13*P(16,22) - HK13*P(4,16) + HK14*P(2,16) + HK15*P(0,16) + HK16*P(1,16) - HK17*P(3,16) - HK9*P(16,23) + HK9*P(5,16));
Kfusion(17) = -HK32*(HK12*P(6,17) + HK13*P(17,22) - HK13*P(4,17) + HK14*P(2,17) + HK15*P(0,17) + HK16*P(1,17) - HK17*P(3,17) - HK9*P(17,23) + HK9*P(5,17));
Kfusion(18) = -HK32*(HK12*P(6,18) + HK13*P(18,22) - HK13*P(4,18) + HK14*P(2,18) + HK15*P(0,18) + HK16*P(1,18) - HK17*P(3,18) - HK9*P(18,23) + HK9*P(5,18));
Kfusion(19) = -HK32*(HK12*P(6,19) + HK13*P(19,22) - HK13*P(4,19) + HK14*P(2,19) + HK15*P(0,19) + HK16*P(1,19) - HK17*P(3,19) - HK9*P(19,23) + HK9*P(5,19));
Kfusion(20) = -HK32*(HK12*P(6,20) + HK13*P(20,22) - HK13*P(4,20) + HK14*P(2,20) + HK15*P(0,20) + HK16*P(1,20) - HK17*P(3,20) - HK9*P(20,23) + HK9*P(5,20));
Kfusion(21) = -HK32*(HK12*P(6,21) + HK13*P(21,22) - HK13*P(4,21) + HK14*P(2,21) + HK15*P(0,21) + HK16*P(1,21) - HK17*P(3,21) - HK9*P(21,23) + HK9*P(5,21));
Kfusion(22) = -HK22*HK32;
Kfusion(23) = -HK28*HK32;
// save output
Hfusion_sympy(0) = Hfusion.at<0>();
Hfusion_sympy(1) = Hfusion.at<1>();
Hfusion_sympy(2) = Hfusion.at<2>();
Hfusion_sympy(3) = Hfusion.at<3>();
Hfusion_sympy(4) = Hfusion.at<4>();
Hfusion_sympy(5) = Hfusion.at<5>();
Hfusion_sympy(6) = Hfusion.at<6>();
Hfusion_sympy(22) = Hfusion.at<22>();
Hfusion_sympy(23) = Hfusion.at<23>();
Kfusion_sympy = Kfusion;
// repeat calculation using matlab generated equations
const float Kacc = Kaccy;
SH_ACC[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3);
SH_ACC[1] = vn - vwn;
SH_ACC[2] = ve - vwe;
H_ACC(0) = -Kacc*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd);
H_ACC(1) = -Kacc*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd);
H_ACC(2) = -Kacc*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd);
H_ACC(3) = Kacc*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd);
H_ACC(4) = Kacc*(2.0f*q0*q3 - 2.0f*q1*q2);
H_ACC(5) = -Kacc*SH_ACC[0];
H_ACC(6) = -Kacc*(2.0f*q0*q1 + 2.0f*q2*q3);
H_ACC(22) = -2.0f*Kacc*(q0*q3 - q1*q2);
H_ACC(23) = Kacc*SH_ACC[0];
drag_innov_var = (R_ACC + Kacc*SH_ACC[0]*(Kacc*P(5,5)*SH_ACC[0] - Kacc*P(23,5)*SH_ACC[0] - Kacc*P(4,5)*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P(6,5)*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P(22,5)*(q0*q3 - q1*q2) + Kacc*P(0,5)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P(1,5)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(2,5)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(3,5)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) - Kacc*SH_ACC[0]*(Kacc*P(5,23)*SH_ACC[0] - Kacc*P(23,23)*SH_ACC[0] - Kacc*P(4,23)*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P(6,23)*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P(22,23)*(q0*q3 - q1*q2) + Kacc*P(0,23)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P(1,23)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(2,23)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(3,23)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) - Kacc*(2.0f*q0*q3 - 2.0f*q1*q2)*(Kacc*P(5,4)*SH_ACC[0] - Kacc*P(23,4)*SH_ACC[0] - Kacc*P(4,4)*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P(6,4)*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P(22,4)*(q0*q3 - q1*q2) + Kacc*P(0,4)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P(1,4)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(2,4)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(3,4)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) + Kacc*(2.0f*q0*q1 + 2.0f*q2*q3)*(Kacc*P(5,6)*SH_ACC[0] - Kacc*P(23,6)*SH_ACC[0] - Kacc*P(4,6)*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P(6,6)*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P(22,6)*(q0*q3 - q1*q2) + Kacc*P(0,6)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P(1,6)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(2,6)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(3,6)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) + 2*Kacc*(q0*q3 - q1*q2)*(Kacc*P(5,22)*SH_ACC[0] - Kacc*P(23,22)*SH_ACC[0] - Kacc*P(4,22)*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P(6,22)*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P(22,22)*(q0*q3 - q1*q2) + Kacc*P(0,22)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P(1,22)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(2,22)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(3,22)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) + Kacc*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)*(Kacc*P(5,0)*SH_ACC[0] - Kacc*P(23,0)*SH_ACC[0] - Kacc*P(4,0)*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P(6,0)*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P(22,0)*(q0*q3 - q1*q2) + Kacc*P(0,0)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P(1,0)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(2,0)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(3,0)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) + Kacc*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd)*(Kacc*P(5,1)*SH_ACC[0] - Kacc*P(23,1)*SH_ACC[0] - Kacc*P(4,1)*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P(6,1)*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P(22,1)*(q0*q3 - q1*q2) + Kacc*P(0,1)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P(1,1)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(2,1)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(3,1)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) + Kacc*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd)*(Kacc*P(5,2)*SH_ACC[0] - Kacc*P(23,2)*SH_ACC[0] - Kacc*P(4,2)*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P(6,2)*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P(22,2)*(q0*q3 - q1*q2) + Kacc*P(0,2)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P(1,2)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(2,2)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(3,2)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) - Kacc*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)*(Kacc*P(5,3)*SH_ACC[0] - Kacc*P(23,3)*SH_ACC[0] - Kacc*P(4,3)*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P(6,3)*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P(22,3)*(q0*q3 - q1*q2) + Kacc*P(0,3)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P(1,3)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(2,3)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(3,3)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)));
SK_ACC[0] = 1.0f/drag_innov_var;
SK_ACC[1] = 2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd;
SK_ACC[2] = 2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd;
SK_ACC[3] = 2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd;
SK_ACC[4] = 2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd;
SK_ACC[5] = 2.0f*q0*q3 - 2.0f*q1*q2;
SK_ACC[6] = q0*q3 - q1*q2;
SK_ACC[7] = 2.0f*q0*q1 + 2.0f*q2*q3;
SK_ACC[8] = SH_ACC[0];
Kfusion(0) = -SK_ACC[0]*(Kacc*P(0,0)*SK_ACC[3] + Kacc*P(0,1)*SK_ACC[2] - Kacc*P(0,3)*SK_ACC[1] + Kacc*P(0,2)*SK_ACC[4] - Kacc*P(0,4)*SK_ACC[5] + Kacc*P(0,5)*SK_ACC[8] + Kacc*P(0,6)*SK_ACC[7] + 2*Kacc*P(0,22)*SK_ACC[6] - Kacc*P(0,23)*SK_ACC[8]);
Kfusion(1) = -SK_ACC[0]*(Kacc*P(1,0)*SK_ACC[3] + Kacc*P(1,1)*SK_ACC[2] - Kacc*P(1,3)*SK_ACC[1] + Kacc*P(1,2)*SK_ACC[4] - Kacc*P(1,4)*SK_ACC[5] + Kacc*P(1,5)*SK_ACC[8] + Kacc*P(1,6)*SK_ACC[7] + 2*Kacc*P(1,22)*SK_ACC[6] - Kacc*P(1,23)*SK_ACC[8]);
Kfusion(2) = -SK_ACC[0]*(Kacc*P(2,0)*SK_ACC[3] + Kacc*P(2,1)*SK_ACC[2] - Kacc*P(2,3)*SK_ACC[1] + Kacc*P(2,2)*SK_ACC[4] - Kacc*P(2,4)*SK_ACC[5] + Kacc*P(2,5)*SK_ACC[8] + Kacc*P(2,6)*SK_ACC[7] + 2*Kacc*P(2,22)*SK_ACC[6] - Kacc*P(2,23)*SK_ACC[8]);
Kfusion(3) = -SK_ACC[0]*(Kacc*P(3,0)*SK_ACC[3] + Kacc*P(3,1)*SK_ACC[2] - Kacc*P(3,3)*SK_ACC[1] + Kacc*P(3,2)*SK_ACC[4] - Kacc*P(3,4)*SK_ACC[5] + Kacc*P(3,5)*SK_ACC[8] + Kacc*P(3,6)*SK_ACC[7] + 2*Kacc*P(3,22)*SK_ACC[6] - Kacc*P(3,23)*SK_ACC[8]);
Kfusion(4) = -SK_ACC[0]*(Kacc*P(4,0)*SK_ACC[3] + Kacc*P(4,1)*SK_ACC[2] - Kacc*P(4,3)*SK_ACC[1] + Kacc*P(4,2)*SK_ACC[4] - Kacc*P(4,4)*SK_ACC[5] + Kacc*P(4,5)*SK_ACC[8] + Kacc*P(4,6)*SK_ACC[7] + 2*Kacc*P(4,22)*SK_ACC[6] - Kacc*P(4,23)*SK_ACC[8]);
Kfusion(5) = -SK_ACC[0]*(Kacc*P(5,0)*SK_ACC[3] + Kacc*P(5,1)*SK_ACC[2] - Kacc*P(5,3)*SK_ACC[1] + Kacc*P(5,2)*SK_ACC[4] - Kacc*P(5,4)*SK_ACC[5] + Kacc*P(5,5)*SK_ACC[8] + Kacc*P(5,6)*SK_ACC[7] + 2*Kacc*P(5,22)*SK_ACC[6] - Kacc*P(5,23)*SK_ACC[8]);
Kfusion(6) = -SK_ACC[0]*(Kacc*P(6,0)*SK_ACC[3] + Kacc*P(6,1)*SK_ACC[2] - Kacc*P(6,3)*SK_ACC[1] + Kacc*P(6,2)*SK_ACC[4] - Kacc*P(6,4)*SK_ACC[5] + Kacc*P(6,5)*SK_ACC[8] + Kacc*P(6,6)*SK_ACC[7] + 2*Kacc*P(6,22)*SK_ACC[6] - Kacc*P(6,23)*SK_ACC[8]);
Kfusion(7) = -SK_ACC[0]*(Kacc*P(7,0)*SK_ACC[3] + Kacc*P(7,1)*SK_ACC[2] - Kacc*P(7,3)*SK_ACC[1] + Kacc*P(7,2)*SK_ACC[4] - Kacc*P(7,4)*SK_ACC[5] + Kacc*P(7,5)*SK_ACC[8] + Kacc*P(7,6)*SK_ACC[7] + 2*Kacc*P(7,22)*SK_ACC[6] - Kacc*P(7,23)*SK_ACC[8]);
Kfusion(8) = -SK_ACC[0]*(Kacc*P(8,0)*SK_ACC[3] + Kacc*P(8,1)*SK_ACC[2] - Kacc*P(8,3)*SK_ACC[1] + Kacc*P(8,2)*SK_ACC[4] - Kacc*P(8,4)*SK_ACC[5] + Kacc*P(8,5)*SK_ACC[8] + Kacc*P(8,6)*SK_ACC[7] + 2*Kacc*P(8,22)*SK_ACC[6] - Kacc*P(8,23)*SK_ACC[8]);
Kfusion(9) = -SK_ACC[0]*(Kacc*P(9,0)*SK_ACC[3] + Kacc*P(9,1)*SK_ACC[2] - Kacc*P(9,3)*SK_ACC[1] + Kacc*P(9,2)*SK_ACC[4] - Kacc*P(9,4)*SK_ACC[5] + Kacc*P(9,5)*SK_ACC[8] + Kacc*P(9,6)*SK_ACC[7] + 2*Kacc*P(9,22)*SK_ACC[6] - Kacc*P(9,23)*SK_ACC[8]);
Kfusion(10) = -SK_ACC[0]*(Kacc*P(10,0)*SK_ACC[3] + Kacc*P(10,1)*SK_ACC[2] - Kacc*P(10,3)*SK_ACC[1] + Kacc*P(10,2)*SK_ACC[4] - Kacc*P(10,4)*SK_ACC[5] + Kacc*P(10,5)*SK_ACC[8] + Kacc*P(10,6)*SK_ACC[7] + 2*Kacc*P(10,22)*SK_ACC[6] - Kacc*P(10,23)*SK_ACC[8]);
Kfusion(11) = -SK_ACC[0]*(Kacc*P(11,0)*SK_ACC[3] + Kacc*P(11,1)*SK_ACC[2] - Kacc*P(11,3)*SK_ACC[1] + Kacc*P(11,2)*SK_ACC[4] - Kacc*P(11,4)*SK_ACC[5] + Kacc*P(11,5)*SK_ACC[8] + Kacc*P(11,6)*SK_ACC[7] + 2*Kacc*P(11,22)*SK_ACC[6] - Kacc*P(11,23)*SK_ACC[8]);
Kfusion(12) = -SK_ACC[0]*(Kacc*P(12,0)*SK_ACC[3] + Kacc*P(12,1)*SK_ACC[2] - Kacc*P(12,3)*SK_ACC[1] + Kacc*P(12,2)*SK_ACC[4] - Kacc*P(12,4)*SK_ACC[5] + Kacc*P(12,5)*SK_ACC[8] + Kacc*P(12,6)*SK_ACC[7] + 2*Kacc*P(12,22)*SK_ACC[6] - Kacc*P(12,23)*SK_ACC[8]);
Kfusion(13) = -SK_ACC[0]*(Kacc*P(13,0)*SK_ACC[3] + Kacc*P(13,1)*SK_ACC[2] - Kacc*P(13,3)*SK_ACC[1] + Kacc*P(13,2)*SK_ACC[4] - Kacc*P(13,4)*SK_ACC[5] + Kacc*P(13,5)*SK_ACC[8] + Kacc*P(13,6)*SK_ACC[7] + 2*Kacc*P(13,22)*SK_ACC[6] - Kacc*P(13,23)*SK_ACC[8]);
Kfusion(14) = -SK_ACC[0]*(Kacc*P(14,0)*SK_ACC[3] + Kacc*P(14,1)*SK_ACC[2] - Kacc*P(14,3)*SK_ACC[1] + Kacc*P(14,2)*SK_ACC[4] - Kacc*P(14,4)*SK_ACC[5] + Kacc*P(14,5)*SK_ACC[8] + Kacc*P(14,6)*SK_ACC[7] + 2*Kacc*P(14,22)*SK_ACC[6] - Kacc*P(14,23)*SK_ACC[8]);
Kfusion(15) = -SK_ACC[0]*(Kacc*P(15,0)*SK_ACC[3] + Kacc*P(15,1)*SK_ACC[2] - Kacc*P(15,3)*SK_ACC[1] + Kacc*P(15,2)*SK_ACC[4] - Kacc*P(15,4)*SK_ACC[5] + Kacc*P(15,5)*SK_ACC[8] + Kacc*P(15,6)*SK_ACC[7] + 2*Kacc*P(15,22)*SK_ACC[6] - Kacc*P(15,23)*SK_ACC[8]);
Kfusion(16) = -SK_ACC[0]*(Kacc*P(16,0)*SK_ACC[3] + Kacc*P(16,1)*SK_ACC[2] - Kacc*P(16,3)*SK_ACC[1] + Kacc*P(16,2)*SK_ACC[4] - Kacc*P(16,4)*SK_ACC[5] + Kacc*P(16,5)*SK_ACC[8] + Kacc*P(16,6)*SK_ACC[7] + 2*Kacc*P(16,22)*SK_ACC[6] - Kacc*P(16,23)*SK_ACC[8]);
Kfusion(17) = -SK_ACC[0]*(Kacc*P(17,0)*SK_ACC[3] + Kacc*P(17,1)*SK_ACC[2] - Kacc*P(17,3)*SK_ACC[1] + Kacc*P(17,2)*SK_ACC[4] - Kacc*P(17,4)*SK_ACC[5] + Kacc*P(17,5)*SK_ACC[8] + Kacc*P(17,6)*SK_ACC[7] + 2*Kacc*P(17,22)*SK_ACC[6] - Kacc*P(17,23)*SK_ACC[8]);
Kfusion(18) = -SK_ACC[0]*(Kacc*P(18,0)*SK_ACC[3] + Kacc*P(18,1)*SK_ACC[2] - Kacc*P(18,3)*SK_ACC[1] + Kacc*P(18,2)*SK_ACC[4] - Kacc*P(18,4)*SK_ACC[5] + Kacc*P(18,5)*SK_ACC[8] + Kacc*P(18,6)*SK_ACC[7] + 2*Kacc*P(18,22)*SK_ACC[6] - Kacc*P(18,23)*SK_ACC[8]);
Kfusion(19) = -SK_ACC[0]*(Kacc*P(19,0)*SK_ACC[3] + Kacc*P(19,1)*SK_ACC[2] - Kacc*P(19,3)*SK_ACC[1] + Kacc*P(19,2)*SK_ACC[4] - Kacc*P(19,4)*SK_ACC[5] + Kacc*P(19,5)*SK_ACC[8] + Kacc*P(19,6)*SK_ACC[7] + 2*Kacc*P(19,22)*SK_ACC[6] - Kacc*P(19,23)*SK_ACC[8]);
Kfusion(20) = -SK_ACC[0]*(Kacc*P(20,0)*SK_ACC[3] + Kacc*P(20,1)*SK_ACC[2] - Kacc*P(20,3)*SK_ACC[1] + Kacc*P(20,2)*SK_ACC[4] - Kacc*P(20,4)*SK_ACC[5] + Kacc*P(20,5)*SK_ACC[8] + Kacc*P(20,6)*SK_ACC[7] + 2*Kacc*P(20,22)*SK_ACC[6] - Kacc*P(20,23)*SK_ACC[8]);
Kfusion(21) = -SK_ACC[0]*(Kacc*P(21,0)*SK_ACC[3] + Kacc*P(21,1)*SK_ACC[2] - Kacc*P(21,3)*SK_ACC[1] + Kacc*P(21,2)*SK_ACC[4] - Kacc*P(21,4)*SK_ACC[5] + Kacc*P(21,5)*SK_ACC[8] + Kacc*P(21,6)*SK_ACC[7] + 2*Kacc*P(21,22)*SK_ACC[6] - Kacc*P(21,23)*SK_ACC[8]);
Kfusion(22) = -SK_ACC[0]*(Kacc*P(22,0)*SK_ACC[3] + Kacc*P(22,1)*SK_ACC[2] - Kacc*P(22,3)*SK_ACC[1] + Kacc*P(22,2)*SK_ACC[4] - Kacc*P(22,4)*SK_ACC[5] + Kacc*P(22,5)*SK_ACC[8] + Kacc*P(22,6)*SK_ACC[7] + 2*Kacc*P(22,22)*SK_ACC[6] - Kacc*P(22,23)*SK_ACC[8]);
Kfusion(23) = -SK_ACC[0]*(Kacc*P(23,0)*SK_ACC[3] + Kacc*P(23,1)*SK_ACC[2] - Kacc*P(23,3)*SK_ACC[1] + Kacc*P(23,2)*SK_ACC[4] - Kacc*P(23,4)*SK_ACC[5] + Kacc*P(23,5)*SK_ACC[8] + Kacc*P(23,6)*SK_ACC[7] + 2*Kacc*P(23,22)*SK_ACC[6] - Kacc*P(23,23)*SK_ACC[8]);
Hfusion_matlab = H_ACC;
Kfusion_matlab = Kfusion;
// 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 (Hfusion_matlab(row) != 0.0f) {
diff_fraction = fabsf(Hfusion_sympy(row) - Hfusion_matlab(row)) / fabsf(Hfusion_matlab(row));
} else if (Hfusion_sympy(row) != 0.0f) {
diff_fraction = fabsf(Hfusion_sympy(row) - H_ACC(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_ACC(row);
max_new = Hfusion_sympy(row);
}
}
if (max_diff_fraction > 1e-5f) {
printf("Fail: Specific Force 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: Specific Force 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: Specific Force 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: Specific Force Y axis Kfusion max diff fraction = %e\n",max_diff_fraction);
}
}
return 0;
}

166
src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp
View File

@ -1,166 +0,0 @@
// Axis 0 equations
// Sub Expressions
const float HK0 = vn - vwn;
const float HK1 = ve - vwe;
const float HK2 = HK0*q0 + HK1*q3 - q2*vd;
const float HK3 = 2*Kaccx;
const float HK4 = HK0*q1 + HK1*q2 + q3*vd;
const float HK5 = HK0*q2 - HK1*q1 + q0*vd;
const float HK6 = -HK0*q3 + HK1*q0 + q1*vd;
const float HK7 = (q0)*(q0) + (q1)*(q1) - (q2)*(q2) - (q3)*(q3);
const float HK8 = HK7*Kaccx;
const float HK9 = q0*q3 + q1*q2;
const float HK10 = HK3*HK9;
const float HK11 = q0*q2 - q1*q3;
const float HK12 = 2*HK5;
const float HK13 = 2*HK11;
const float HK14 = 2*HK9;
const float HK15 = 2*HK2;
const float HK16 = 2*HK4;
const float HK17 = 2*HK6;
const float HK18 = -HK14*P(0,23) + HK14*P(0,5) + HK15*P(0,0) + HK16*P(0,1) + HK17*P(0,3) + HK7*P(0,4);
const float HK19 = (Kaccx)*(Kaccx);
const float HK20 = -HK7;
const float HK21 = -2*HK5;
const float HK22 = -2*HK11;
const float HK23 = HK14*P(5,23);
const float HK24 = -HK14*P(23,23) + HK15*P(0,23) + HK16*P(1,23) + HK17*P(3,23) + HK23 + HK7*P(4,23);
const float HK25 = HK14*P(5,5) + HK15*P(0,5) + HK16*P(1,5) + HK17*P(3,5) - HK23 + HK7*P(4,5);
const float HK26 = HK14*P(5,6) - HK14*P(6,23) + HK15*P(0,6) + HK16*P(1,6) + HK17*P(3,6) + HK7*P(4,6);
const float HK27 = -HK14*P(4,23) + HK14*P(4,5) + HK15*P(0,4) + HK16*P(1,4) + HK17*P(3,4) + HK7*P(4,4);
const float HK28 = HK7*P(4,22);
const float HK29 = -HK14*P(22,23) + HK14*P(5,22) + HK15*P(0,22) + HK16*P(1,22) + HK17*P(3,22) + HK28;
const float HK30 = -HK14*P(1,23) + HK14*P(1,5) + HK15*P(0,1) + HK16*P(1,1) + HK17*P(1,3) + HK7*P(1,4);
const float HK31 = -HK14*P(2,23) + HK14*P(2,5) + HK15*P(0,2) + HK16*P(1,2) + HK17*P(2,3) + HK7*P(2,4);
const float HK32 = -HK14*P(3,23) + HK14*P(3,5) + HK15*P(0,3) + HK16*P(1,3) + HK17*P(3,3) + HK7*P(3,4);
const float HK33 = Kaccx/(2*HK11*HK19*(HK20*P(6,22) + HK21*P(2,6) + HK22*P(6,6) + HK26) - HK14*HK19*(HK20*P(5,22) + HK21*P(2,5) + HK22*P(5,6) + HK25) - HK15*HK19*(HK18 + HK20*P(0,22) + HK21*P(0,2) + HK22*P(0,6)) - HK16*HK19*(HK20*P(1,22) + HK21*P(1,2) + HK22*P(1,6) + HK30) - HK17*HK19*(HK20*P(3,22) + HK21*P(2,3) + HK22*P(3,6) + HK32) + 2*HK19*HK5*(HK20*P(2,22) + HK21*P(2,2) + HK22*P(2,6) + HK31) + HK19*HK7*(HK20*P(22,22) + HK21*P(2,22) + HK22*P(6,22) + HK29) - HK19*HK7*(HK20*P(4,22) + HK21*P(2,4) + HK22*P(4,6) + HK27) + 2*HK19*HK9*(HK20*P(22,23) + HK21*P(2,23) + HK22*P(6,23) + HK24) - R_ACC);
// Observation Jacobians
Hfusion.at<0>() = -HK2*HK3;
Hfusion.at<1>() = -HK3*HK4;
Hfusion.at<2>() = HK3*HK5;
Hfusion.at<3>() = -HK3*HK6;
Hfusion.at<4>() = -HK8;
Hfusion.at<5>() = -HK10;
Hfusion.at<6>() = HK11*HK3;
Hfusion.at<22>() = HK8;
Hfusion.at<23>() = HK10;
// Kalman gains
Kfusion(0) = HK33*(-HK12*P(0,2) - HK13*P(0,6) + HK18 - HK7*P(0,22));
Kfusion(1) = HK33*(-HK12*P(1,2) - HK13*P(1,6) + HK30 - HK7*P(1,22));
Kfusion(2) = HK33*(-HK12*P(2,2) - HK13*P(2,6) + HK31 - HK7*P(2,22));
Kfusion(3) = HK33*(-HK12*P(2,3) - HK13*P(3,6) + HK32 - HK7*P(3,22));
Kfusion(4) = HK33*(-HK12*P(2,4) - HK13*P(4,6) + HK27 - HK28);
Kfusion(5) = HK33*(-HK12*P(2,5) - HK13*P(5,6) + HK25 - HK7*P(5,22));
Kfusion(6) = HK33*(-HK12*P(2,6) - HK13*P(6,6) + HK26 - HK7*P(6,22));
Kfusion(7) = HK33*(-HK12*P(2,7) - HK13*P(6,7) + HK14*P(5,7) - HK14*P(7,23) + HK15*P(0,7) + HK16*P(1,7) + HK17*P(3,7) + HK7*P(4,7) - HK7*P(7,22));
Kfusion(8) = HK33*(-HK12*P(2,8) - HK13*P(6,8) + HK14*P(5,8) - HK14*P(8,23) + HK15*P(0,8) + HK16*P(1,8) + HK17*P(3,8) + HK7*P(4,8) - HK7*P(8,22));
Kfusion(9) = HK33*(-HK12*P(2,9) - HK13*P(6,9) + HK14*P(5,9) - HK14*P(9,23) + HK15*P(0,9) + HK16*P(1,9) + HK17*P(3,9) + HK7*P(4,9) - HK7*P(9,22));
Kfusion(10) = HK33*(-HK12*P(2,10) - HK13*P(6,10) - HK14*P(10,23) + HK14*P(5,10) + HK15*P(0,10) + HK16*P(1,10) + HK17*P(3,10) - HK7*P(10,22) + HK7*P(4,10));
Kfusion(11) = HK33*(-HK12*P(2,11) - HK13*P(6,11) - HK14*P(11,23) + HK14*P(5,11) + HK15*P(0,11) + HK16*P(1,11) + HK17*P(3,11) - HK7*P(11,22) + HK7*P(4,11));
Kfusion(12) = HK33*(-HK12*P(2,12) - HK13*P(6,12) - HK14*P(12,23) + HK14*P(5,12) + HK15*P(0,12) + HK16*P(1,12) + HK17*P(3,12) - HK7*P(12,22) + HK7*P(4,12));
Kfusion(13) = HK33*(-HK12*P(2,13) - HK13*P(6,13) - HK14*P(13,23) + HK14*P(5,13) + HK15*P(0,13) + HK16*P(1,13) + HK17*P(3,13) - HK7*P(13,22) + HK7*P(4,13));
Kfusion(14) = HK33*(-HK12*P(2,14) - HK13*P(6,14) - HK14*P(14,23) + HK14*P(5,14) + HK15*P(0,14) + HK16*P(1,14) + HK17*P(3,14) - HK7*P(14,22) + HK7*P(4,14));
Kfusion(15) = HK33*(-HK12*P(2,15) - HK13*P(6,15) - HK14*P(15,23) + HK14*P(5,15) + HK15*P(0,15) + HK16*P(1,15) + HK17*P(3,15) - HK7*P(15,22) + HK7*P(4,15));
Kfusion(16) = HK33*(-HK12*P(2,16) - HK13*P(6,16) - HK14*P(16,23) + HK14*P(5,16) + HK15*P(0,16) + HK16*P(1,16) + HK17*P(3,16) - HK7*P(16,22) + HK7*P(4,16));
Kfusion(17) = HK33*(-HK12*P(2,17) - HK13*P(6,17) - HK14*P(17,23) + HK14*P(5,17) + HK15*P(0,17) + HK16*P(1,17) + HK17*P(3,17) - HK7*P(17,22) + HK7*P(4,17));
Kfusion(18) = HK33*(-HK12*P(2,18) - HK13*P(6,18) - HK14*P(18,23) + HK14*P(5,18) + HK15*P(0,18) + HK16*P(1,18) + HK17*P(3,18) - HK7*P(18,22) + HK7*P(4,18));
Kfusion(19) = HK33*(-HK12*P(2,19) - HK13*P(6,19) - HK14*P(19,23) + HK14*P(5,19) + HK15*P(0,19) + HK16*P(1,19) + HK17*P(3,19) - HK7*P(19,22) + HK7*P(4,19));
Kfusion(20) = HK33*(-HK12*P(2,20) - HK13*P(6,20) - HK14*P(20,23) + HK14*P(5,20) + HK15*P(0,20) + HK16*P(1,20) + HK17*P(3,20) - HK7*P(20,22) + HK7*P(4,20));
Kfusion(21) = HK33*(-HK12*P(2,21) - HK13*P(6,21) - HK14*P(21,23) + HK14*P(5,21) + HK15*P(0,21) + HK16*P(1,21) + HK17*P(3,21) - HK7*P(21,22) + HK7*P(4,21));
Kfusion(22) = HK33*(-HK12*P(2,22) - HK13*P(6,22) + HK29 - HK7*P(22,22));
Kfusion(23) = HK33*(-HK12*P(2,23) - HK13*P(6,23) + HK24 - HK7*P(22,23));
// Predicted observation
// Innovation variance
// Axis 1 equations
// Sub Expressions
const float HK0 = ve - vwe;
const float HK1 = vn - vwn;
const float HK2 = HK0*q0 - HK1*q3 + q1*vd;
const float HK3 = 2*Kaccy;
const float HK4 = -HK0*q1 + HK1*q2 + q0*vd;
const float HK5 = HK0*q2 + HK1*q1 + q3*vd;
const float HK6 = HK0*q3 + HK1*q0 - q2*vd;
const float HK7 = q0*q3 - q1*q2;
const float HK8 = HK3*HK7;
const float HK9 = (q0)*(q0) - (q1)*(q1) + (q2)*(q2) - (q3)*(q3);
const float HK10 = HK9*Kaccy;
const float HK11 = q0*q1 + q2*q3;
const float HK12 = 2*HK6;
const float HK13 = 2*HK7;
const float HK14 = 2*HK2;
const float HK15 = 2*HK4;
const float HK16 = 2*HK5;
const float HK17 = 2*HK11;
const float HK18 = HK13*P(0,22) + HK14*P(0,0) + HK15*P(0,1) + HK16*P(0,2) + HK17*P(0,6) + HK9*P(0,5);
const float HK19 = (Kaccy)*(Kaccy);
const float HK20 = -HK9;
const float HK21 = -2*HK6;
const float HK22 = -2*HK7;
const float HK23 = HK13*P(6,22) + HK14*P(0,6) + HK15*P(1,6) + HK16*P(2,6) + HK17*P(6,6) + HK9*P(5,6);
const float HK24 = HK13*P(22,22) + HK14*P(0,22) + HK15*P(1,22) + HK16*P(2,22) + HK17*P(6,22) + HK9*P(5,22);
const float HK25 = HK13*P(4,22);
const float HK26 = HK14*P(0,4) + HK15*P(1,4) + HK16*P(2,4) + HK17*P(4,6) + HK25 + HK9*P(4,5);
const float HK27 = HK13*P(5,22) + HK14*P(0,5) + HK15*P(1,5) + HK16*P(2,5) + HK17*P(5,6) + HK9*P(5,5);
const float HK28 = HK9*P(5,23);
const float HK29 = HK13*P(22,23) + HK14*P(0,23) + HK15*P(1,23) + HK16*P(2,23) + HK17*P(6,23) + HK28;
const float HK30 = HK13*P(2,22) + HK14*P(0,2) + HK15*P(1,2) + HK16*P(2,2) + HK17*P(2,6) + HK9*P(2,5);
const float HK31 = HK13*P(1,22) + HK14*P(0,1) + HK15*P(1,1) + HK16*P(1,2) + HK17*P(1,6) + HK9*P(1,5);
const float HK32 = HK13*P(3,22) + HK14*P(0,3) + HK15*P(1,3) + HK16*P(2,3) + HK17*P(3,6) + HK9*P(3,5);
const float HK33 = Kaccy/(-HK13*HK19*(HK20*P(22,23) + HK21*P(3,22) + HK22*P(4,22) + HK24) - HK14*HK19*(HK18 + HK20*P(0,23) + HK21*P(0,3) + HK22*P(0,4)) - HK15*HK19*(HK20*P(1,23) + HK21*P(1,3) + HK22*P(1,4) + HK31) - HK16*HK19*(HK20*P(2,23) + HK21*P(2,3) + HK22*P(2,4) + HK30) - HK17*HK19*(HK20*P(6,23) + HK21*P(3,6) + HK22*P(4,6) + HK23) + 2*HK19*HK6*(HK20*P(3,23) + HK21*P(3,3) + HK22*P(3,4) + HK32) + 2*HK19*HK7*(HK20*P(4,23) + HK21*P(3,4) + HK22*P(4,4) + HK26) + HK19*HK9*(HK20*P(23,23) + HK21*P(3,23) + HK22*P(4,23) + HK29) - HK19*HK9*(HK20*P(5,23) + HK21*P(3,5) + HK22*P(4,5) + HK27) - R_ACC);
// Observation Jacobians
Hfusion.at<0>() = -HK2*HK3;
Hfusion.at<1>() = -HK3*HK4;
Hfusion.at<2>() = -HK3*HK5;
Hfusion.at<3>() = HK3*HK6;
Hfusion.at<4>() = HK8;
Hfusion.at<5>() = -HK10;
Hfusion.at<6>() = -HK11*HK3;
Hfusion.at<22>() = -HK8;
Hfusion.at<23>() = HK10;
// Kalman gains
Kfusion(0) = HK33*(-HK12*P(0,3) - HK13*P(0,4) + HK18 - HK9*P(0,23));
Kfusion(1) = HK33*(-HK12*P(1,3) - HK13*P(1,4) + HK31 - HK9*P(1,23));
Kfusion(2) = HK33*(-HK12*P(2,3) - HK13*P(2,4) + HK30 - HK9*P(2,23));
Kfusion(3) = HK33*(-HK12*P(3,3) - HK13*P(3,4) + HK32 - HK9*P(3,23));
Kfusion(4) = HK33*(-HK12*P(3,4) - HK13*P(4,4) + HK26 - HK9*P(4,23));
Kfusion(5) = HK33*(-HK12*P(3,5) - HK13*P(4,5) + HK27 - HK28);
Kfusion(6) = HK33*(-HK12*P(3,6) - HK13*P(4,6) + HK23 - HK9*P(6,23));
Kfusion(7) = HK33*(-HK12*P(3,7) - HK13*P(4,7) + HK13*P(7,22) + HK14*P(0,7) + HK15*P(1,7) + HK16*P(2,7) + HK17*P(6,7) + HK9*P(5,7) - HK9*P(7,23));
Kfusion(8) = HK33*(-HK12*P(3,8) - HK13*P(4,8) + HK13*P(8,22) + HK14*P(0,8) + HK15*P(1,8) + HK16*P(2,8) + HK17*P(6,8) + HK9*P(5,8) - HK9*P(8,23));
Kfusion(9) = HK33*(-HK12*P(3,9) - HK13*P(4,9) + HK13*P(9,22) + HK14*P(0,9) + HK15*P(1,9) + HK16*P(2,9) + HK17*P(6,9) + HK9*P(5,9) - HK9*P(9,23));
Kfusion(10) = HK33*(-HK12*P(3,10) + HK13*P(10,22) - HK13*P(4,10) + HK14*P(0,10) + HK15*P(1,10) + HK16*P(2,10) + HK17*P(6,10) - HK9*P(10,23) + HK9*P(5,10));
Kfusion(11) = HK33*(-HK12*P(3,11) + HK13*P(11,22) - HK13*P(4,11) + HK14*P(0,11) + HK15*P(1,11) + HK16*P(2,11) + HK17*P(6,11) - HK9*P(11,23) + HK9*P(5,11));
Kfusion(12) = HK33*(-HK12*P(3,12) + HK13*P(12,22) - HK13*P(4,12) + HK14*P(0,12) + HK15*P(1,12) + HK16*P(2,12) + HK17*P(6,12) - HK9*P(12,23) + HK9*P(5,12));
Kfusion(13) = HK33*(-HK12*P(3,13) + HK13*P(13,22) - HK13*P(4,13) + HK14*P(0,13) + HK15*P(1,13) + HK16*P(2,13) + HK17*P(6,13) - HK9*P(13,23) + HK9*P(5,13));
Kfusion(14) = HK33*(-HK12*P(3,14) + HK13*P(14,22) - HK13*P(4,14) + HK14*P(0,14) + HK15*P(1,14) + HK16*P(2,14) + HK17*P(6,14) - HK9*P(14,23) + HK9*P(5,14));
Kfusion(15) = HK33*(-HK12*P(3,15) + HK13*P(15,22) - HK13*P(4,15) + HK14*P(0,15) + HK15*P(1,15) + HK16*P(2,15) + HK17*P(6,15) - HK9*P(15,23) + HK9*P(5,15));
Kfusion(16) = HK33*(-HK12*P(3,16) + HK13*P(16,22) - HK13*P(4,16) + HK14*P(0,16) + HK15*P(1,16) + HK16*P(2,16) + HK17*P(6,16) - HK9*P(16,23) + HK9*P(5,16));
Kfusion(17) = HK33*(-HK12*P(3,17) + HK13*P(17,22) - HK13*P(4,17) + HK14*P(0,17) + HK15*P(1,17) + HK16*P(2,17) + HK17*P(6,17) - HK9*P(17,23) + HK9*P(5,17));
Kfusion(18) = HK33*(-HK12*P(3,18) + HK13*P(18,22) - HK13*P(4,18) + HK14*P(0,18) + HK15*P(1,18) + HK16*P(2,18) + HK17*P(6,18) - HK9*P(18,23) + HK9*P(5,18));
Kfusion(19) = HK33*(-HK12*P(3,19) + HK13*P(19,22) - HK13*P(4,19) + HK14*P(0,19) + HK15*P(1,19) + HK16*P(2,19) + HK17*P(6,19) - HK9*P(19,23) + HK9*P(5,19));
Kfusion(20) = HK33*(-HK12*P(3,20) + HK13*P(20,22) - HK13*P(4,20) + HK14*P(0,20) + HK15*P(1,20) + HK16*P(2,20) + HK17*P(6,20) - HK9*P(20,23) + HK9*P(5,20));
Kfusion(21) = HK33*(-HK12*P(3,21) + HK13*P(21,22) - HK13*P(4,21) + HK14*P(0,21) + HK15*P(1,21) + HK16*P(2,21) + HK17*P(6,21) - HK9*P(21,23) + HK9*P(5,21));
Kfusion(22) = HK33*(-HK12*P(3,22) + HK24 - HK25 - HK9*P(22,23));
Kfusion(23) = HK33*(-HK12*P(3,23) - HK13*P(4,23) + HK29 - HK9*P(23,23));
// Predicted observation
// Innovation variance

154
src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp
View File

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

24
src/modules/ekf2/EKF/python/ekf_derivation/generated/util.h
View File

@ -1,24 +0,0 @@
#include "../../../../../matrix/matrix/math.hpp"
typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
inline float sq(float in) {
return in * in;
}
namespace ecl{
inline float powf(float x, int exp)
{
float ret;
if (exp > 0) {
ret = x;
for (int count = 1; count < exp; count++) {
ret *= x;
}
return ret;
} else if (exp < 0) {
return 1.0f / ecl::powf(x, -exp);
}
return 1.0f;
}
}

177
src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp
View File

@ -1,177 +0,0 @@
// axis 0
const float HK0 = q0*vn - q2*vd + q3*ve;
const float HK1 = q1*vn + q2*ve + q3*vd;
const float HK2 = q0*vd - q1*ve + q2*vn;
const float HK3 = q0*ve + q1*vd - q3*vn;
const float HK4 = (q0)*(q0) + (q1)*(q1) - (q2)*(q2) - (q3)*(q3);
const float HK5 = q0*q3 + q1*q2;
const float HK6 = q0*q2 - q1*q3;
const float HK7 = 2*HK5;
const float HK8 = 2*HK6;
const float HK9 = 2*HK1;
const float HK10 = 2*HK0;
const float HK11 = 2*HK2;
const float HK12 = 2*HK3;
const float HK13 = HK10*P(0,0) - HK11*P(0,2) + HK12*P(0,3) + HK4*P(0,4) + HK7*P(0,5) - HK8*P(0,6) + HK9*P(0,1);
const float HK14 = HK10*P(0,5) - HK11*P(2,5) + HK12*P(3,5) + HK4*P(4,5) + HK7*P(5,5) - HK8*P(5,6) + HK9*P(1,5);
const float HK15 = HK10*P(0,6) - HK11*P(2,6) + HK12*P(3,6) + HK4*P(4,6) + HK7*P(5,6) - HK8*P(6,6) + HK9*P(1,6);
const float HK16 = HK10*P(0,1) - HK11*P(1,2) + HK12*P(1,3) + HK4*P(1,4) + HK7*P(1,5) - HK8*P(1,6) + HK9*P(1,1);
const float HK17 = HK10*P(0,2) - HK11*P(2,2) + HK12*P(2,3) + HK4*P(2,4) + HK7*P(2,5) - HK8*P(2,6) + HK9*P(1,2);
const float HK18 = HK10*P(0,3) - HK11*P(2,3) + HK12*P(3,3) + HK4*P(3,4) + HK7*P(3,5) - HK8*P(3,6) + HK9*P(1,3);
const float HK19 = HK10*P(0,4) - HK11*P(2,4) + HK12*P(3,4) + HK4*P(4,4) + HK7*P(4,5) - HK8*P(4,6) + HK9*P(1,4);
const float HK20 = 1.0F/(HK10*HK13 - HK11*HK17 + HK12*HK18 + HK14*HK7 - HK15*HK8 + HK16*HK9 + HK19*HK4 + R_VEL);
H_VEL(0) = 2*HK0;
H_VEL(1) = 2*HK1;
H_VEL(2) = -2*HK2;
H_VEL(3) = 2*HK3;
H_VEL(4) = HK4;
H_VEL(5) = 2*HK5;
H_VEL(6) = -2*HK6;
Kfusion(0) = HK13*HK20;
Kfusion(1) = HK16*HK20;
Kfusion(2) = HK17*HK20;
Kfusion(3) = HK18*HK20;
Kfusion(4) = HK19*HK20;
Kfusion(5) = HK14*HK20;
Kfusion(6) = HK15*HK20;
Kfusion(7) = HK20*(HK10*P(0,7) - HK11*P(2,7) + HK12*P(3,7) + HK4*P(4,7) + HK7*P(5,7) - HK8*P(6,7) + HK9*P(1,7));
Kfusion(8) = HK20*(HK10*P(0,8) - HK11*P(2,8) + HK12*P(3,8) + HK4*P(4,8) + HK7*P(5,8) - HK8*P(6,8) + HK9*P(1,8));
Kfusion(9) = HK20*(HK10*P(0,9) - HK11*P(2,9) + HK12*P(3,9) + HK4*P(4,9) + HK7*P(5,9) - HK8*P(6,9) + HK9*P(1,9));
Kfusion(10) = HK20*(HK10*P(0,10) - HK11*P(2,10) + HK12*P(3,10) + HK4*P(4,10) + HK7*P(5,10) - HK8*P(6,10) + HK9*P(1,10));
Kfusion(11) = HK20*(HK10*P(0,11) - HK11*P(2,11) + HK12*P(3,11) + HK4*P(4,11) + HK7*P(5,11) - HK8*P(6,11) + HK9*P(1,11));
Kfusion(12) = HK20*(HK10*P(0,12) - HK11*P(2,12) + HK12*P(3,12) + HK4*P(4,12) + HK7*P(5,12) - HK8*P(6,12) + HK9*P(1,12));
Kfusion(13) = HK20*(HK10*P(0,13) - HK11*P(2,13) + HK12*P(3,13) + HK4*P(4,13) + HK7*P(5,13) - HK8*P(6,13) + HK9*P(1,13));
Kfusion(14) = HK20*(HK10*P(0,14) - HK11*P(2,14) + HK12*P(3,14) + HK4*P(4,14) + HK7*P(5,14) - HK8*P(6,14) + HK9*P(1,14));
Kfusion(15) = HK20*(HK10*P(0,15) - HK11*P(2,15) + HK12*P(3,15) + HK4*P(4,15) + HK7*P(5,15) - HK8*P(6,15) + HK9*P(1,15));
Kfusion(16) = HK20*(HK10*P(0,16) - HK11*P(2,16) + HK12*P(3,16) + HK4*P(4,16) + HK7*P(5,16) - HK8*P(6,16) + HK9*P(1,16));
Kfusion(17) = HK20*(HK10*P(0,17) - HK11*P(2,17) + HK12*P(3,17) + HK4*P(4,17) + HK7*P(5,17) - HK8*P(6,17) + HK9*P(1,17));
Kfusion(18) = HK20*(HK10*P(0,18) - HK11*P(2,18) + HK12*P(3,18) + HK4*P(4,18) + HK7*P(5,18) - HK8*P(6,18) + HK9*P(1,18));
Kfusion(19) = HK20*(HK10*P(0,19) - HK11*P(2,19) + HK12*P(3,19) + HK4*P(4,19) + HK7*P(5,19) - HK8*P(6,19) + HK9*P(1,19));
Kfusion(20) = HK20*(HK10*P(0,20) - HK11*P(2,20) + HK12*P(3,20) + HK4*P(4,20) + HK7*P(5,20) - HK8*P(6,20) + HK9*P(1,20));
Kfusion(21) = HK20*(HK10*P(0,21) - HK11*P(2,21) + HK12*P(3,21) + HK4*P(4,21) + HK7*P(5,21) - HK8*P(6,21) + HK9*P(1,21));
Kfusion(22) = HK20*(HK10*P(0,22) - HK11*P(2,22) + HK12*P(3,22) + HK4*P(4,22) + HK7*P(5,22) - HK8*P(6,22) + HK9*P(1,22));
Kfusion(23) = HK20*(HK10*P(0,23) - HK11*P(2,23) + HK12*P(3,23) + HK4*P(4,23) + HK7*P(5,23) - HK8*P(6,23) + HK9*P(1,23));
// axis 1
const float HK0 = q0*ve + q1*vd - q3*vn;
const float HK1 = q0*vd - q1*ve + q2*vn;
const float HK2 = q1*vn + q2*ve + q3*vd;
const float HK3 = q0*vn - q2*vd + q3*ve;
const float HK4 = q0*q3 - q1*q2;
const float HK5 = (q0)*(q0) - (q1)*(q1) + (q2)*(q2) - (q3)*(q3);
const float HK6 = q0*q1 + q2*q3;
const float HK7 = 2*HK6;
const float HK8 = 2*HK4;
const float HK9 = 2*HK2;
const float HK10 = 2*HK0;
const float HK11 = 2*HK1;
const float HK12 = 2*HK3;
const float HK13 = HK10*P(0,0) + HK11*P(0,1) - HK12*P(0,3) + HK5*P(0,5) + HK7*P(0,6) - HK8*P(0,4) + HK9*P(0,2);
const float HK14 = HK10*P(0,6) + HK11*P(1,6) - HK12*P(3,6) + HK5*P(5,6) + HK7*P(6,6) - HK8*P(4,6) + HK9*P(2,6);
const float HK15 = HK10*P(0,4) + HK11*P(1,4) - HK12*P(3,4) + HK5*P(4,5) + HK7*P(4,6) - HK8*P(4,4) + HK9*P(2,4);
const float HK16 = HK10*P(0,2) + HK11*P(1,2) - HK12*P(2,3) + HK5*P(2,5) + HK7*P(2,6) - HK8*P(2,4) + HK9*P(2,2);
const float HK17 = HK10*P(0,1) + HK11*P(1,1) - HK12*P(1,3) + HK5*P(1,5) + HK7*P(1,6) - HK8*P(1,4) + HK9*P(1,2);
const float HK18 = HK10*P(0,3) + HK11*P(1,3) - HK12*P(3,3) + HK5*P(3,5) + HK7*P(3,6) - HK8*P(3,4) + HK9*P(2,3);
const float HK19 = HK10*P(0,5) + HK11*P(1,5) - HK12*P(3,5) + HK5*P(5,5) + HK7*P(5,6) - HK8*P(4,5) + HK9*P(2,5);
const float HK20 = 1.0F/(HK10*HK13 + HK11*HK17 - HK12*HK18 + HK14*HK7 - HK15*HK8 + HK16*HK9 + HK19*HK5 + R_VEL);
H_VEL(0) = 2*HK0;
H_VEL(1) = 2*HK1;
H_VEL(2) = 2*HK2;
H_VEL(3) = -2*HK3;
H_VEL(4) = -2*HK4;
H_VEL(5) = HK5;
H_VEL(6) = 2*HK6;
Kfusion(0) = HK13*HK20;
Kfusion(1) = HK17*HK20;
Kfusion(2) = HK16*HK20;
Kfusion(3) = HK18*HK20;
Kfusion(4) = HK15*HK20;
Kfusion(5) = HK19*HK20;
Kfusion(6) = HK14*HK20;
Kfusion(7) = HK20*(HK10*P(0,7) + HK11*P(1,7) - HK12*P(3,7) + HK5*P(5,7) + HK7*P(6,7) - HK8*P(4,7) + HK9*P(2,7));
Kfusion(8) = HK20*(HK10*P(0,8) + HK11*P(1,8) - HK12*P(3,8) + HK5*P(5,8) + HK7*P(6,8) - HK8*P(4,8) + HK9*P(2,8));
Kfusion(9) = HK20*(HK10*P(0,9) + HK11*P(1,9) - HK12*P(3,9) + HK5*P(5,9) + HK7*P(6,9) - HK8*P(4,9) + HK9*P(2,9));
Kfusion(10) = HK20*(HK10*P(0,10) + HK11*P(1,10) - HK12*P(3,10) + HK5*P(5,10) + HK7*P(6,10) - HK8*P(4,10) + HK9*P(2,10));
Kfusion(11) = HK20*(HK10*P(0,11) + HK11*P(1,11) - HK12*P(3,11) + HK5*P(5,11) + HK7*P(6,11) - HK8*P(4,11) + HK9*P(2,11));
Kfusion(12) = HK20*(HK10*P(0,12) + HK11*P(1,12) - HK12*P(3,12) + HK5*P(5,12) + HK7*P(6,12) - HK8*P(4,12) + HK9*P(2,12));
Kfusion(13) = HK20*(HK10*P(0,13) + HK11*P(1,13) - HK12*P(3,13) + HK5*P(5,13) + HK7*P(6,13) - HK8*P(4,13) + HK9*P(2,13));
Kfusion(14) = HK20*(HK10*P(0,14) + HK11*P(1,14) - HK12*P(3,14) + HK5*P(5,14) + HK7*P(6,14) - HK8*P(4,14) + HK9*P(2,14));
Kfusion(15) = HK20*(HK10*P(0,15) + HK11*P(1,15) - HK12*P(3,15) + HK5*P(5,15) + HK7*P(6,15) - HK8*P(4,15) + HK9*P(2,15));
Kfusion(16) = HK20*(HK10*P(0,16) + HK11*P(1,16) - HK12*P(3,16) + HK5*P(5,16) + HK7*P(6,16) - HK8*P(4,16) + HK9*P(2,16));
Kfusion(17) = HK20*(HK10*P(0,17) + HK11*P(1,17) - HK12*P(3,17) + HK5*P(5,17) + HK7*P(6,17) - HK8*P(4,17) + HK9*P(2,17));
Kfusion(18) = HK20*(HK10*P(0,18) + HK11*P(1,18) - HK12*P(3,18) + HK5*P(5,18) + HK7*P(6,18) - HK8*P(4,18) + HK9*P(2,18));
Kfusion(19) = HK20*(HK10*P(0,19) + HK11*P(1,19) - HK12*P(3,19) + HK5*P(5,19) + HK7*P(6,19) - HK8*P(4,19) + HK9*P(2,19));
Kfusion(20) = HK20*(HK10*P(0,20) + HK11*P(1,20) - HK12*P(3,20) + HK5*P(5,20) + HK7*P(6,20) - HK8*P(4,20) + HK9*P(2,20));
Kfusion(21) = HK20*(HK10*P(0,21) + HK11*P(1,21) - HK12*P(3,21) + HK5*P(5,21) + HK7*P(6,21) - HK8*P(4,21) + HK9*P(2,21));
Kfusion(22) = HK20*(HK10*P(0,22) + HK11*P(1,22) - HK12*P(3,22) + HK5*P(5,22) + HK7*P(6,22) - HK8*P(4,22) + HK9*P(2,22));
Kfusion(23) = HK20*(HK10*P(0,23) + HK11*P(1,23) - HK12*P(3,23) + HK5*P(5,23) + HK7*P(6,23) - HK8*P(4,23) + HK9*P(2,23));
// axis 2
const float HK0 = q0*vd - q1*ve + q2*vn;
const float HK1 = q0*ve + q1*vd - q3*vn;
const float HK2 = q0*vn - q2*vd + q3*ve;
const float HK3 = q1*vn + q2*ve + q3*vd;
const float HK4 = q0*q2 + q1*q3;
const float HK5 = q0*q1 - q2*q3;
const float HK6 = (q0)*(q0) - (q1)*(q1) - (q2)*(q2) + (q3)*(q3);
const float HK7 = 2*HK4;
const float HK8 = 2*HK5;
const float HK9 = 2*HK3;
const float HK10 = 2*HK0;
const float HK11 = 2*HK1;
const float HK12 = 2*HK2;
const float HK13 = HK10*P(0,0) - HK11*P(0,1) + HK12*P(0,2) + HK6*P(0,6) + HK7*P(0,4) - HK8*P(0,5) + HK9*P(0,3);
const float HK14 = HK10*P(0,4) - HK11*P(1,4) + HK12*P(2,4) + HK6*P(4,6) + HK7*P(4,4) - HK8*P(4,5) + HK9*P(3,4);
const float HK15 = HK10*P(0,5) - HK11*P(1,5) + HK12*P(2,5) + HK6*P(5,6) + HK7*P(4,5) - HK8*P(5,5) + HK9*P(3,5);
const float HK16 = HK10*P(0,3) - HK11*P(1,3) + HK12*P(2,3) + HK6*P(3,6) + HK7*P(3,4) - HK8*P(3,5) + HK9*P(3,3);
const float HK17 = HK10*P(0,1) - HK11*P(1,1) + HK12*P(1,2) + HK6*P(1,6) + HK7*P(1,4) - HK8*P(1,5) + HK9*P(1,3);
const float HK18 = HK10*P(0,2) - HK11*P(1,2) + HK12*P(2,2) + HK6*P(2,6) + HK7*P(2,4) - HK8*P(2,5) + HK9*P(2,3);
const float HK19 = HK10*P(0,6) - HK11*P(1,6) + HK12*P(2,6) + HK6*P(6,6) + HK7*P(4,6) - HK8*P(5,6) + HK9*P(3,6);
const float HK20 = 1.0F/(HK10*HK13 - HK11*HK17 + HK12*HK18 + HK14*HK7 - HK15*HK8 + HK16*HK9 + HK19*HK6 + R_VEL);
H_VEL(0) = 2*HK0;
H_VEL(1) = -2*HK1;
H_VEL(2) = 2*HK2;
H_VEL(3) = 2*HK3;
H_VEL(4) = 2*HK4;
H_VEL(5) = -2*HK5;
H_VEL(6) = HK6;
Kfusion(0) = HK13*HK20;
Kfusion(1) = HK17*HK20;
Kfusion(2) = HK18*HK20;
Kfusion(3) = HK16*HK20;
Kfusion(4) = HK14*HK20;
Kfusion(5) = HK15*HK20;
Kfusion(6) = HK19*HK20;
Kfusion(7) = HK20*(HK10*P(0,7) - HK11*P(1,7) + HK12*P(2,7) + HK6*P(6,7) + HK7*P(4,7) - HK8*P(5,7) + HK9*P(3,7));
Kfusion(8) = HK20*(HK10*P(0,8) - HK11*P(1,8) + HK12*P(2,8) + HK6*P(6,8) + HK7*P(4,8) - HK8*P(5,8) + HK9*P(3,8));
Kfusion(9) = HK20*(HK10*P(0,9) - HK11*P(1,9) + HK12*P(2,9) + HK6*P(6,9) + HK7*P(4,9) - HK8*P(5,9) + HK9*P(3,9));
Kfusion(10) = HK20*(HK10*P(0,10) - HK11*P(1,10) + HK12*P(2,10) + HK6*P(6,10) + HK7*P(4,10) - HK8*P(5,10) + HK9*P(3,10));
Kfusion(11) = HK20*(HK10*P(0,11) - HK11*P(1,11) + HK12*P(2,11) + HK6*P(6,11) + HK7*P(4,11) - HK8*P(5,11) + HK9*P(3,11));
Kfusion(12) = HK20*(HK10*P(0,12) - HK11*P(1,12) + HK12*P(2,12) + HK6*P(6,12) + HK7*P(4,12) - HK8*P(5,12) + HK9*P(3,12));
Kfusion(13) = HK20*(HK10*P(0,13) - HK11*P(1,13) + HK12*P(2,13) + HK6*P(6,13) + HK7*P(4,13) - HK8*P(5,13) + HK9*P(3,13));
Kfusion(14) = HK20*(HK10*P(0,14) - HK11*P(1,14) + HK12*P(2,14) + HK6*P(6,14) + HK7*P(4,14) - HK8*P(5,14) + HK9*P(3,14));
Kfusion(15) = HK20*(HK10*P(0,15) - HK11*P(1,15) + HK12*P(2,15) + HK6*P(6,15) + HK7*P(4,15) - HK8*P(5,15) + HK9*P(3,15));
Kfusion(16) = HK20*(HK10*P(0,16) - HK11*P(1,16) + HK12*P(2,16) + HK6*P(6,16) + HK7*P(4,16) - HK8*P(5,16) + HK9*P(3,16));
Kfusion(17) = HK20*(HK10*P(0,17) - HK11*P(1,17) + HK12*P(2,17) + HK6*P(6,17) + HK7*P(4,17) - HK8*P(5,17) + HK9*P(3,17));
Kfusion(18) = HK20*(HK10*P(0,18) - HK11*P(1,18) + HK12*P(2,18) + HK6*P(6,18) + HK7*P(4,18) - HK8*P(5,18) + HK9*P(3,18));
Kfusion(19) = HK20*(HK10*P(0,19) - HK11*P(1,19) + HK12*P(2,19) + HK6*P(6,19) + HK7*P(4,19) - HK8*P(5,19) + HK9*P(3,19));
Kfusion(20) = HK20*(HK10*P(0,20) - HK11*P(1,20) + HK12*P(2,20) + HK6*P(6,20) + HK7*P(4,20) - HK8*P(5,20) + HK9*P(3,20));
Kfusion(21) = HK20*(HK10*P(0,21) - HK11*P(1,21) + HK12*P(2,21) + HK6*P(6,21) + HK7*P(4,21) - HK8*P(5,21) + HK9*P(3,21));
Kfusion(22) = HK20*(HK10*P(0,22) - HK11*P(1,22) + HK12*P(2,22) + HK6*P(6,22) + HK7*P(4,22) - HK8*P(5,22) + HK9*P(3,22));
Kfusion(23) = HK20*(HK10*P(0,23) - HK11*P(1,23) + HK12*P(2,23) + HK6*P(6,23) + HK7*P(4,23) - HK8*P(5,23) + HK9*P(3,23));

187
src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp
View File

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

690
src/modules/ekf2/EKF/python/ekf_derivation/main.py
View File

@ -1,690 +0,0 @@
#!/usr/bin/env python3
from sympy import *
from code_gen import *
# q: quaternion describing rotation from frame 1 to frame 2
# returns a rotation matrix derived form q which describes the same
# rotation
def quat2RotSimplified(q):
q0 = q[0]
q1 = q[1]
q2 = q[2]
q3 = q[3]
# Use the simplified formula for unit quaternion to rotation matrix
# as it produces a simpler and more stable EKF derivation given
# the additional constraint: q0^2 + q1^2 + q2^2 + q3^2 = 1
Rot = Matrix([[1 - 2*q2**2 - 2*q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)],
[2*(q1*q2 + q0*q3), 1 - 2*q1**2 - 2*q3**2, 2*(q2*q3 - q0*q1)],
[2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), 1 - 2*q1**2 - 2*q2**2]])
return Rot
def quat2RotUnSimplified(q):
q0 = q[0]
q1 = q[1]
q2 = q[2]
q3 = q[3]
Rot = Matrix([[q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)],
[2*(q1*q2 + q0*q3), q0**2 - q1**2 + q2**2 - q3**2, 2*(q2*q3 - q0*q1)],
[2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0**2 - q1**2 - q2**2 + q3**2]])
return Rot
def create_cov_matrix(i, j):
if j >= i:
return Symbol("P(" + str(i) + "," + str(j) + ")", real=True)
# legacy array format
# return Symbol("P[" + str(i) + "][" + str(j) + "]", real=True)
else:
return 0
def create_yaw_estimator_cov_matrix():
# define a symbolic covariance matrix
P = Matrix(3,3,create_cov_matrix)
for index in range(3):
for j in range(3):
if index > j:
P[index,j] = P[j,index]
return P
def create_Tbs_matrix(i, j):
return Symbol("Tbs(" + str(i) + "," + str(j) + ")", real=True)
# legacy array format
# return Symbol("Tbs[" + str(i) + "][" + str(j) + "]", real=True)
def quat_mult(p,q):
r = Matrix([p[0] * q[0] - p[1] * q[1] - p[2] * q[2] - p[3] * q[3],
p[0] * q[1] + p[1] * q[0] + p[2] * q[3] - p[3] * q[2],
p[0] * q[2] - p[1] * q[3] + p[2] * q[0] + p[3] * q[1],
p[0] * q[3] + p[1] * q[2] - p[2] * q[1] + p[3] * q[0]])
return r
def create_symmetric_cov_matrix():
# define a symbolic covariance matrix
P = Matrix(24,24,create_cov_matrix)
for index in range(24):
for j in range(24):
if index > j:
P[index,j] = P[j,index]
return P
# generate equations for observation vector innovation variances
def generate_observation_vector_innovation_variances(P,state,observation,variance,n_obs):
H = observation.jacobian(state)
innovation_variance = zeros(n_obs,1)
for index in range(n_obs):
H[index,:] = Matrix([observation[index]]).jacobian(state)
innovation_variance[index] = H[index,:] * P * H[index,:].T + Matrix([variance])
IV_simple = cse(innovation_variance, symbols("IV0:1000"), optimizations='basic')
return IV_simple
# generate equations for observation Jacobian and Kalman gain
def generate_observation_equations(P,state,observation,variance,varname="HK"):
H = Matrix([observation]).jacobian(state)
innov_var = H * P * H.T + Matrix([variance])
assert(innov_var.shape[0] == 1)
assert(innov_var.shape[1] == 1)
K = P * H.T / innov_var[0,0]
extension="0:1000"
var_string = varname+extension
HK_simple = cse(Matrix([H.transpose(), K]), symbols(var_string), optimizations='basic')
return HK_simple
def generate_observation_equations_innov_var_only(P,state,observation,variance,varname="IV"):
H = Matrix([observation]).jacobian(state)
innov_var = H * P * H.T + Matrix([variance])
assert(innov_var.shape[0] == 1)
assert(innov_var.shape[1] == 1)
extension="0:1000"
var_string = varname+extension
IV_simple = cse(Matrix([zeros(24,1), zeros(24,1), observation, innov_var]), symbols(var_string), optimizations='basic')
return IV_simple
def generate_observation_equations_hk_only(P,state,observation,varname="HK"):
H = Matrix([observation]).jacobian(state)
K = P * H.T / Symbol("innov_var")
extension="0:1000"
var_string = varname+extension
# optimizations=None produces a set of equations that fits nicely in a for-loop
HK_simple = cse(Matrix([H.transpose(), K, observation]), symbols(var_string), optimizations=None)
return HK_simple
# generate equations for observation vector Jacobian and Kalman gain
# n_obs is the vector dimension and must be >= 2
def generate_observation_vector_equations(P,state,observation,variance,n_obs):
K = zeros(24,n_obs)
H = observation.jacobian(state)
HK = zeros(n_obs*48,1)
for index in range(n_obs):
H[index,:] = Matrix([observation[index]]).jacobian(state)
innov_var = H[index,:] * P * H[index,:].T + Matrix([variance])
assert(innov_var.shape[0] == 1)
assert(innov_var.shape[1] == 1)
K[:,index] = P * H[index,:].T / innov_var[0,0]
HK[index*48:(index+1)*48,0] = Matrix([H[index,:].transpose(), K[:,index]])
HK_simple = cse(HK, symbols("HK0:1000"), optimizations='basic')
return HK_simple
# write single observation equations to file
def write_equations_to_file(equations,code_generator_id,n_obs):
if (n_obs < 1):
return
if (n_obs == 1):
code_generator_id.print_string("Sub Expressions")
code_generator_id.write_subexpressions(equations[0])
code_generator_id.print_string("Observation Jacobians")
code_generator_id.write_matrix(Matrix(equations[1][0][0:24]), "Hfusion", False, ".at<", ">()")
code_generator_id.print_string("Kalman gains")
code_generator_id.write_matrix(Matrix(equations[1][0][24:48]), "Kfusion", False, "(", ")")
code_generator_id.print_string("Predicted observation")
code_generator_id.write_matrix(Matrix(equations[1][0][48:49]), "meas_pred")
code_generator_id.print_string("Innovation variance")
code_generator_id.write_matrix(Matrix(equations[1][0][49:50]), "innov_var", False, "(", ")")
else:
code_generator_id.print_string("Sub Expressions")
code_generator_id.write_subexpressions(equations[0])
for axis_index in range(n_obs):
start_index = axis_index*48
code_generator_id.print_string("Observation Jacobians - axis %i" % axis_index)
code_generator_id.write_matrix(Matrix(equations[1][0][start_index:start_index+24]), "Hfusion", False, ".at<", ">()")
code_generator_id.print_string("Kalman gains - axis %i" % axis_index)
code_generator_id.write_matrix(Matrix(equations[1][0][start_index+24:start_index+48]), "Kfusion", False, "(", ")")
return
# derive equations for sequential fusion of optical flow measurements
def optical_flow_observation(P,state,R_to_body,vx,vy,vz):
flow_code_generator = CodeGenerator("./generated/flow_generated.cpp")
range = symbols("range", real=True) # range from camera focal point to ground along sensor Z axis
obs_var = symbols("R_LOS", real=True) # optical flow line of sight rate measurement noise variance
# Define rotation matrix from body to sensor frame
Tbs = Matrix(3,3,create_Tbs_matrix)
# Calculate earth relative velocity in a non-rotating sensor frame
relVelSensor = Tbs * R_to_body * Matrix([vx,vy,vz])
# Divide by range to get predicted angular LOS rates relative to X and Y
# axes. Note these are rates in a non-rotating sensor frame
losRateSensorX = +relVelSensor[1]/range
losRateSensorY = -relVelSensor[0]/range
# calculate the observation Jacobian and Kalman gains for the X axis
equations = generate_observation_equations(P,state,losRateSensorX,obs_var)
flow_code_generator.print_string("X Axis Equations")
write_equations_to_file(equations,flow_code_generator,1)
# calculate the observation Jacobian and Kalman gains for the Y axis
equations = generate_observation_equations(P,state,losRateSensorY,obs_var)
flow_code_generator.print_string("Y Axis Equations")
write_equations_to_file(equations,flow_code_generator,1)
flow_code_generator.close()
# calculate a combined result for a possible reduction in operations, but will use more stack
observation = Matrix([relVelSensor[1]/range,-relVelSensor[0]/range])
equations = generate_observation_vector_equations(P,state,observation,obs_var,2)
flow_code_generator_alt = CodeGenerator("./generated/flow_generated_alt.cpp")
write_equations_to_file(equations,flow_code_generator_alt,2)
flow_code_generator_alt.close()
return
# Derive equations for sequential fusion of body frame velocity measurements
def body_frame_velocity_observation(P,state,R_to_body,vx,vy,vz):
obs_var = symbols("R_VEL", real=True) # measurement noise variance
# Calculate earth relative velocity in a non-rotating sensor frame
vel_bf = R_to_body * Matrix([vx,vy,vz])
vel_bf_code_generator = CodeGenerator("./generated/vel_bf_generated.cpp")
axes = [0,1,2]
H_obs = vel_bf.jacobian(state) # observation Jacobians
K_gain = zeros(24,3)
for index in axes:
equations = generate_observation_equations(P,state,vel_bf[index],obs_var)
vel_bf_code_generator.print_string("axis %i" % index)
vel_bf_code_generator.write_subexpressions(equations[0])
vel_bf_code_generator.write_matrix(Matrix(equations[1][0][0:24]), "H_VEL", False, "(", ")")
vel_bf_code_generator.write_matrix(Matrix(equations[1][0][24:]), "Kfusion", False, "(", ")")
vel_bf_code_generator.close()
# calculate a combined result for a possible reduction in operations, but will use more stack
equations = generate_observation_vector_equations(P,state,vel_bf,obs_var,3)
vel_bf_code_generator_alt = CodeGenerator("./generated/vel_bf_generated_alt.cpp")
write_equations_to_file(equations,vel_bf_code_generator_alt,3)
vel_bf_code_generator_alt.close()
# derive equations for fusion of dual antenna yaw measurement
def gps_yaw_observation(P,state,R_to_body):
obs_var = symbols("R_YAW", real=True) # measurement noise variance
ant_yaw = symbols("ant_yaw", real=True) # yaw angle of antenna array axis wrt X body axis
# define antenna vector in body frame
ant_vec_bf = Matrix([cos(ant_yaw),sin(ant_yaw),0])
# rotate into earth frame
ant_vec_ef = R_to_body.T * ant_vec_bf
# Calculate the yaw angle from the projection
observation = atan(ant_vec_ef[1]/ant_vec_ef[0])
equations = generate_observation_equations(P,state,observation,obs_var)
gps_yaw_code_generator = CodeGenerator("./generated/gps_yaw_generated.cpp")
write_equations_to_file(equations,gps_yaw_code_generator,1)
gps_yaw_code_generator.close()
return
# derive equations for fusion of declination
def declination_observation(P,state,ix,iy):
obs_var = symbols("R_DECL", real=True) # measurement noise variance
# the predicted measurement is the angle wrt magnetic north of the horizontal
# component of the measured field
observation = atan(iy/ix)
equations = generate_observation_equations(P,state,observation,obs_var)
mag_decl_code_generator = CodeGenerator("./generated/mag_decl_generated.cpp")
write_equations_to_file(equations,mag_decl_code_generator,1)
mag_decl_code_generator.close()
return
# derive equations for fusion of lateral body acceleration (multirotors only)
def body_frame_accel_observation(P,state,R_to_body,vx,vy,vz,wx,wy):
obs_var = symbols("R_ACC", real=True) # measurement noise variance
Kaccx = symbols("Kaccx", real=True) # measurement noise variance
Kaccy = symbols("Kaccy", real=True) # measurement noise variance
# use relationship between airspeed along the X and Y body axis and the
# drag to predict the lateral acceleration for a multirotor vehicle type
# where propulsion forces are generated primarily along the Z body axis
vrel = R_to_body*Matrix([vx-wx,vy-wy,vz]) # predicted wind relative velocity
# Use this nonlinear model for the prediction in the implementation only
# It uses a ballistic coefficient for each axis and a propeller momentum drag coefficient
#
# accXpred = -sign(vrel[0]) * vrel[0]*(0.5*rho*vrel[0]/BCoefX + MCoef) # predicted acceleration measured along X body axis
# accYpred = -sign(vrel[1]) * vrel[1]*(0.5*rho*vrel[1]/BCoefY + MCoef) # predicted acceleration measured along Y body axis
#
# BcoefX and BcoefY have units of Kg/m^2
# Mcoef has units of 1/s
# Use a simple viscous drag model for the linear estimator equations
# Use the the derivative from speed to acceleration averaged across the
# speed range. This avoids the generation of a dirac function in the derivation
# The nonlinear equation will be used to calculate the predicted measurement in implementation
observation = Matrix([-Kaccx*vrel[0],-Kaccy*vrel[1]])
acc_bf_code_generator = CodeGenerator("./generated/acc_bf_generated.cpp")
H = observation.jacobian(state)
K = zeros(24,2)
axes = [0,1]
for index in axes:
equations = generate_observation_equations(P,state,observation[index],obs_var)
acc_bf_code_generator.print_string("Axis %i equations" % index)
write_equations_to_file(equations,acc_bf_code_generator,1)
acc_bf_code_generator.close()
# calculate a combined result for a possible reduction in operations, but will use more stack
equations = generate_observation_vector_equations(P,state,observation,obs_var,2)
acc_bf_code_generator_alt = CodeGenerator("./generated/acc_bf_generated_alt.cpp")
write_equations_to_file(equations,acc_bf_code_generator_alt,3)
acc_bf_code_generator_alt.close()
return
# yaw fusion
def yaw_observation(P,state,R_to_earth):
yaw_code_generator = CodeGenerator("./generated/yaw_generated.cpp")
# Derive observation Jacobian for fusion of 321 sequence yaw measurement
# Calculate the yaw (first rotation) angle from the 321 rotation sequence
# Provide alternative angle that avoids singularity at +-pi/2 yaw
angMeasA = atan(R_to_earth[1,0]/R_to_earth[0,0])
H_YAW321_A = Matrix([angMeasA]).jacobian(state)
H_YAW321_A_simple = cse(H_YAW321_A, symbols('SA0:200'))
angMeasB = pi/2 - atan(R_to_earth[0,0]/R_to_earth[1,0])
H_YAW321_B = Matrix([angMeasB]).jacobian(state)
H_YAW321_B_simple = cse(H_YAW321_B, symbols('SB0:200'))
yaw_code_generator.print_string("calculate 321 yaw observation matrix - option A")
yaw_code_generator.write_subexpressions(H_YAW321_A_simple[0])
yaw_code_generator.write_matrix(Matrix(H_YAW321_A_simple[1]).T, "H_YAW", False, ".at<", ">()")
yaw_code_generator.print_string("calculate 321 yaw observation matrix - option B")
yaw_code_generator.write_subexpressions(H_YAW321_B_simple[0])
yaw_code_generator.write_matrix(Matrix(H_YAW321_B_simple[1]).T, "H_YAW", False, ".at<", ">()")
# Derive observation Jacobian for fusion of 312 sequence yaw measurement
# Calculate the yaw (first rotation) angle from an Euler 312 sequence
# Provide alternative angle that avoids singularity at +-pi/2 yaw
angMeasA = atan(-R_to_earth[0,1]/R_to_earth[1,1])
H_YAW312_A = Matrix([angMeasA]).jacobian(state)
H_YAW312_A_simple = cse(H_YAW312_A, symbols('SA0:200'))
angMeasB = pi/2 - atan(-R_to_earth[1,1]/R_to_earth[0,1])
H_YAW312_B = Matrix([angMeasB]).jacobian(state)
H_YAW312_B_simple = cse(H_YAW312_B, symbols('SB0:200'))
yaw_code_generator.print_string("calculate 312 yaw observation matrix - option A")
yaw_code_generator.write_subexpressions(H_YAW312_A_simple[0])
yaw_code_generator.write_matrix(Matrix(H_YAW312_A_simple[1]).T, "H_YAW", False, ".at<", ">()")
yaw_code_generator.print_string("calculate 312 yaw observation matrix - option B")
yaw_code_generator.write_subexpressions(H_YAW312_B_simple[0])
yaw_code_generator.write_matrix(Matrix(H_YAW312_B_simple[1]).T, "H_YAW", False, ".at<", ">()")
yaw_code_generator.close()
return
# 3D magnetometer fusion
def mag_observation_variance(P,state,R_to_body,i,ib):
obs_var = symbols("R_MAG", real=True) # magnetometer measurement noise variance
m_mag = R_to_body * i + ib
# separate calculation of innovation variance equations for the y and z axes
m_mag[0]=0
innov_var_equations = generate_observation_vector_innovation_variances(P,state,m_mag,obs_var,3)
mag_innov_var_code_generator = CodeGenerator("./generated/3Dmag_innov_var_generated.cpp")
write_equations_to_file(innov_var_equations,mag_innov_var_code_generator,3)
mag_innov_var_code_generator.close()
return
# 3D magnetometer fusion
def mag_observation(P,state,R_to_body,i,ib):
obs_var = symbols("R_MAG", real=True) # magnetometer measurement noise variance
m_mag = R_to_body * i + ib
# calculate a separate set of equations for each axis
mag_code_generator = CodeGenerator("./generated/3Dmag_generated.cpp")
axes = [0,1,2]
label="HK"
for index in axes:
if (index==0):
label="HKX"
elif (index==1):
label="HKY"
elif (index==2):
label="HKZ"
else:
return
equations = generate_observation_equations(P,state,m_mag[index],obs_var,varname=label)
mag_code_generator.print_string("Axis %i equations" % index)
write_equations_to_file(equations,mag_code_generator,1)
mag_code_generator.close()
# calculate a combined set of equations for a possible reduction in operations, but will use slighlty more stack
equations = generate_observation_vector_equations(P,state,m_mag,obs_var,3)
mag_code_generator_alt = CodeGenerator("./generated/3Dmag_generated_alt.cpp")
write_equations_to_file(equations,mag_code_generator_alt,3)
mag_code_generator_alt.close()
return
# airspeed fusion
def tas_observation(P,state,vx,vy,vz,wx,wy):
obs_var = symbols("R_TAS", real=True) # true airspeed measurement noise variance
observation = sqrt((vx-wx)*(vx-wx)+(vy-wy)*(vy-wy)+vz*vz)
equations = generate_observation_equations(P,state,observation,obs_var)
tas_code_generator = CodeGenerator("./generated/tas_generated.cpp")
write_equations_to_file(equations,tas_code_generator,1)
tas_code_generator.close()
equations = generate_observation_equations_innov_var_only(P,state,observation,obs_var)
tas_code_generator = CodeGenerator("./generated/tas_var_generated.cpp")
write_equations_to_file(equations,tas_code_generator,1)
tas_code_generator.close()
equations = generate_observation_equations_hk_only(P,state,observation)
tas_code_generator = CodeGenerator("./generated/tas_hk_generated.cpp")
write_equations_to_file(equations,tas_code_generator,1)
tas_code_generator.close()
return
# sideslip fusion
def beta_observation(P,state,R_to_body,vx,vy,vz,wx,wy):
obs_var = symbols("R_BETA", real=True) # sideslip measurement noise variance
v_rel_ef = Matrix([vx-wx,vy-wy,vz])
v_rel_bf = R_to_body * v_rel_ef
observation = v_rel_bf[1]/v_rel_bf[0]
equations = generate_observation_equations(P,state,observation,obs_var)
beta_code_generator = CodeGenerator("./generated/beta_generated.cpp")
write_equations_to_file(equations,beta_code_generator,1)
beta_code_generator.close()
return
# yaw estimator prediction and observation code
def yaw_estimator():
dt = symbols("dt", real=True) # dt (sec)
psi = symbols("psi", real=True) # yaw angle of body frame wrt earth frame
vn, ve = symbols("vn ve", real=True) # velocity in world frame (north/east) - m/sec
daz = symbols("daz", real=True) # IMU z axis delta angle measurement in body axes - rad
dazVar = symbols("dazVar", real=True) # IMU Z axis delta angle measurement variance (rad^2)
dvx, dvy = symbols("dvx dvy", real=True) # IMU x and y axis delta velocity measurement in body axes - m/sec
dvxVar, dvyVar = symbols("dvxVar dvyVar", real=True) # IMU x and y axis delta velocity measurement variance (m/s)^2
# derive the body to nav direction transformation matrix
Tbn = Matrix([[cos(psi) , -sin(psi)],
[sin(psi) , cos(psi)]])
# attitude update equation
psiNew = psi + daz
# velocity update equations
velNew = Matrix([vn,ve]) + Tbn*Matrix([dvx,dvy])
# Define the state vectors
stateVector = Matrix([vn,ve,psi])
# Define vector of process equations
newStateVector = Matrix([velNew,psiNew])
# Calculate state transition matrix
F = newStateVector.jacobian(stateVector)
# Derive the covariance prediction equations
# Error growth in the inertial solution is assumed to be driven by 'noise' in the delta angles and
# velocities, after bias effects have been removed.
# derive the control(disturbance) influence matrix from IMU noise to state noise
G = newStateVector.jacobian(Matrix([dvx,dvy,daz]))
# derive the state error matrix
distMatrix = Matrix([[dvxVar , 0 , 0],
[0 , dvyVar , 0],
[0 , 0 , dazVar]])
Q = G * distMatrix * G.T
# propagate covariance matrix
P = create_yaw_estimator_cov_matrix()
P_new = F * P * F.T + Q
P_new_simple = cse(P_new, symbols("S0:1000"), optimizations='basic')
yaw_estimator_covariance_generator = CodeGenerator("./generated/yaw_estimator_covariance_prediction_generated.cpp")
yaw_estimator_covariance_generator.print_string("Equations for covariance matrix prediction")
yaw_estimator_covariance_generator.write_subexpressions(P_new_simple[0])
yaw_estimator_covariance_generator.write_matrix(Matrix(P_new_simple[1]), "_ekf_gsf[model_index].P", True)
yaw_estimator_covariance_generator.close()
# derive the covariance update equation for a NE velocity observation
velObsVar = symbols("velObsVar", real=True) # velocity observation variance (m/s)^2
H = Matrix([[1,0,0],
[0,1,0]])
R = Matrix([[velObsVar , 0],
[0 , velObsVar]])
S = H * P * H.T + R
S_det_inv = 1 / S.det()
S_inv = S.inv()
K = (P * H.T) * S_inv
P_new = P - K * S * K.T
# optimize code
t, [S_det_inv_s, S_inv_s, K_s, P_new_s] = cse([S_det_inv, S_inv, K, P_new], symbols("t0:1000"), optimizations='basic')
yaw_estimator_observation_generator = CodeGenerator("./generated/yaw_estimator_measurement_update_generated.cpp")
yaw_estimator_observation_generator.print_string("Intermediate variables")
yaw_estimator_observation_generator.write_subexpressions(t)
yaw_estimator_observation_generator.print_string("Equations for NE velocity innovation variance's determinante inverse")
yaw_estimator_observation_generator.write_matrix(Matrix([[S_det_inv_s]]), "_ekf_gsf[model_index].S_det_inverse", False)
yaw_estimator_observation_generator.print_string("Equations for NE velocity innovation variance inverse")
yaw_estimator_observation_generator.write_matrix(Matrix(S_inv_s), "_ekf_gsf[model_index].S_inverse", True)
yaw_estimator_observation_generator.print_string("Equations for NE velocity Kalman gain")
yaw_estimator_observation_generator.write_matrix(Matrix(K_s), "K", False)
yaw_estimator_observation_generator.print_string("Equations for covariance matrix update")
yaw_estimator_observation_generator.write_matrix(Matrix(P_new_s), "_ekf_gsf[model_index].P", True)
yaw_estimator_observation_generator.close()
def generate_code():
print('Starting code generation:')
print('Creating symbolic variables ...')
dt = symbols("dt", real=True) # dt
g = symbols("g", real=True) # gravity constant
r_hor_vel = symbols("R_hor_vel", real=True) # horizontal velocity noise variance
r_ver_vel = symbols("R_vert_vel", real=True) # vertical velocity noise variance
r_hor_pos = symbols("R_hor_pos", real=True) # horizontal position noise variance
# inputs, integrated gyro measurements
# delta angle x y z
d_ang_x, d_ang_y, d_ang_z = symbols("dax day daz", real=True) # delta angle x
d_ang = Matrix([d_ang_x, d_ang_y, d_ang_z])
# inputs, integrated accelerometer measurements
# delta velocity x y z
d_v_x, d_v_y, d_v_z = symbols("dvx dvy dvz", real=True)
d_v = Matrix([d_v_x, d_v_y,d_v_z])
u = Matrix([d_ang, d_v])
# input noise
d_ang_x_var, d_ang_y_var, d_ang_z_var = symbols("daxVar dayVar dazVar", real=True)
d_v_x_var, d_v_y_var, d_v_z_var = symbols("dvxVar dvyVar dvzVar", real=True)
var_u = Matrix.diag(d_ang_x_var, d_ang_y_var, d_ang_z_var, d_v_x_var, d_v_y_var, d_v_z_var)
# define state vector
# attitude quaternion
qw, qx, qy, qz = symbols("q0 q1 q2 q3", real=True)
q = Matrix([qw,qx,qy,qz])
R_to_earth = quat2RotSimplified(q)
R_to_body = R_to_earth.T
# velocity in NED local frame (north, east, down)
vx, vy, vz = symbols("vn ve vd", real=True)
v = Matrix([vx,vy,vz])
# position in NED local frame (north, east, down)
px, py, pz = symbols("pn pe pd", real=True)
p = Matrix([px,py,pz])
# delta angle bias x y z
d_ang_bx, d_ang_by, d_ang_bz = symbols("dax_b day_b daz_b", real=True)
d_ang_b = Matrix([d_ang_bx, d_ang_by, d_ang_bz])
d_ang_true = d_ang - d_ang_b
# delta velocity bias x y z
d_vel_bx, d_vel_by, d_vel_bz = symbols("dvx_b dvy_b dvz_b", real=True)
d_vel_b = Matrix([d_vel_bx, d_vel_by, d_vel_bz])
d_vel_true = d_v - d_vel_b
# earth magnetic field vector x y z
ix, iy, iz = symbols("magN magE magD", real=True)
i = Matrix([ix,iy,iz])
# earth magnetic field bias in body frame
ibx, iby, ibz = symbols("ibx iby ibz", real=True)
ib = Matrix([ibx,iby,ibz])
# wind in local NE frame (north, east)
wx, wy = symbols("vwn, vwe", real=True)
w = Matrix([wx,wy])
# state vector at arbitrary time t
state = Matrix([q,v,p,d_ang_b,d_vel_b,i,ib,w])
print('Defining state propagation ...')
q_new = quat_mult(q, Matrix([1, 0.5 * d_ang_true[0], 0.5 * d_ang_true[1], 0.5 * d_ang_true[2]]))
v_new = v + R_to_earth * d_vel_true + Matrix([0,0,g]) * dt
p_new = p + v * dt
d_ang_b_new = d_ang_b
d_vel_b_new = d_vel_b
i_new = i
ib_new = ib
w_new = w
# predicted state vector at time t + dt
state_new = Matrix([q_new, v_new, p_new, d_ang_b_new, d_vel_b_new, i_new, ib_new, w_new])
print('Computing state propagation jacobian ...')
A = state_new.jacobian(state)
G = state_new.jacobian(u)
P = create_symmetric_cov_matrix()
print('Computing covariance propagation ...')
P_new = A * P * A.T + G * var_u * G.T
for index in range(24):
for j in range(24):
if index > j:
P_new[index,j] = 0
print('Simplifying covariance propagation ...')
P_new_simple = cse(P_new, symbols("PS0:400"), optimizations='basic')
print('Writing covariance propagation to file ...')
cov_code_generator = CodeGenerator("./generated/covariance_generated.cpp")
cov_code_generator.print_string("Equations for covariance matrix prediction, without process noise!")
cov_code_generator.write_subexpressions(P_new_simple[0])
cov_code_generator.write_matrix(Matrix(P_new_simple[1]), "nextP", True, "(", ")")
cov_code_generator.close()
# Use legacy quaternion to rotation matrix conversion for observaton equation as it gives
# simpler equations
R_to_earth = quat2RotUnSimplified(q)
R_to_body = R_to_earth.T
# derive autocode for observation methods
print('Generating heading observation code ...')
yaw_observation(P,state,R_to_earth)
print('Generating gps heading observation code ...')
gps_yaw_observation(P,state,R_to_body)
print('Generating mag observation code ...')
mag_observation_variance(P,state,R_to_body,i,ib)
mag_observation(P,state,R_to_body,i,ib)
print('Generating declination observation code ...')
declination_observation(P,state,ix,iy)
print('Generating airspeed observation code ...')
tas_observation(P,state,vx,vy,vz,wx,wy)
print('Generating sideslip observation code ...')
beta_observation(P,state,R_to_body,vx,vy,vz,wx,wy)
print('Generating optical flow observation code ...')
optical_flow_observation(P,state,R_to_body,vx,vy,vz)
print('Generating body frame velocity observation code ...')
body_frame_velocity_observation(P,state,R_to_body,vx,vy,vz)
print('Generating body frame acceleration observation code ...')
body_frame_accel_observation(P,state,R_to_body,vx,vy,vz,wx,wy)
print('Generating yaw estimator code ...')
yaw_estimator()
print('Code generation finished!')
if __name__ == "__main__":
generate_code()