diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/__init__.py b/src/modules/ekf2/EKF/python/ekf_derivation/__init__.py deleted file mode 100644 index 1d2b6d584d..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/__init__.py +++ /dev/null @@ -1,7 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Sat Mar 14 13:02:26 2020 - -@author: roman -""" diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/code_gen.py b/src/modules/ekf2/EKF/python/ekf_derivation/code_gen.py deleted file mode 100644 index 513ea8c957..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/code_gen.py +++ /dev/null @@ -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() diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_fusion_generated_compare.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_fusion_generated_compare.cpp deleted file mode 100644 index 523bd90654..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_fusion_generated_compare.cpp +++ /dev/null @@ -1,476 +0,0 @@ -#include -#include -#include -#include "../../../../../matrix/matrix/math.hpp" -#include "util.h" - -typedef matrix::Vector Vector24f; -typedef matrix::SquareMatrix SquareMatrix24f; -template -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; -} diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp deleted file mode 100644 index d312984d26..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp +++ /dev/null @@ -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 - - diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp deleted file mode 100644 index 14a6b21a0b..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp +++ /dev/null @@ -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 - - diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/util.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/util.h deleted file mode 100644 index 36ba9f4ffc..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/util.h +++ /dev/null @@ -1,24 +0,0 @@ -#include "../../../../../matrix/matrix/math.hpp" - -typedef matrix::SquareMatrix 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; - } -} diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp deleted file mode 100644 index d75236640e..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp +++ /dev/null @@ -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)); - - diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp deleted file mode 100644 index 457c92a310..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp +++ /dev/null @@ -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)); - - diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/main.py b/src/modules/ekf2/EKF/python/ekf_derivation/main.py deleted file mode 100755 index 66708278bb..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/main.py +++ /dev/null @@ -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()