Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 04:33:10 +08:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

ekf2: purge old ekf2 derivation

Browse Source
This commit is contained in:
bresch
2023-01-06 15:05:23 +01:00
committed by Daniel Agar
parent d1b95a21e4
commit 49f8bcfc69
9 changed files with 0 additions and 1946 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/python/ekf_derivation/__init__.py
-7
View File
@@ -1,7 +0,0 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Sat Mar 14 13:02:26 2020
@author: roman
"""
src/modules/ekf2/EKF/python/ekf_derivation/code_gen.py
-65
View File
@@ -1,65 +0,0 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Sat Mar 14 12:47:24 2020
@author: roman
"""
from sympy import ccode
from sympy.codegen.ast import float32, real
class CodeGenerator:
def __init__(self, file_name):
self.file_name = file_name
self.file = open(self.file_name, 'w')
def print_string(self, string):
self.file.write("// " + string + "\n")
def get_ccode(self, expression):
custom_functions = {
"Pow": [
(lambda b, e: e == 2, lambda b, e: f"({b})*({b})"),
(lambda b, e: e == -1, lambda b, e: f"1.0F/({b})"),
(lambda b, e: e == -2, lambda b, e: f"1.0F/(({b})*({b}))"),
(lambda b, e: e == 0.5, lambda b, e: f"sqrtf({b})"),
(lambda b, e: e == -0.5, lambda b, e: f"1.0F/sqrtf({b})"),
(lambda b, e: True, "ecl::powf"),
]
}
return ccode(expression, type_aliases={real:float32}, user_functions=custom_functions)
def write_subexpressions(self,subexpressions):
write_string = ""
for item in subexpressions:
write_string = write_string + "const float " + str(item[0]) + " = " + self.get_ccode(item[1]) + ";\n"
write_string = write_string + "\n\n"
self.file.write(write_string)
def write_matrix(self, matrix, variable_name, is_symmetric=False, pre_bracket="(", post_bracket=")"):
write_string = ""
if matrix.shape[0] * matrix.shape[1] == 1:
if matrix[0] != 0:
write_string = write_string + variable_name + " = " + self.get_ccode(matrix[0]) + ";\n"
elif matrix.shape[0] == 1 or matrix.shape[1] == 1:
for i in range(0,len(matrix)):
if matrix[i] == 0:
continue
write_string = write_string + variable_name + pre_bracket + str(i) + post_bracket + " = " + self.get_ccode(matrix[i]) + ";\n"
else:
for j in range(0, matrix.shape[1]):
for i in range(0, matrix.shape[0]):
if j >= i or not is_symmetric:
if matrix[i,j] == 0:
continue
write_string = write_string + variable_name + pre_bracket + str(i) + "," + str(j) + post_bracket + " = " + self.get_ccode(matrix[i,j]) + ";\n"
write_string = write_string + "\n\n"
self.file.write(write_string)
def close(self):
self.file.close()
src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_fusion_generated_compare.cpp
-476
View File
File diff suppressed because one or more lines are too long
src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp
-166
View File
@@ -1,166 +0,0 @@
// Axis 0 equations
// Sub Expressions
const float HK0 = vn - vwn;
const float HK1 = ve - vwe;
const float HK2 = HK0*q0 + HK1*q3 - q2*vd;
const float HK3 = 2*Kaccx;
const float HK4 = HK0*q1 + HK1*q2 + q3*vd;
const float HK5 = HK0*q2 - HK1*q1 + q0*vd;
const float HK6 = -HK0*q3 + HK1*q0 + q1*vd;
const float HK7 = (q0)*(q0) + (q1)*(q1) - (q2)*(q2) - (q3)*(q3);
const float HK8 = HK7*Kaccx;
const float HK9 = q0*q3 + q1*q2;
const float HK10 = HK3*HK9;
const float HK11 = q0*q2 - q1*q3;
const float HK12 = 2*HK5;
const float HK13 = 2*HK11;
const float HK14 = 2*HK9;
const float HK15 = 2*HK2;
const float HK16 = 2*HK4;
const float HK17 = 2*HK6;
const float HK18 = -HK14*P(0,23) + HK14*P(0,5) + HK15*P(0,0) + HK16*P(0,1) + HK17*P(0,3) + HK7*P(0,4);
const float HK19 = (Kaccx)*(Kaccx);
const float HK20 = -HK7;
const float HK21 = -2*HK5;
const float HK22 = -2*HK11;
const float HK23 = HK14*P(5,23);
const float HK24 = -HK14*P(23,23) + HK15*P(0,23) + HK16*P(1,23) + HK17*P(3,23) + HK23 + HK7*P(4,23);
const float HK25 = HK14*P(5,5) + HK15*P(0,5) + HK16*P(1,5) + HK17*P(3,5) - HK23 + HK7*P(4,5);
const float HK26 = HK14*P(5,6) - HK14*P(6,23) + HK15*P(0,6) + HK16*P(1,6) + HK17*P(3,6) + HK7*P(4,6);
const float HK27 = -HK14*P(4,23) + HK14*P(4,5) + HK15*P(0,4) + HK16*P(1,4) + HK17*P(3,4) + HK7*P(4,4);
const float HK28 = HK7*P(4,22);
const float HK29 = -HK14*P(22,23) + HK14*P(5,22) + HK15*P(0,22) + HK16*P(1,22) + HK17*P(3,22) + HK28;
const float HK30 = -HK14*P(1,23) + HK14*P(1,5) + HK15*P(0,1) + HK16*P(1,1) + HK17*P(1,3) + HK7*P(1,4);
const float HK31 = -HK14*P(2,23) + HK14*P(2,5) + HK15*P(0,2) + HK16*P(1,2) + HK17*P(2,3) + HK7*P(2,4);
const float HK32 = -HK14*P(3,23) + HK14*P(3,5) + HK15*P(0,3) + HK16*P(1,3) + HK17*P(3,3) + HK7*P(3,4);
const float HK33 = Kaccx/(2*HK11*HK19*(HK20*P(6,22) + HK21*P(2,6) + HK22*P(6,6) + HK26) - HK14*HK19*(HK20*P(5,22) + HK21*P(2,5) + HK22*P(5,6) + HK25) - HK15*HK19*(HK18 + HK20*P(0,22) + HK21*P(0,2) + HK22*P(0,6)) - HK16*HK19*(HK20*P(1,22) + HK21*P(1,2) + HK22*P(1,6) + HK30) - HK17*HK19*(HK20*P(3,22) + HK21*P(2,3) + HK22*P(3,6) + HK32) + 2*HK19*HK5*(HK20*P(2,22) + HK21*P(2,2) + HK22*P(2,6) + HK31) + HK19*HK7*(HK20*P(22,22) + HK21*P(2,22) + HK22*P(6,22) + HK29) - HK19*HK7*(HK20*P(4,22) + HK21*P(2,4) + HK22*P(4,6) + HK27) + 2*HK19*HK9*(HK20*P(22,23) + HK21*P(2,23) + HK22*P(6,23) + HK24) - R_ACC);
// Observation Jacobians
Hfusion.at<0>() = -HK2*HK3;
Hfusion.at<1>() = -HK3*HK4;
Hfusion.at<2>() = HK3*HK5;
Hfusion.at<3>() = -HK3*HK6;
Hfusion.at<4>() = -HK8;
Hfusion.at<5>() = -HK10;
Hfusion.at<6>() = HK11*HK3;
Hfusion.at<22>() = HK8;
Hfusion.at<23>() = HK10;
// Kalman gains
Kfusion(0) = HK33*(-HK12*P(0,2) - HK13*P(0,6) + HK18 - HK7*P(0,22));
Kfusion(1) = HK33*(-HK12*P(1,2) - HK13*P(1,6) + HK30 - HK7*P(1,22));
Kfusion(2) = HK33*(-HK12*P(2,2) - HK13*P(2,6) + HK31 - HK7*P(2,22));
Kfusion(3) = HK33*(-HK12*P(2,3) - HK13*P(3,6) + HK32 - HK7*P(3,22));
Kfusion(4) = HK33*(-HK12*P(2,4) - HK13*P(4,6) + HK27 - HK28);
Kfusion(5) = HK33*(-HK12*P(2,5) - HK13*P(5,6) + HK25 - HK7*P(5,22));
Kfusion(6) = HK33*(-HK12*P(2,6) - HK13*P(6,6) + HK26 - HK7*P(6,22));
Kfusion(7) = HK33*(-HK12*P(2,7) - HK13*P(6,7) + HK14*P(5,7) - HK14*P(7,23) + HK15*P(0,7) + HK16*P(1,7) + HK17*P(3,7) + HK7*P(4,7) - HK7*P(7,22));
Kfusion(8) = HK33*(-HK12*P(2,8) - HK13*P(6,8) + HK14*P(5,8) - HK14*P(8,23) + HK15*P(0,8) + HK16*P(1,8) + HK17*P(3,8) + HK7*P(4,8) - HK7*P(8,22));
Kfusion(9) = HK33*(-HK12*P(2,9) - HK13*P(6,9) + HK14*P(5,9) - HK14*P(9,23) + HK15*P(0,9) + HK16*P(1,9) + HK17*P(3,9) + HK7*P(4,9) - HK7*P(9,22));
Kfusion(10) = HK33*(-HK12*P(2,10) - HK13*P(6,10) - HK14*P(10,23) + HK14*P(5,10) + HK15*P(0,10) + HK16*P(1,10) + HK17*P(3,10) - HK7*P(10,22) + HK7*P(4,10));
Kfusion(11) = HK33*(-HK12*P(2,11) - HK13*P(6,11) - HK14*P(11,23) + HK14*P(5,11) + HK15*P(0,11) + HK16*P(1,11) + HK17*P(3,11) - HK7*P(11,22) + HK7*P(4,11));
Kfusion(12) = HK33*(-HK12*P(2,12) - HK13*P(6,12) - HK14*P(12,23) + HK14*P(5,12) + HK15*P(0,12) + HK16*P(1,12) + HK17*P(3,12) - HK7*P(12,22) + HK7*P(4,12));
Kfusion(13) = HK33*(-HK12*P(2,13) - HK13*P(6,13) - HK14*P(13,23) + HK14*P(5,13) + HK15*P(0,13) + HK16*P(1,13) + HK17*P(3,13) - HK7*P(13,22) + HK7*P(4,13));
Kfusion(14) = HK33*(-HK12*P(2,14) - HK13*P(6,14) - HK14*P(14,23) + HK14*P(5,14) + HK15*P(0,14) + HK16*P(1,14) + HK17*P(3,14) - HK7*P(14,22) + HK7*P(4,14));
Kfusion(15) = HK33*(-HK12*P(2,15) - HK13*P(6,15) - HK14*P(15,23) + HK14*P(5,15) + HK15*P(0,15) + HK16*P(1,15) + HK17*P(3,15) - HK7*P(15,22) + HK7*P(4,15));
Kfusion(16) = HK33*(-HK12*P(2,16) - HK13*P(6,16) - HK14*P(16,23) + HK14*P(5,16) + HK15*P(0,16) + HK16*P(1,16) + HK17*P(3,16) - HK7*P(16,22) + HK7*P(4,16));
Kfusion(17) = HK33*(-HK12*P(2,17) - HK13*P(6,17) - HK14*P(17,23) + HK14*P(5,17) + HK15*P(0,17) + HK16*P(1,17) + HK17*P(3,17) - HK7*P(17,22) + HK7*P(4,17));
Kfusion(18) = HK33*(-HK12*P(2,18) - HK13*P(6,18) - HK14*P(18,23) + HK14*P(5,18) + HK15*P(0,18) + HK16*P(1,18) + HK17*P(3,18) - HK7*P(18,22) + HK7*P(4,18));
Kfusion(19) = HK33*(-HK12*P(2,19) - HK13*P(6,19) - HK14*P(19,23) + HK14*P(5,19) + HK15*P(0,19) + HK16*P(1,19) + HK17*P(3,19) - HK7*P(19,22) + HK7*P(4,19));
Kfusion(20) = HK33*(-HK12*P(2,20) - HK13*P(6,20) - HK14*P(20,23) + HK14*P(5,20) + HK15*P(0,20) + HK16*P(1,20) + HK17*P(3,20) - HK7*P(20,22) + HK7*P(4,20));
Kfusion(21) = HK33*(-HK12*P(2,21) - HK13*P(6,21) - HK14*P(21,23) + HK14*P(5,21) + HK15*P(0,21) + HK16*P(1,21) + HK17*P(3,21) - HK7*P(21,22) + HK7*P(4,21));
Kfusion(22) = HK33*(-HK12*P(2,22) - HK13*P(6,22) + HK29 - HK7*P(22,22));
Kfusion(23) = HK33*(-HK12*P(2,23) - HK13*P(6,23) + HK24 - HK7*P(22,23));
// Predicted observation
// Innovation variance
// Axis 1 equations
// Sub Expressions
const float HK0 = ve - vwe;
const float HK1 = vn - vwn;
const float HK2 = HK0*q0 - HK1*q3 + q1*vd;
const float HK3 = 2*Kaccy;
const float HK4 = -HK0*q1 + HK1*q2 + q0*vd;
const float HK5 = HK0*q2 + HK1*q1 + q3*vd;
const float HK6 = HK0*q3 + HK1*q0 - q2*vd;
const float HK7 = q0*q3 - q1*q2;
const float HK8 = HK3*HK7;
const float HK9 = (q0)*(q0) - (q1)*(q1) + (q2)*(q2) - (q3)*(q3);
const float HK10 = HK9*Kaccy;
const float HK11 = q0*q1 + q2*q3;
const float HK12 = 2*HK6;
const float HK13 = 2*HK7;
const float HK14 = 2*HK2;
const float HK15 = 2*HK4;
const float HK16 = 2*HK5;
const float HK17 = 2*HK11;
const float HK18 = HK13*P(0,22) + HK14*P(0,0) + HK15*P(0,1) + HK16*P(0,2) + HK17*P(0,6) + HK9*P(0,5);
const float HK19 = (Kaccy)*(Kaccy);
const float HK20 = -HK9;
const float HK21 = -2*HK6;
const float HK22 = -2*HK7;
const float HK23 = HK13*P(6,22) + HK14*P(0,6) + HK15*P(1,6) + HK16*P(2,6) + HK17*P(6,6) + HK9*P(5,6);
const float HK24 = HK13*P(22,22) + HK14*P(0,22) + HK15*P(1,22) + HK16*P(2,22) + HK17*P(6,22) + HK9*P(5,22);
const float HK25 = HK13*P(4,22);
const float HK26 = HK14*P(0,4) + HK15*P(1,4) + HK16*P(2,4) + HK17*P(4,6) + HK25 + HK9*P(4,5);
const float HK27 = HK13*P(5,22) + HK14*P(0,5) + HK15*P(1,5) + HK16*P(2,5) + HK17*P(5,6) + HK9*P(5,5);
const float HK28 = HK9*P(5,23);
const float HK29 = HK13*P(22,23) + HK14*P(0,23) + HK15*P(1,23) + HK16*P(2,23) + HK17*P(6,23) + HK28;
const float HK30 = HK13*P(2,22) + HK14*P(0,2) + HK15*P(1,2) + HK16*P(2,2) + HK17*P(2,6) + HK9*P(2,5);
const float HK31 = HK13*P(1,22) + HK14*P(0,1) + HK15*P(1,1) + HK16*P(1,2) + HK17*P(1,6) + HK9*P(1,5);
const float HK32 = HK13*P(3,22) + HK14*P(0,3) + HK15*P(1,3) + HK16*P(2,3) + HK17*P(3,6) + HK9*P(3,5);
const float HK33 = Kaccy/(-HK13*HK19*(HK20*P(22,23) + HK21*P(3,22) + HK22*P(4,22) + HK24) - HK14*HK19*(HK18 + HK20*P(0,23) + HK21*P(0,3) + HK22*P(0,4)) - HK15*HK19*(HK20*P(1,23) + HK21*P(1,3) + HK22*P(1,4) + HK31) - HK16*HK19*(HK20*P(2,23) + HK21*P(2,3) + HK22*P(2,4) + HK30) - HK17*HK19*(HK20*P(6,23) + HK21*P(3,6) + HK22*P(4,6) + HK23) + 2*HK19*HK6*(HK20*P(3,23) + HK21*P(3,3) + HK22*P(3,4) + HK32) + 2*HK19*HK7*(HK20*P(4,23) + HK21*P(3,4) + HK22*P(4,4) + HK26) + HK19*HK9*(HK20*P(23,23) + HK21*P(3,23) + HK22*P(4,23) + HK29) - HK19*HK9*(HK20*P(5,23) + HK21*P(3,5) + HK22*P(4,5) + HK27) - R_ACC);
// Observation Jacobians
Hfusion.at<0>() = -HK2*HK3;
Hfusion.at<1>() = -HK3*HK4;
Hfusion.at<2>() = -HK3*HK5;
Hfusion.at<3>() = HK3*HK6;
Hfusion.at<4>() = HK8;
Hfusion.at<5>() = -HK10;
Hfusion.at<6>() = -HK11*HK3;
Hfusion.at<22>() = -HK8;
Hfusion.at<23>() = HK10;
// Kalman gains
Kfusion(0) = HK33*(-HK12*P(0,3) - HK13*P(0,4) + HK18 - HK9*P(0,23));
Kfusion(1) = HK33*(-HK12*P(1,3) - HK13*P(1,4) + HK31 - HK9*P(1,23));
Kfusion(2) = HK33*(-HK12*P(2,3) - HK13*P(2,4) + HK30 - HK9*P(2,23));
Kfusion(3) = HK33*(-HK12*P(3,3) - HK13*P(3,4) + HK32 - HK9*P(3,23));
Kfusion(4) = HK33*(-HK12*P(3,4) - HK13*P(4,4) + HK26 - HK9*P(4,23));
Kfusion(5) = HK33*(-HK12*P(3,5) - HK13*P(4,5) + HK27 - HK28);
Kfusion(6) = HK33*(-HK12*P(3,6) - HK13*P(4,6) + HK23 - HK9*P(6,23));
Kfusion(7) = HK33*(-HK12*P(3,7) - HK13*P(4,7) + HK13*P(7,22) + HK14*P(0,7) + HK15*P(1,7) + HK16*P(2,7) + HK17*P(6,7) + HK9*P(5,7) - HK9*P(7,23));
Kfusion(8) = HK33*(-HK12*P(3,8) - HK13*P(4,8) + HK13*P(8,22) + HK14*P(0,8) + HK15*P(1,8) + HK16*P(2,8) + HK17*P(6,8) + HK9*P(5,8) - HK9*P(8,23));
Kfusion(9) = HK33*(-HK12*P(3,9) - HK13*P(4,9) + HK13*P(9,22) + HK14*P(0,9) + HK15*P(1,9) + HK16*P(2,9) + HK17*P(6,9) + HK9*P(5,9) - HK9*P(9,23));
Kfusion(10) = HK33*(-HK12*P(3,10) + HK13*P(10,22) - HK13*P(4,10) + HK14*P(0,10) + HK15*P(1,10) + HK16*P(2,10) + HK17*P(6,10) - HK9*P(10,23) + HK9*P(5,10));
Kfusion(11) = HK33*(-HK12*P(3,11) + HK13*P(11,22) - HK13*P(4,11) + HK14*P(0,11) + HK15*P(1,11) + HK16*P(2,11) + HK17*P(6,11) - HK9*P(11,23) + HK9*P(5,11));
Kfusion(12) = HK33*(-HK12*P(3,12) + HK13*P(12,22) - HK13*P(4,12) + HK14*P(0,12) + HK15*P(1,12) + HK16*P(2,12) + HK17*P(6,12) - HK9*P(12,23) + HK9*P(5,12));
Kfusion(13) = HK33*(-HK12*P(3,13) + HK13*P(13,22) - HK13*P(4,13) + HK14*P(0,13) + HK15*P(1,13) + HK16*P(2,13) + HK17*P(6,13) - HK9*P(13,23) + HK9*P(5,13));
Kfusion(14) = HK33*(-HK12*P(3,14) + HK13*P(14,22) - HK13*P(4,14) + HK14*P(0,14) + HK15*P(1,14) + HK16*P(2,14) + HK17*P(6,14) - HK9*P(14,23) + HK9*P(5,14));
Kfusion(15) = HK33*(-HK12*P(3,15) + HK13*P(15,22) - HK13*P(4,15) + HK14*P(0,15) + HK15*P(1,15) + HK16*P(2,15) + HK17*P(6,15) - HK9*P(15,23) + HK9*P(5,15));
Kfusion(16) = HK33*(-HK12*P(3,16) + HK13*P(16,22) - HK13*P(4,16) + HK14*P(0,16) + HK15*P(1,16) + HK16*P(2,16) + HK17*P(6,16) - HK9*P(16,23) + HK9*P(5,16));
Kfusion(17) = HK33*(-HK12*P(3,17) + HK13*P(17,22) - HK13*P(4,17) + HK14*P(0,17) + HK15*P(1,17) + HK16*P(2,17) + HK17*P(6,17) - HK9*P(17,23) + HK9*P(5,17));
Kfusion(18) = HK33*(-HK12*P(3,18) + HK13*P(18,22) - HK13*P(4,18) + HK14*P(0,18) + HK15*P(1,18) + HK16*P(2,18) + HK17*P(6,18) - HK9*P(18,23) + HK9*P(5,18));
Kfusion(19) = HK33*(-HK12*P(3,19) + HK13*P(19,22) - HK13*P(4,19) + HK14*P(0,19) + HK15*P(1,19) + HK16*P(2,19) + HK17*P(6,19) - HK9*P(19,23) + HK9*P(5,19));
Kfusion(20) = HK33*(-HK12*P(3,20) + HK13*P(20,22) - HK13*P(4,20) + HK14*P(0,20) + HK15*P(1,20) + HK16*P(2,20) + HK17*P(6,20) - HK9*P(20,23) + HK9*P(5,20));
Kfusion(21) = HK33*(-HK12*P(3,21) + HK13*P(21,22) - HK13*P(4,21) + HK14*P(0,21) + HK15*P(1,21) + HK16*P(2,21) + HK17*P(6,21) - HK9*P(21,23) + HK9*P(5,21));
Kfusion(22) = HK33*(-HK12*P(3,22) + HK24 - HK25 - HK9*P(22,23));
Kfusion(23) = HK33*(-HK12*P(3,23) - HK13*P(4,23) + HK29 - HK9*P(23,23));
// Predicted observation
// Innovation variance
src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp
-154
View File
@@ -1,154 +0,0 @@
// Sub Expressions
const float HK0 = vn - vwn;
const float HK1 = ve - vwe;
const float HK2 = HK0*q0 + HK1*q3 - q2*vd;
const float HK3 = 2*Kaccx;
const float HK4 = HK0*q1 + HK1*q2 + q3*vd;
const float HK5 = HK0*q2 - HK1*q1 + q0*vd;
const float HK6 = -HK0*q3 + HK1*q0 + q1*vd;
const float HK7 = (q1)*(q1);
const float HK8 = (q2)*(q2);
const float HK9 = (q0)*(q0) - (q3)*(q3);
const float HK10 = HK7 - HK8 + HK9;
const float HK11 = HK10*Kaccx;
const float HK12 = q0*q3;
const float HK13 = q1*q2;
const float HK14 = HK12 + HK13;
const float HK15 = HK14*HK3;
const float HK16 = q0*q2 - q1*q3;
const float HK17 = 2*HK5;
const float HK18 = 2*HK16;
const float HK19 = 2*HK14;
const float HK20 = 2*HK2;
const float HK21 = 2*HK4;
const float HK22 = 2*HK6;
const float HK23 = HK22*P(0,3);
const float HK24 = HK10*P(0,4) - HK19*P(0,23) + HK19*P(0,5) + HK20*P(0,0) + HK21*P(0,1) + HK23;
const float HK25 = (Kaccx)*(Kaccx);
const float HK26 = -HK10;
const float HK27 = -2*HK5;
const float HK28 = -2*HK16;
const float HK29 = HK19*P(5,23);
const float HK30 = HK10*P(4,23) - HK19*P(23,23) + HK20*P(0,23) + HK21*P(1,23) + HK22*P(3,23) + HK29;
const float HK31 = HK10*P(4,5) + HK19*P(5,5) + HK20*P(0,5) + HK21*P(1,5) + HK22*P(3,5) - HK29;
const float HK32 = HK10*P(4,6) + HK19*P(5,6) - HK19*P(6,23) + HK20*P(0,6) + HK21*P(1,6) + HK22*P(3,6);
const float HK33 = HK10*P(4,4) - HK19*P(4,23) + HK19*P(4,5) + HK20*P(0,4) + HK21*P(1,4) + HK22*P(3,4);
const float HK34 = HK10*P(4,22);
const float HK35 = -HK19*P(22,23) + HK19*P(5,22) + HK20*P(0,22) + HK21*P(1,22) + HK22*P(3,22) + HK34;
const float HK36 = HK10*P(1,4) - HK19*P(1,23) + HK19*P(1,5) + HK20*P(0,1) + HK21*P(1,1) + HK22*P(1,3);
const float HK37 = HK21*P(1,2);
const float HK38 = HK10*P(2,4) - HK19*P(2,23) + HK19*P(2,5) + HK20*P(0,2) + HK22*P(2,3) + HK37;
const float HK39 = HK20*P(0,3);
const float HK40 = HK10*P(3,4) - HK19*P(3,23) + HK19*P(3,5) + HK21*P(1,3) + HK22*P(3,3) + HK39;
const float HK41 = Kaccx/(HK10*HK25*(HK26*P(22,22) + HK27*P(2,22) + HK28*P(6,22) + HK35) - HK10*HK25*(HK26*P(4,22) + HK27*P(2,4) + HK28*P(4,6) + HK33) + 2*HK14*HK25*(HK26*P(22,23) + HK27*P(2,23) + HK28*P(6,23) + HK30) + 2*HK16*HK25*(HK26*P(6,22) + HK27*P(2,6) + HK28*P(6,6) + HK32) - HK19*HK25*(HK26*P(5,22) + HK27*P(2,5) + HK28*P(5,6) + HK31) - HK20*HK25*(HK24 + HK26*P(0,22) + HK27*P(0,2) + HK28*P(0,6)) - HK21*HK25*(HK26*P(1,22) + HK27*P(1,2) + HK28*P(1,6) + HK36) - HK22*HK25*(HK26*P(3,22) + HK27*P(2,3) + HK28*P(3,6) + HK40) + 2*HK25*HK5*(HK26*P(2,22) + HK27*P(2,2) + HK28*P(2,6) + HK38) - R_ACC);
const float HK42 = HK17*P(1,2);
const float HK43 = HK12 - HK13;
const float HK44 = 2*Kaccy;
const float HK45 = HK43*HK44;
const float HK46 = -HK7 + HK8 + HK9;
const float HK47 = HK46*Kaccy;
const float HK48 = q0*q1 + q2*q3;
const float HK49 = 2*HK43;
const float HK50 = 2*HK48;
const float HK51 = HK17*P(0,1) + HK21*P(0,2) + HK22*P(0,0) + HK46*P(0,5) + HK49*P(0,22) + HK50*P(0,6);
const float HK52 = (Kaccy)*(Kaccy);
const float HK53 = -HK46;
const float HK54 = -2*HK2;
const float HK55 = -2*HK43;
const float HK56 = HK17*P(1,6) + HK21*P(2,6) + HK22*P(0,6) + HK46*P(5,6) + HK49*P(6,22) + HK50*P(6,6);
const float HK57 = HK17*P(1,22) + HK21*P(2,22) + HK22*P(0,22) + HK46*P(5,22) + HK49*P(22,22) + HK50*P(6,22);
const float HK58 = HK49*P(4,22);
const float HK59 = HK17*P(1,4) + HK21*P(2,4) + HK22*P(0,4) + HK46*P(4,5) + HK50*P(4,6) + HK58;
const float HK60 = HK17*P(1,5) + HK21*P(2,5) + HK22*P(0,5) + HK46*P(5,5) + HK49*P(5,22) + HK50*P(5,6);
const float HK61 = HK46*P(5,23);
const float HK62 = HK17*P(1,23) + HK21*P(2,23) + HK22*P(0,23) + HK49*P(22,23) + HK50*P(6,23) + HK61;
const float HK63 = HK21*P(2,2) + HK22*P(0,2) + HK42 + HK46*P(2,5) + HK49*P(2,22) + HK50*P(2,6);
const float HK64 = HK17*P(1,1) + HK22*P(0,1) + HK37 + HK46*P(1,5) + HK49*P(1,22) + HK50*P(1,6);
const float HK65 = HK17*P(1,3) + HK21*P(2,3) + HK23 + HK46*P(3,5) + HK49*P(3,22) + HK50*P(3,6);
const float HK66 = Kaccy/(-HK17*HK52*(HK53*P(1,23) + HK54*P(1,3) + HK55*P(1,4) + HK64) + 2*HK2*HK52*(HK53*P(3,23) + HK54*P(3,3) + HK55*P(3,4) + HK65) - HK21*HK52*(HK53*P(2,23) + HK54*P(2,3) + HK55*P(2,4) + HK63) - HK22*HK52*(HK51 + HK53*P(0,23) + HK54*P(0,3) + HK55*P(0,4)) + 2*HK43*HK52*(HK53*P(4,23) + HK54*P(3,4) + HK55*P(4,4) + HK59) + HK46*HK52*(HK53*P(23,23) + HK54*P(3,23) + HK55*P(4,23) + HK62) - HK46*HK52*(HK53*P(5,23) + HK54*P(3,5) + HK55*P(4,5) + HK60) - HK49*HK52*(HK53*P(22,23) + HK54*P(3,22) + HK55*P(4,22) + HK57) - HK50*HK52*(HK53*P(6,23) + HK54*P(3,6) + HK55*P(4,6) + HK56) - R_ACC);
// Observation Jacobians - axis 0
Hfusion.at<0>() = -HK2*HK3;
Hfusion.at<1>() = -HK3*HK4;
Hfusion.at<2>() = HK3*HK5;
Hfusion.at<3>() = -HK3*HK6;
Hfusion.at<4>() = -HK11;
Hfusion.at<5>() = -HK15;
Hfusion.at<6>() = HK16*HK3;
Hfusion.at<22>() = HK11;
Hfusion.at<23>() = HK15;
// Kalman gains - axis 0
Kfusion(0) = HK41*(-HK10*P(0,22) - HK17*P(0,2) - HK18*P(0,6) + HK24);
Kfusion(1) = HK41*(-HK10*P(1,22) - HK18*P(1,6) + HK36 - HK42);
Kfusion(2) = HK41*(-HK10*P(2,22) - HK17*P(2,2) - HK18*P(2,6) + HK38);
Kfusion(3) = HK41*(-HK10*P(3,22) - HK17*P(2,3) - HK18*P(3,6) + HK40);
Kfusion(4) = HK41*(-HK17*P(2,4) - HK18*P(4,6) + HK33 - HK34);
Kfusion(5) = HK41*(-HK10*P(5,22) - HK17*P(2,5) - HK18*P(5,6) + HK31);
Kfusion(6) = HK41*(-HK10*P(6,22) - HK17*P(2,6) - HK18*P(6,6) + HK32);
Kfusion(7) = HK41*(HK10*P(4,7) - HK10*P(7,22) - HK17*P(2,7) - HK18*P(6,7) + HK19*P(5,7) - HK19*P(7,23) + HK20*P(0,7) + HK21*P(1,7) + HK22*P(3,7));
Kfusion(8) = HK41*(HK10*P(4,8) - HK10*P(8,22) - HK17*P(2,8) - HK18*P(6,8) + HK19*P(5,8) - HK19*P(8,23) + HK20*P(0,8) + HK21*P(1,8) + HK22*P(3,8));
Kfusion(9) = HK41*(HK10*P(4,9) - HK10*P(9,22) - HK17*P(2,9) - HK18*P(6,9) + HK19*P(5,9) - HK19*P(9,23) + HK20*P(0,9) + HK21*P(1,9) + HK22*P(3,9));
Kfusion(10) = HK41*(-HK10*P(10,22) + HK10*P(4,10) - HK17*P(2,10) - HK18*P(6,10) - HK19*P(10,23) + HK19*P(5,10) + HK20*P(0,10) + HK21*P(1,10) + HK22*P(3,10));
Kfusion(11) = HK41*(-HK10*P(11,22) + HK10*P(4,11) - HK17*P(2,11) - HK18*P(6,11) - HK19*P(11,23) + HK19*P(5,11) + HK20*P(0,11) + HK21*P(1,11) + HK22*P(3,11));
Kfusion(12) = HK41*(-HK10*P(12,22) + HK10*P(4,12) - HK17*P(2,12) - HK18*P(6,12) - HK19*P(12,23) + HK19*P(5,12) + HK20*P(0,12) + HK21*P(1,12) + HK22*P(3,12));
Kfusion(13) = HK41*(-HK10*P(13,22) + HK10*P(4,13) - HK17*P(2,13) - HK18*P(6,13) - HK19*P(13,23) + HK19*P(5,13) + HK20*P(0,13) + HK21*P(1,13) + HK22*P(3,13));
Kfusion(14) = HK41*(-HK10*P(14,22) + HK10*P(4,14) - HK17*P(2,14) - HK18*P(6,14) - HK19*P(14,23) + HK19*P(5,14) + HK20*P(0,14) + HK21*P(1,14) + HK22*P(3,14));
Kfusion(15) = HK41*(-HK10*P(15,22) + HK10*P(4,15) - HK17*P(2,15) - HK18*P(6,15) - HK19*P(15,23) + HK19*P(5,15) + HK20*P(0,15) + HK21*P(1,15) + HK22*P(3,15));
Kfusion(16) = HK41*(-HK10*P(16,22) + HK10*P(4,16) - HK17*P(2,16) - HK18*P(6,16) - HK19*P(16,23) + HK19*P(5,16) + HK20*P(0,16) + HK21*P(1,16) + HK22*P(3,16));
Kfusion(17) = HK41*(-HK10*P(17,22) + HK10*P(4,17) - HK17*P(2,17) - HK18*P(6,17) - HK19*P(17,23) + HK19*P(5,17) + HK20*P(0,17) + HK21*P(1,17) + HK22*P(3,17));
Kfusion(18) = HK41*(-HK10*P(18,22) + HK10*P(4,18) - HK17*P(2,18) - HK18*P(6,18) - HK19*P(18,23) + HK19*P(5,18) + HK20*P(0,18) + HK21*P(1,18) + HK22*P(3,18));
Kfusion(19) = HK41*(-HK10*P(19,22) + HK10*P(4,19) - HK17*P(2,19) - HK18*P(6,19) - HK19*P(19,23) + HK19*P(5,19) + HK20*P(0,19) + HK21*P(1,19) + HK22*P(3,19));
Kfusion(20) = HK41*(-HK10*P(20,22) + HK10*P(4,20) - HK17*P(2,20) - HK18*P(6,20) - HK19*P(20,23) + HK19*P(5,20) + HK20*P(0,20) + HK21*P(1,20) + HK22*P(3,20));
Kfusion(21) = HK41*(-HK10*P(21,22) + HK10*P(4,21) - HK17*P(2,21) - HK18*P(6,21) - HK19*P(21,23) + HK19*P(5,21) + HK20*P(0,21) + HK21*P(1,21) + HK22*P(3,21));
Kfusion(22) = HK41*(-HK10*P(22,22) - HK17*P(2,22) - HK18*P(6,22) + HK35);
Kfusion(23) = HK41*(-HK10*P(22,23) - HK17*P(2,23) - HK18*P(6,23) + HK30);
// Observation Jacobians - axis 1
Hfusion.at<0>() = -HK22*Kaccy;
Hfusion.at<1>() = -HK17*Kaccy;
Hfusion.at<2>() = -HK21*Kaccy;
Hfusion.at<3>() = HK20*Kaccy;
Hfusion.at<4>() = HK45;
Hfusion.at<5>() = -HK47;
Hfusion.at<6>() = -HK44*HK48;
Hfusion.at<22>() = -HK45;
Hfusion.at<23>() = HK47;
// Kalman gains - axis 1
Kfusion(0) = HK66*(-HK39 - HK46*P(0,23) - HK49*P(0,4) + HK51);
Kfusion(1) = HK66*(-HK20*P(1,3) - HK46*P(1,23) - HK49*P(1,4) + HK64);
Kfusion(2) = HK66*(-HK20*P(2,3) - HK46*P(2,23) - HK49*P(2,4) + HK63);
Kfusion(3) = HK66*(-HK20*P(3,3) - HK46*P(3,23) - HK49*P(3,4) + HK65);
Kfusion(4) = HK66*(-HK20*P(3,4) - HK46*P(4,23) - HK49*P(4,4) + HK59);
Kfusion(5) = HK66*(-HK20*P(3,5) - HK49*P(4,5) + HK60 - HK61);
Kfusion(6) = HK66*(-HK20*P(3,6) - HK46*P(6,23) - HK49*P(4,6) + HK56);
Kfusion(7) = HK66*(HK17*P(1,7) - HK20*P(3,7) + HK21*P(2,7) + HK22*P(0,7) + HK46*P(5,7) - HK46*P(7,23) - HK49*P(4,7) + HK49*P(7,22) + HK50*P(6,7));
Kfusion(8) = HK66*(HK17*P(1,8) - HK20*P(3,8) + HK21*P(2,8) + HK22*P(0,8) + HK46*P(5,8) - HK46*P(8,23) - HK49*P(4,8) + HK49*P(8,22) + HK50*P(6,8));
Kfusion(9) = HK66*(HK17*P(1,9) - HK20*P(3,9) + HK21*P(2,9) + HK22*P(0,9) + HK46*P(5,9) - HK46*P(9,23) - HK49*P(4,9) + HK49*P(9,22) + HK50*P(6,9));
Kfusion(10) = HK66*(HK17*P(1,10) - HK20*P(3,10) + HK21*P(2,10) + HK22*P(0,10) - HK46*P(10,23) + HK46*P(5,10) + HK49*P(10,22) - HK49*P(4,10) + HK50*P(6,10));
Kfusion(11) = HK66*(HK17*P(1,11) - HK20*P(3,11) + HK21*P(2,11) + HK22*P(0,11) - HK46*P(11,23) + HK46*P(5,11) + HK49*P(11,22) - HK49*P(4,11) + HK50*P(6,11));
Kfusion(12) = HK66*(HK17*P(1,12) - HK20*P(3,12) + HK21*P(2,12) + HK22*P(0,12) - HK46*P(12,23) + HK46*P(5,12) + HK49*P(12,22) - HK49*P(4,12) + HK50*P(6,12));
Kfusion(13) = HK66*(HK17*P(1,13) - HK20*P(3,13) + HK21*P(2,13) + HK22*P(0,13) - HK46*P(13,23) + HK46*P(5,13) + HK49*P(13,22) - HK49*P(4,13) + HK50*P(6,13));
Kfusion(14) = HK66*(HK17*P(1,14) - HK20*P(3,14) + HK21*P(2,14) + HK22*P(0,14) - HK46*P(14,23) + HK46*P(5,14) + HK49*P(14,22) - HK49*P(4,14) + HK50*P(6,14));
Kfusion(15) = HK66*(HK17*P(1,15) - HK20*P(3,15) + HK21*P(2,15) + HK22*P(0,15) - HK46*P(15,23) + HK46*P(5,15) + HK49*P(15,22) - HK49*P(4,15) + HK50*P(6,15));
Kfusion(16) = HK66*(HK17*P(1,16) - HK20*P(3,16) + HK21*P(2,16) + HK22*P(0,16) - HK46*P(16,23) + HK46*P(5,16) + HK49*P(16,22) - HK49*P(4,16) + HK50*P(6,16));
Kfusion(17) = HK66*(HK17*P(1,17) - HK20*P(3,17) + HK21*P(2,17) + HK22*P(0,17) - HK46*P(17,23) + HK46*P(5,17) + HK49*P(17,22) - HK49*P(4,17) + HK50*P(6,17));
Kfusion(18) = HK66*(HK17*P(1,18) - HK20*P(3,18) + HK21*P(2,18) + HK22*P(0,18) - HK46*P(18,23) + HK46*P(5,18) + HK49*P(18,22) - HK49*P(4,18) + HK50*P(6,18));
Kfusion(19) = HK66*(HK17*P(1,19) - HK20*P(3,19) + HK21*P(2,19) + HK22*P(0,19) - HK46*P(19,23) + HK46*P(5,19) + HK49*P(19,22) - HK49*P(4,19) + HK50*P(6,19));
Kfusion(20) = HK66*(HK17*P(1,20) - HK20*P(3,20) + HK21*P(2,20) + HK22*P(0,20) - HK46*P(20,23) + HK46*P(5,20) + HK49*P(20,22) - HK49*P(4,20) + HK50*P(6,20));
Kfusion(21) = HK66*(HK17*P(1,21) - HK20*P(3,21) + HK21*P(2,21) + HK22*P(0,21) - HK46*P(21,23) + HK46*P(5,21) + HK49*P(21,22) - HK49*P(4,21) + HK50*P(6,21));
Kfusion(22) = HK66*(-HK20*P(3,22) - HK46*P(22,23) + HK57 - HK58);
Kfusion(23) = HK66*(-HK20*P(3,23) - HK46*P(23,23) - HK49*P(4,23) + HK62);
// Observation Jacobians - axis 2
// Kalman gains - axis 2
src/modules/ekf2/EKF/python/ekf_derivation/generated/util.h
-24
View File
@@ -1,24 +0,0 @@
#include "../../../../../matrix/matrix/math.hpp"
typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
inline float sq(float in) {
return in * in;
}
namespace ecl{
inline float powf(float x, int exp)
{
float ret;
if (exp > 0) {
ret = x;
for (int count = 1; count < exp; count++) {
ret *= x;
}
return ret;
} else if (exp < 0) {
return 1.0f / ecl::powf(x, -exp);
}
return 1.0f;
}
}
src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp
-177
View File
@@ -1,177 +0,0 @@
// axis 0
const float HK0 = q0*vn - q2*vd + q3*ve;
const float HK1 = q1*vn + q2*ve + q3*vd;
const float HK2 = q0*vd - q1*ve + q2*vn;
const float HK3 = q0*ve + q1*vd - q3*vn;
const float HK4 = (q0)*(q0) + (q1)*(q1) - (q2)*(q2) - (q3)*(q3);
const float HK5 = q0*q3 + q1*q2;
const float HK6 = q0*q2 - q1*q3;
const float HK7 = 2*HK5;
const float HK8 = 2*HK6;
const float HK9 = 2*HK1;
const float HK10 = 2*HK0;
const float HK11 = 2*HK2;
const float HK12 = 2*HK3;
const float HK13 = HK10*P(0,0) - HK11*P(0,2) + HK12*P(0,3) + HK4*P(0,4) + HK7*P(0,5) - HK8*P(0,6) + HK9*P(0,1);
const float HK14 = HK10*P(0,5) - HK11*P(2,5) + HK12*P(3,5) + HK4*P(4,5) + HK7*P(5,5) - HK8*P(5,6) + HK9*P(1,5);
const float HK15 = HK10*P(0,6) - HK11*P(2,6) + HK12*P(3,6) + HK4*P(4,6) + HK7*P(5,6) - HK8*P(6,6) + HK9*P(1,6);
const float HK16 = HK10*P(0,1) - HK11*P(1,2) + HK12*P(1,3) + HK4*P(1,4) + HK7*P(1,5) - HK8*P(1,6) + HK9*P(1,1);
const float HK17 = HK10*P(0,2) - HK11*P(2,2) + HK12*P(2,3) + HK4*P(2,4) + HK7*P(2,5) - HK8*P(2,6) + HK9*P(1,2);
const float HK18 = HK10*P(0,3) - HK11*P(2,3) + HK12*P(3,3) + HK4*P(3,4) + HK7*P(3,5) - HK8*P(3,6) + HK9*P(1,3);
const float HK19 = HK10*P(0,4) - HK11*P(2,4) + HK12*P(3,4) + HK4*P(4,4) + HK7*P(4,5) - HK8*P(4,6) + HK9*P(1,4);
const float HK20 = 1.0F/(HK10*HK13 - HK11*HK17 + HK12*HK18 + HK14*HK7 - HK15*HK8 + HK16*HK9 + HK19*HK4 + R_VEL);
H_VEL(0) = 2*HK0;
H_VEL(1) = 2*HK1;
H_VEL(2) = -2*HK2;
H_VEL(3) = 2*HK3;
H_VEL(4) = HK4;
H_VEL(5) = 2*HK5;
H_VEL(6) = -2*HK6;
Kfusion(0) = HK13*HK20;
Kfusion(1) = HK16*HK20;
Kfusion(2) = HK17*HK20;
Kfusion(3) = HK18*HK20;
Kfusion(4) = HK19*HK20;
Kfusion(5) = HK14*HK20;
Kfusion(6) = HK15*HK20;
Kfusion(7) = HK20*(HK10*P(0,7) - HK11*P(2,7) + HK12*P(3,7) + HK4*P(4,7) + HK7*P(5,7) - HK8*P(6,7) + HK9*P(1,7));
Kfusion(8) = HK20*(HK10*P(0,8) - HK11*P(2,8) + HK12*P(3,8) + HK4*P(4,8) + HK7*P(5,8) - HK8*P(6,8) + HK9*P(1,8));
Kfusion(9) = HK20*(HK10*P(0,9) - HK11*P(2,9) + HK12*P(3,9) + HK4*P(4,9) + HK7*P(5,9) - HK8*P(6,9) + HK9*P(1,9));
Kfusion(10) = HK20*(HK10*P(0,10) - HK11*P(2,10) + HK12*P(3,10) + HK4*P(4,10) + HK7*P(5,10) - HK8*P(6,10) + HK9*P(1,10));
Kfusion(11) = HK20*(HK10*P(0,11) - HK11*P(2,11) + HK12*P(3,11) + HK4*P(4,11) + HK7*P(5,11) - HK8*P(6,11) + HK9*P(1,11));
Kfusion(12) = HK20*(HK10*P(0,12) - HK11*P(2,12) + HK12*P(3,12) + HK4*P(4,12) + HK7*P(5,12) - HK8*P(6,12) + HK9*P(1,12));
Kfusion(13) = HK20*(HK10*P(0,13) - HK11*P(2,13) + HK12*P(3,13) + HK4*P(4,13) + HK7*P(5,13) - HK8*P(6,13) + HK9*P(1,13));
Kfusion(14) = HK20*(HK10*P(0,14) - HK11*P(2,14) + HK12*P(3,14) + HK4*P(4,14) + HK7*P(5,14) - HK8*P(6,14) + HK9*P(1,14));
Kfusion(15) = HK20*(HK10*P(0,15) - HK11*P(2,15) + HK12*P(3,15) + HK4*P(4,15) + HK7*P(5,15) - HK8*P(6,15) + HK9*P(1,15));
Kfusion(16) = HK20*(HK10*P(0,16) - HK11*P(2,16) + HK12*P(3,16) + HK4*P(4,16) + HK7*P(5,16) - HK8*P(6,16) + HK9*P(1,16));
Kfusion(17) = HK20*(HK10*P(0,17) - HK11*P(2,17) + HK12*P(3,17) + HK4*P(4,17) + HK7*P(5,17) - HK8*P(6,17) + HK9*P(1,17));
Kfusion(18) = HK20*(HK10*P(0,18) - HK11*P(2,18) + HK12*P(3,18) + HK4*P(4,18) + HK7*P(5,18) - HK8*P(6,18) + HK9*P(1,18));
Kfusion(19) = HK20*(HK10*P(0,19) - HK11*P(2,19) + HK12*P(3,19) + HK4*P(4,19) + HK7*P(5,19) - HK8*P(6,19) + HK9*P(1,19));
Kfusion(20) = HK20*(HK10*P(0,20) - HK11*P(2,20) + HK12*P(3,20) + HK4*P(4,20) + HK7*P(5,20) - HK8*P(6,20) + HK9*P(1,20));
Kfusion(21) = HK20*(HK10*P(0,21) - HK11*P(2,21) + HK12*P(3,21) + HK4*P(4,21) + HK7*P(5,21) - HK8*P(6,21) + HK9*P(1,21));
Kfusion(22) = HK20*(HK10*P(0,22) - HK11*P(2,22) + HK12*P(3,22) + HK4*P(4,22) + HK7*P(5,22) - HK8*P(6,22) + HK9*P(1,22));
Kfusion(23) = HK20*(HK10*P(0,23) - HK11*P(2,23) + HK12*P(3,23) + HK4*P(4,23) + HK7*P(5,23) - HK8*P(6,23) + HK9*P(1,23));
// axis 1
const float HK0 = q0*ve + q1*vd - q3*vn;
const float HK1 = q0*vd - q1*ve + q2*vn;
const float HK2 = q1*vn + q2*ve + q3*vd;
const float HK3 = q0*vn - q2*vd + q3*ve;
const float HK4 = q0*q3 - q1*q2;
const float HK5 = (q0)*(q0) - (q1)*(q1) + (q2)*(q2) - (q3)*(q3);
const float HK6 = q0*q1 + q2*q3;
const float HK7 = 2*HK6;
const float HK8 = 2*HK4;
const float HK9 = 2*HK2;
const float HK10 = 2*HK0;
const float HK11 = 2*HK1;
const float HK12 = 2*HK3;
const float HK13 = HK10*P(0,0) + HK11*P(0,1) - HK12*P(0,3) + HK5*P(0,5) + HK7*P(0,6) - HK8*P(0,4) + HK9*P(0,2);
const float HK14 = HK10*P(0,6) + HK11*P(1,6) - HK12*P(3,6) + HK5*P(5,6) + HK7*P(6,6) - HK8*P(4,6) + HK9*P(2,6);
const float HK15 = HK10*P(0,4) + HK11*P(1,4) - HK12*P(3,4) + HK5*P(4,5) + HK7*P(4,6) - HK8*P(4,4) + HK9*P(2,4);
const float HK16 = HK10*P(0,2) + HK11*P(1,2) - HK12*P(2,3) + HK5*P(2,5) + HK7*P(2,6) - HK8*P(2,4) + HK9*P(2,2);
const float HK17 = HK10*P(0,1) + HK11*P(1,1) - HK12*P(1,3) + HK5*P(1,5) + HK7*P(1,6) - HK8*P(1,4) + HK9*P(1,2);
const float HK18 = HK10*P(0,3) + HK11*P(1,3) - HK12*P(3,3) + HK5*P(3,5) + HK7*P(3,6) - HK8*P(3,4) + HK9*P(2,3);
const float HK19 = HK10*P(0,5) + HK11*P(1,5) - HK12*P(3,5) + HK5*P(5,5) + HK7*P(5,6) - HK8*P(4,5) + HK9*P(2,5);
const float HK20 = 1.0F/(HK10*HK13 + HK11*HK17 - HK12*HK18 + HK14*HK7 - HK15*HK8 + HK16*HK9 + HK19*HK5 + R_VEL);
H_VEL(0) = 2*HK0;
H_VEL(1) = 2*HK1;
H_VEL(2) = 2*HK2;
H_VEL(3) = -2*HK3;
H_VEL(4) = -2*HK4;
H_VEL(5) = HK5;
H_VEL(6) = 2*HK6;
Kfusion(0) = HK13*HK20;
Kfusion(1) = HK17*HK20;
Kfusion(2) = HK16*HK20;
Kfusion(3) = HK18*HK20;
Kfusion(4) = HK15*HK20;
Kfusion(5) = HK19*HK20;
Kfusion(6) = HK14*HK20;
Kfusion(7) = HK20*(HK10*P(0,7) + HK11*P(1,7) - HK12*P(3,7) + HK5*P(5,7) + HK7*P(6,7) - HK8*P(4,7) + HK9*P(2,7));
Kfusion(8) = HK20*(HK10*P(0,8) + HK11*P(1,8) - HK12*P(3,8) + HK5*P(5,8) + HK7*P(6,8) - HK8*P(4,8) + HK9*P(2,8));
Kfusion(9) = HK20*(HK10*P(0,9) + HK11*P(1,9) - HK12*P(3,9) + HK5*P(5,9) + HK7*P(6,9) - HK8*P(4,9) + HK9*P(2,9));
Kfusion(10) = HK20*(HK10*P(0,10) + HK11*P(1,10) - HK12*P(3,10) + HK5*P(5,10) + HK7*P(6,10) - HK8*P(4,10) + HK9*P(2,10));
Kfusion(11) = HK20*(HK10*P(0,11) + HK11*P(1,11) - HK12*P(3,11) + HK5*P(5,11) + HK7*P(6,11) - HK8*P(4,11) + HK9*P(2,11));
Kfusion(12) = HK20*(HK10*P(0,12) + HK11*P(1,12) - HK12*P(3,12) + HK5*P(5,12) + HK7*P(6,12) - HK8*P(4,12) + HK9*P(2,12));
Kfusion(13) = HK20*(HK10*P(0,13) + HK11*P(1,13) - HK12*P(3,13) + HK5*P(5,13) + HK7*P(6,13) - HK8*P(4,13) + HK9*P(2,13));
Kfusion(14) = HK20*(HK10*P(0,14) + HK11*P(1,14) - HK12*P(3,14) + HK5*P(5,14) + HK7*P(6,14) - HK8*P(4,14) + HK9*P(2,14));
Kfusion(15) = HK20*(HK10*P(0,15) + HK11*P(1,15) - HK12*P(3,15) + HK5*P(5,15) + HK7*P(6,15) - HK8*P(4,15) + HK9*P(2,15));
Kfusion(16) = HK20*(HK10*P(0,16) + HK11*P(1,16) - HK12*P(3,16) + HK5*P(5,16) + HK7*P(6,16) - HK8*P(4,16) + HK9*P(2,16));
Kfusion(17) = HK20*(HK10*P(0,17) + HK11*P(1,17) - HK12*P(3,17) + HK5*P(5,17) + HK7*P(6,17) - HK8*P(4,17) + HK9*P(2,17));
Kfusion(18) = HK20*(HK10*P(0,18) + HK11*P(1,18) - HK12*P(3,18) + HK5*P(5,18) + HK7*P(6,18) - HK8*P(4,18) + HK9*P(2,18));
Kfusion(19) = HK20*(HK10*P(0,19) + HK11*P(1,19) - HK12*P(3,19) + HK5*P(5,19) + HK7*P(6,19) - HK8*P(4,19) + HK9*P(2,19));
Kfusion(20) = HK20*(HK10*P(0,20) + HK11*P(1,20) - HK12*P(3,20) + HK5*P(5,20) + HK7*P(6,20) - HK8*P(4,20) + HK9*P(2,20));
Kfusion(21) = HK20*(HK10*P(0,21) + HK11*P(1,21) - HK12*P(3,21) + HK5*P(5,21) + HK7*P(6,21) - HK8*P(4,21) + HK9*P(2,21));
Kfusion(22) = HK20*(HK10*P(0,22) + HK11*P(1,22) - HK12*P(3,22) + HK5*P(5,22) + HK7*P(6,22) - HK8*P(4,22) + HK9*P(2,22));
Kfusion(23) = HK20*(HK10*P(0,23) + HK11*P(1,23) - HK12*P(3,23) + HK5*P(5,23) + HK7*P(6,23) - HK8*P(4,23) + HK9*P(2,23));
// axis 2
const float HK0 = q0*vd - q1*ve + q2*vn;
const float HK1 = q0*ve + q1*vd - q3*vn;
const float HK2 = q0*vn - q2*vd + q3*ve;
const float HK3 = q1*vn + q2*ve + q3*vd;
const float HK4 = q0*q2 + q1*q3;
const float HK5 = q0*q1 - q2*q3;
const float HK6 = (q0)*(q0) - (q1)*(q1) - (q2)*(q2) + (q3)*(q3);
const float HK7 = 2*HK4;
const float HK8 = 2*HK5;
const float HK9 = 2*HK3;
const float HK10 = 2*HK0;
const float HK11 = 2*HK1;
const float HK12 = 2*HK2;
const float HK13 = HK10*P(0,0) - HK11*P(0,1) + HK12*P(0,2) + HK6*P(0,6) + HK7*P(0,4) - HK8*P(0,5) + HK9*P(0,3);
const float HK14 = HK10*P(0,4) - HK11*P(1,4) + HK12*P(2,4) + HK6*P(4,6) + HK7*P(4,4) - HK8*P(4,5) + HK9*P(3,4);
const float HK15 = HK10*P(0,5) - HK11*P(1,5) + HK12*P(2,5) + HK6*P(5,6) + HK7*P(4,5) - HK8*P(5,5) + HK9*P(3,5);
const float HK16 = HK10*P(0,3) - HK11*P(1,3) + HK12*P(2,3) + HK6*P(3,6) + HK7*P(3,4) - HK8*P(3,5) + HK9*P(3,3);
const float HK17 = HK10*P(0,1) - HK11*P(1,1) + HK12*P(1,2) + HK6*P(1,6) + HK7*P(1,4) - HK8*P(1,5) + HK9*P(1,3);
const float HK18 = HK10*P(0,2) - HK11*P(1,2) + HK12*P(2,2) + HK6*P(2,6) + HK7*P(2,4) - HK8*P(2,5) + HK9*P(2,3);
const float HK19 = HK10*P(0,6) - HK11*P(1,6) + HK12*P(2,6) + HK6*P(6,6) + HK7*P(4,6) - HK8*P(5,6) + HK9*P(3,6);
const float HK20 = 1.0F/(HK10*HK13 - HK11*HK17 + HK12*HK18 + HK14*HK7 - HK15*HK8 + HK16*HK9 + HK19*HK6 + R_VEL);
H_VEL(0) = 2*HK0;
H_VEL(1) = -2*HK1;
H_VEL(2) = 2*HK2;
H_VEL(3) = 2*HK3;
H_VEL(4) = 2*HK4;
H_VEL(5) = -2*HK5;
H_VEL(6) = HK6;
Kfusion(0) = HK13*HK20;
Kfusion(1) = HK17*HK20;
Kfusion(2) = HK18*HK20;
Kfusion(3) = HK16*HK20;
Kfusion(4) = HK14*HK20;
Kfusion(5) = HK15*HK20;
Kfusion(6) = HK19*HK20;
Kfusion(7) = HK20*(HK10*P(0,7) - HK11*P(1,7) + HK12*P(2,7) + HK6*P(6,7) + HK7*P(4,7) - HK8*P(5,7) + HK9*P(3,7));
Kfusion(8) = HK20*(HK10*P(0,8) - HK11*P(1,8) + HK12*P(2,8) + HK6*P(6,8) + HK7*P(4,8) - HK8*P(5,8) + HK9*P(3,8));
Kfusion(9) = HK20*(HK10*P(0,9) - HK11*P(1,9) + HK12*P(2,9) + HK6*P(6,9) + HK7*P(4,9) - HK8*P(5,9) + HK9*P(3,9));
Kfusion(10) = HK20*(HK10*P(0,10) - HK11*P(1,10) + HK12*P(2,10) + HK6*P(6,10) + HK7*P(4,10) - HK8*P(5,10) + HK9*P(3,10));
Kfusion(11) = HK20*(HK10*P(0,11) - HK11*P(1,11) + HK12*P(2,11) + HK6*P(6,11) + HK7*P(4,11) - HK8*P(5,11) + HK9*P(3,11));
Kfusion(12) = HK20*(HK10*P(0,12) - HK11*P(1,12) + HK12*P(2,12) + HK6*P(6,12) + HK7*P(4,12) - HK8*P(5,12) + HK9*P(3,12));
Kfusion(13) = HK20*(HK10*P(0,13) - HK11*P(1,13) + HK12*P(2,13) + HK6*P(6,13) + HK7*P(4,13) - HK8*P(5,13) + HK9*P(3,13));
Kfusion(14) = HK20*(HK10*P(0,14) - HK11*P(1,14) + HK12*P(2,14) + HK6*P(6,14) + HK7*P(4,14) - HK8*P(5,14) + HK9*P(3,14));
Kfusion(15) = HK20*(HK10*P(0,15) - HK11*P(1,15) + HK12*P(2,15) + HK6*P(6,15) + HK7*P(4,15) - HK8*P(5,15) + HK9*P(3,15));
Kfusion(16) = HK20*(HK10*P(0,16) - HK11*P(1,16) + HK12*P(2,16) + HK6*P(6,16) + HK7*P(4,16) - HK8*P(5,16) + HK9*P(3,16));
Kfusion(17) = HK20*(HK10*P(0,17) - HK11*P(1,17) + HK12*P(2,17) + HK6*P(6,17) + HK7*P(4,17) - HK8*P(5,17) + HK9*P(3,17));
Kfusion(18) = HK20*(HK10*P(0,18) - HK11*P(1,18) + HK12*P(2,18) + HK6*P(6,18) + HK7*P(4,18) - HK8*P(5,18) + HK9*P(3,18));
Kfusion(19) = HK20*(HK10*P(0,19) - HK11*P(1,19) + HK12*P(2,19) + HK6*P(6,19) + HK7*P(4,19) - HK8*P(5,19) + HK9*P(3,19));
Kfusion(20) = HK20*(HK10*P(0,20) - HK11*P(1,20) + HK12*P(2,20) + HK6*P(6,20) + HK7*P(4,20) - HK8*P(5,20) + HK9*P(3,20));
Kfusion(21) = HK20*(HK10*P(0,21) - HK11*P(1,21) + HK12*P(2,21) + HK6*P(6,21) + HK7*P(4,21) - HK8*P(5,21) + HK9*P(3,21));
Kfusion(22) = HK20*(HK10*P(0,22) - HK11*P(1,22) + HK12*P(2,22) + HK6*P(6,22) + HK7*P(4,22) - HK8*P(5,22) + HK9*P(3,22));
Kfusion(23) = HK20*(HK10*P(0,23) - HK11*P(1,23) + HK12*P(2,23) + HK6*P(6,23) + HK7*P(4,23) - HK8*P(5,23) + HK9*P(3,23));
src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp
-187
View File
@@ -1,187 +0,0 @@
// Sub Expressions
const float HK0 = q0*vn - q2*vd + q3*ve;
const float HK1 = 2*HK0;
const float HK2 = q1*vn + q2*ve + q3*vd;
const float HK3 = 2*HK2;
const float HK4 = q0*vd - q1*ve + q2*vn;
const float HK5 = q0*ve + q1*vd - q3*vn;
const float HK6 = 2*HK5;
const float HK7 = (q1)*(q1);
const float HK8 = (q2)*(q2);
const float HK9 = -HK8;
const float HK10 = (q0)*(q0);
const float HK11 = (q3)*(q3);
const float HK12 = HK10 - HK11;
const float HK13 = HK12 + HK7 + HK9;
const float HK14 = q0*q3;
const float HK15 = HK14 + q1*q2;
const float HK16 = q0*q2;
const float HK17 = HK16 - q1*q3;
const float HK18 = 2*HK15;
const float HK19 = 2*HK17;
const float HK20 = 2*HK2;
const float HK21 = 2*HK0;
const float HK22 = 2*HK4;
const float HK23 = HK22*P(0,2);
const float HK24 = 2*HK5;
const float HK25 = HK24*P(0,3);
const float HK26 = HK13*P(0,4) + HK18*P(0,5) - HK19*P(0,6) + HK20*P(0,1) + HK21*P(0,0) - HK23 + HK25;
const float HK27 = HK13*P(4,5) + HK18*P(5,5) - HK19*P(5,6) + HK20*P(1,5) + HK21*P(0,5) - HK22*P(2,5) + HK24*P(3,5);
const float HK28 = HK13*P(4,6) + HK18*P(5,6) - HK19*P(6,6) + HK20*P(1,6) + HK21*P(0,6) - HK22*P(2,6) + HK24*P(3,6);
const float HK29 = HK22*P(1,2);
const float HK30 = HK24*P(1,3);
const float HK31 = HK13*P(1,4) + HK18*P(1,5) - HK19*P(1,6) + HK20*P(1,1) + HK21*P(0,1) - HK29 + HK30;
const float HK32 = HK20*P(1,2);
const float HK33 = HK21*P(0,2);
const float HK34 = HK13*P(2,4) + HK18*P(2,5) - HK19*P(2,6) - HK22*P(2,2) + HK24*P(2,3) + HK32 + HK33;
const float HK35 = HK20*P(1,3);
const float HK36 = HK21*P(0,3);
const float HK37 = HK13*P(3,4) + HK18*P(3,5) - HK19*P(3,6) - HK22*P(2,3) + HK24*P(3,3) + HK35 + HK36;
const float HK38 = HK13*P(4,4) + HK18*P(4,5) - HK19*P(4,6) + HK20*P(1,4) + HK21*P(0,4) - HK22*P(2,4) + HK24*P(3,4);
const float HK39 = 1.0F/(HK13*HK38 + HK18*HK27 - HK19*HK28 + HK20*HK31 + HK21*HK26 - HK22*HK34 + HK24*HK37 + R_VEL);
const float HK40 = 2*HK4;
const float HK41 = HK14 - q1*q2;
const float HK42 = -HK7;
const float HK43 = HK12 + HK42 + HK8;
const float HK44 = q0*q1;
const float HK45 = HK44 + q2*q3;
const float HK46 = 2*HK45;
const float HK47 = 2*HK41;
const float HK48 = HK22*P(0,1);
const float HK49 = HK20*P(0,2) + HK24*P(0,0) - HK36 + HK43*P(0,5) + HK46*P(0,6) - HK47*P(0,4) + HK48;
const float HK50 = HK20*P(2,6) - HK21*P(3,6) + HK22*P(1,6) + HK24*P(0,6) + HK43*P(5,6) + HK46*P(6,6) - HK47*P(4,6);
const float HK51 = HK20*P(2,4) - HK21*P(3,4) + HK22*P(1,4) + HK24*P(0,4) + HK43*P(4,5) + HK46*P(4,6) - HK47*P(4,4);
const float HK52 = HK21*P(2,3);
const float HK53 = HK20*P(2,2) + HK24*P(0,2) + HK29 + HK43*P(2,5) + HK46*P(2,6) - HK47*P(2,4) - HK52;
const float HK54 = HK24*P(0,1);
const float HK55 = -HK21*P(1,3) + HK22*P(1,1) + HK32 + HK43*P(1,5) + HK46*P(1,6) - HK47*P(1,4) + HK54;
const float HK56 = HK20*P(2,3);
const float HK57 = -HK21*P(3,3) + HK22*P(1,3) + HK25 + HK43*P(3,5) + HK46*P(3,6) - HK47*P(3,4) + HK56;
const float HK58 = HK20*P(2,5) - HK21*P(3,5) + HK22*P(1,5) + HK24*P(0,5) + HK43*P(5,5) + HK46*P(5,6) - HK47*P(4,5);
const float HK59 = 1.0F/(HK20*HK53 - HK21*HK57 + HK22*HK55 + HK24*HK49 + HK43*HK58 + HK46*HK50 - HK47*HK51 + R_VEL);
const float HK60 = HK16 + q1*q3;
const float HK61 = HK44 - q2*q3;
const float HK62 = HK10 + HK11 + HK42 + HK9;
const float HK63 = 2*HK60;
const float HK64 = 2*HK61;
const float HK65 = HK20*P(0,3) + HK22*P(0,0) + HK33 - HK54 + HK62*P(0,6) + HK63*P(0,4) - HK64*P(0,5);
const float HK66 = HK20*P(3,4) + HK21*P(2,4) + HK22*P(0,4) - HK24*P(1,4) + HK62*P(4,6) + HK63*P(4,4) - HK64*P(4,5);
const float HK67 = HK20*P(3,5) + HK21*P(2,5) + HK22*P(0,5) - HK24*P(1,5) + HK62*P(5,6) + HK63*P(4,5) - HK64*P(5,5);
const float HK68 = HK20*P(3,3) + HK22*P(0,3) - HK30 + HK52 + HK62*P(3,6) + HK63*P(3,4) - HK64*P(3,5);
const float HK69 = HK21*P(1,2) - HK24*P(1,1) + HK35 + HK48 + HK62*P(1,6) + HK63*P(1,4) - HK64*P(1,5);
const float HK70 = HK21*P(2,2) + HK23 - HK24*P(1,2) + HK56 + HK62*P(2,6) + HK63*P(2,4) - HK64*P(2,5);
const float HK71 = HK20*P(3,6) + HK21*P(2,6) + HK22*P(0,6) - HK24*P(1,6) + HK62*P(6,6) + HK63*P(4,6) - HK64*P(5,6);
const float HK72 = 1.0F/(HK20*HK68 + HK21*HK70 + HK22*HK65 - HK24*HK69 + HK62*HK71 + HK63*HK66 - HK64*HK67 + R_VEL);
// Observation Jacobians - axis 0
Hfusion.at<0>() = HK1;
Hfusion.at<1>() = HK3;
Hfusion.at<2>() = -2*HK4;
Hfusion.at<3>() = HK6;
Hfusion.at<4>() = HK13;
Hfusion.at<5>() = 2*HK15;
Hfusion.at<6>() = -2*HK17;
// Kalman gains - axis 0
Kfusion(0) = HK26*HK39;
Kfusion(1) = HK31*HK39;
Kfusion(2) = HK34*HK39;
Kfusion(3) = HK37*HK39;
Kfusion(4) = HK38*HK39;
Kfusion(5) = HK27*HK39;
Kfusion(6) = HK28*HK39;
Kfusion(7) = HK39*(HK13*P(4,7) + HK18*P(5,7) - HK19*P(6,7) + HK20*P(1,7) + HK21*P(0,7) - HK22*P(2,7) + HK24*P(3,7));
Kfusion(8) = HK39*(HK13*P(4,8) + HK18*P(5,8) - HK19*P(6,8) + HK20*P(1,8) + HK21*P(0,8) - HK22*P(2,8) + HK24*P(3,8));
Kfusion(9) = HK39*(HK13*P(4,9) + HK18*P(5,9) - HK19*P(6,9) + HK20*P(1,9) + HK21*P(0,9) - HK22*P(2,9) + HK24*P(3,9));
Kfusion(10) = HK39*(HK13*P(4,10) + HK18*P(5,10) - HK19*P(6,10) + HK20*P(1,10) + HK21*P(0,10) - HK22*P(2,10) + HK24*P(3,10));
Kfusion(11) = HK39*(HK13*P(4,11) + HK18*P(5,11) - HK19*P(6,11) + HK20*P(1,11) + HK21*P(0,11) - HK22*P(2,11) + HK24*P(3,11));
Kfusion(12) = HK39*(HK13*P(4,12) + HK18*P(5,12) - HK19*P(6,12) + HK20*P(1,12) + HK21*P(0,12) - HK22*P(2,12) + HK24*P(3,12));
Kfusion(13) = HK39*(HK13*P(4,13) + HK18*P(5,13) - HK19*P(6,13) + HK20*P(1,13) + HK21*P(0,13) - HK22*P(2,13) + HK24*P(3,13));
Kfusion(14) = HK39*(HK13*P(4,14) + HK18*P(5,14) - HK19*P(6,14) + HK20*P(1,14) + HK21*P(0,14) - HK22*P(2,14) + HK24*P(3,14));
Kfusion(15) = HK39*(HK13*P(4,15) + HK18*P(5,15) - HK19*P(6,15) + HK20*P(1,15) + HK21*P(0,15) - HK22*P(2,15) + HK24*P(3,15));
Kfusion(16) = HK39*(HK13*P(4,16) + HK18*P(5,16) - HK19*P(6,16) + HK20*P(1,16) + HK21*P(0,16) - HK22*P(2,16) + HK24*P(3,16));
Kfusion(17) = HK39*(HK13*P(4,17) + HK18*P(5,17) - HK19*P(6,17) + HK20*P(1,17) + HK21*P(0,17) - HK22*P(2,17) + HK24*P(3,17));
Kfusion(18) = HK39*(HK13*P(4,18) + HK18*P(5,18) - HK19*P(6,18) + HK20*P(1,18) + HK21*P(0,18) - HK22*P(2,18) + HK24*P(3,18));
Kfusion(19) = HK39*(HK13*P(4,19) + HK18*P(5,19) - HK19*P(6,19) + HK20*P(1,19) + HK21*P(0,19) - HK22*P(2,19) + HK24*P(3,19));
Kfusion(20) = HK39*(HK13*P(4,20) + HK18*P(5,20) - HK19*P(6,20) + HK20*P(1,20) + HK21*P(0,20) - HK22*P(2,20) + HK24*P(3,20));
Kfusion(21) = HK39*(HK13*P(4,21) + HK18*P(5,21) - HK19*P(6,21) + HK20*P(1,21) + HK21*P(0,21) - HK22*P(2,21) + HK24*P(3,21));
Kfusion(22) = HK39*(HK13*P(4,22) + HK18*P(5,22) - HK19*P(6,22) + HK20*P(1,22) + HK21*P(0,22) - HK22*P(2,22) + HK24*P(3,22));
Kfusion(23) = HK39*(HK13*P(4,23) + HK18*P(5,23) - HK19*P(6,23) + HK20*P(1,23) + HK21*P(0,23) - HK22*P(2,23) + HK24*P(3,23));
// Observation Jacobians - axis 1
Hfusion.at<0>() = HK6;
Hfusion.at<1>() = HK40;
Hfusion.at<2>() = HK3;
Hfusion.at<3>() = -2*HK0;
Hfusion.at<4>() = -2*HK41;
Hfusion.at<5>() = HK43;
Hfusion.at<6>() = 2*HK45;
// Kalman gains - axis 1
Kfusion(0) = HK49*HK59;
Kfusion(1) = HK55*HK59;
Kfusion(2) = HK53*HK59;
Kfusion(3) = HK57*HK59;
Kfusion(4) = HK51*HK59;
Kfusion(5) = HK58*HK59;
Kfusion(6) = HK50*HK59;
Kfusion(7) = HK59*(HK20*P(2,7) - HK21*P(3,7) + HK22*P(1,7) + HK24*P(0,7) + HK43*P(5,7) + HK46*P(6,7) - HK47*P(4,7));
Kfusion(8) = HK59*(HK20*P(2,8) - HK21*P(3,8) + HK22*P(1,8) + HK24*P(0,8) + HK43*P(5,8) + HK46*P(6,8) - HK47*P(4,8));
Kfusion(9) = HK59*(HK20*P(2,9) - HK21*P(3,9) + HK22*P(1,9) + HK24*P(0,9) + HK43*P(5,9) + HK46*P(6,9) - HK47*P(4,9));
Kfusion(10) = HK59*(HK20*P(2,10) - HK21*P(3,10) + HK22*P(1,10) + HK24*P(0,10) + HK43*P(5,10) + HK46*P(6,10) - HK47*P(4,10));
Kfusion(11) = HK59*(HK20*P(2,11) - HK21*P(3,11) + HK22*P(1,11) + HK24*P(0,11) + HK43*P(5,11) + HK46*P(6,11) - HK47*P(4,11));
Kfusion(12) = HK59*(HK20*P(2,12) - HK21*P(3,12) + HK22*P(1,12) + HK24*P(0,12) + HK43*P(5,12) + HK46*P(6,12) - HK47*P(4,12));
Kfusion(13) = HK59*(HK20*P(2,13) - HK21*P(3,13) + HK22*P(1,13) + HK24*P(0,13) + HK43*P(5,13) + HK46*P(6,13) - HK47*P(4,13));
Kfusion(14) = HK59*(HK20*P(2,14) - HK21*P(3,14) + HK22*P(1,14) + HK24*P(0,14) + HK43*P(5,14) + HK46*P(6,14) - HK47*P(4,14));
Kfusion(15) = HK59*(HK20*P(2,15) - HK21*P(3,15) + HK22*P(1,15) + HK24*P(0,15) + HK43*P(5,15) + HK46*P(6,15) - HK47*P(4,15));
Kfusion(16) = HK59*(HK20*P(2,16) - HK21*P(3,16) + HK22*P(1,16) + HK24*P(0,16) + HK43*P(5,16) + HK46*P(6,16) - HK47*P(4,16));
Kfusion(17) = HK59*(HK20*P(2,17) - HK21*P(3,17) + HK22*P(1,17) + HK24*P(0,17) + HK43*P(5,17) + HK46*P(6,17) - HK47*P(4,17));
Kfusion(18) = HK59*(HK20*P(2,18) - HK21*P(3,18) + HK22*P(1,18) + HK24*P(0,18) + HK43*P(5,18) + HK46*P(6,18) - HK47*P(4,18));
Kfusion(19) = HK59*(HK20*P(2,19) - HK21*P(3,19) + HK22*P(1,19) + HK24*P(0,19) + HK43*P(5,19) + HK46*P(6,19) - HK47*P(4,19));
Kfusion(20) = HK59*(HK20*P(2,20) - HK21*P(3,20) + HK22*P(1,20) + HK24*P(0,20) + HK43*P(5,20) + HK46*P(6,20) - HK47*P(4,20));
Kfusion(21) = HK59*(HK20*P(2,21) - HK21*P(3,21) + HK22*P(1,21) + HK24*P(0,21) + HK43*P(5,21) + HK46*P(6,21) - HK47*P(4,21));
Kfusion(22) = HK59*(HK20*P(2,22) - HK21*P(3,22) + HK22*P(1,22) + HK24*P(0,22) + HK43*P(5,22) + HK46*P(6,22) - HK47*P(4,22));
Kfusion(23) = HK59*(HK20*P(2,23) - HK21*P(3,23) + HK22*P(1,23) + HK24*P(0,23) + HK43*P(5,23) + HK46*P(6,23) - HK47*P(4,23));
// Observation Jacobians - axis 2
Hfusion.at<0>() = HK40;
Hfusion.at<1>() = -2*HK5;
Hfusion.at<2>() = HK1;
Hfusion.at<3>() = HK3;
Hfusion.at<4>() = 2*HK60;
Hfusion.at<5>() = -2*HK61;
Hfusion.at<6>() = HK62;
// Kalman gains - axis 2
Kfusion(0) = HK65*HK72;
Kfusion(1) = HK69*HK72;
Kfusion(2) = HK70*HK72;
Kfusion(3) = HK68*HK72;
Kfusion(4) = HK66*HK72;
Kfusion(5) = HK67*HK72;
Kfusion(6) = HK71*HK72;
Kfusion(7) = HK72*(HK20*P(3,7) + HK21*P(2,7) + HK22*P(0,7) - HK24*P(1,7) + HK62*P(6,7) + HK63*P(4,7) - HK64*P(5,7));
Kfusion(8) = HK72*(HK20*P(3,8) + HK21*P(2,8) + HK22*P(0,8) - HK24*P(1,8) + HK62*P(6,8) + HK63*P(4,8) - HK64*P(5,8));
Kfusion(9) = HK72*(HK20*P(3,9) + HK21*P(2,9) + HK22*P(0,9) - HK24*P(1,9) + HK62*P(6,9) + HK63*P(4,9) - HK64*P(5,9));
Kfusion(10) = HK72*(HK20*P(3,10) + HK21*P(2,10) + HK22*P(0,10) - HK24*P(1,10) + HK62*P(6,10) + HK63*P(4,10) - HK64*P(5,10));
Kfusion(11) = HK72*(HK20*P(3,11) + HK21*P(2,11) + HK22*P(0,11) - HK24*P(1,11) + HK62*P(6,11) + HK63*P(4,11) - HK64*P(5,11));
Kfusion(12) = HK72*(HK20*P(3,12) + HK21*P(2,12) + HK22*P(0,12) - HK24*P(1,12) + HK62*P(6,12) + HK63*P(4,12) - HK64*P(5,12));
Kfusion(13) = HK72*(HK20*P(3,13) + HK21*P(2,13) + HK22*P(0,13) - HK24*P(1,13) + HK62*P(6,13) + HK63*P(4,13) - HK64*P(5,13));
Kfusion(14) = HK72*(HK20*P(3,14) + HK21*P(2,14) + HK22*P(0,14) - HK24*P(1,14) + HK62*P(6,14) + HK63*P(4,14) - HK64*P(5,14));
Kfusion(15) = HK72*(HK20*P(3,15) + HK21*P(2,15) + HK22*P(0,15) - HK24*P(1,15) + HK62*P(6,15) + HK63*P(4,15) - HK64*P(5,15));
Kfusion(16) = HK72*(HK20*P(3,16) + HK21*P(2,16) + HK22*P(0,16) - HK24*P(1,16) + HK62*P(6,16) + HK63*P(4,16) - HK64*P(5,16));
Kfusion(17) = HK72*(HK20*P(3,17) + HK21*P(2,17) + HK22*P(0,17) - HK24*P(1,17) + HK62*P(6,17) + HK63*P(4,17) - HK64*P(5,17));
Kfusion(18) = HK72*(HK20*P(3,18) + HK21*P(2,18) + HK22*P(0,18) - HK24*P(1,18) + HK62*P(6,18) + HK63*P(4,18) - HK64*P(5,18));
Kfusion(19) = HK72*(HK20*P(3,19) + HK21*P(2,19) + HK22*P(0,19) - HK24*P(1,19) + HK62*P(6,19) + HK63*P(4,19) - HK64*P(5,19));
Kfusion(20) = HK72*(HK20*P(3,20) + HK21*P(2,20) + HK22*P(0,20) - HK24*P(1,20) + HK62*P(6,20) + HK63*P(4,20) - HK64*P(5,20));
Kfusion(21) = HK72*(HK20*P(3,21) + HK21*P(2,21) + HK22*P(0,21) - HK24*P(1,21) + HK62*P(6,21) + HK63*P(4,21) - HK64*P(5,21));
Kfusion(22) = HK72*(HK20*P(3,22) + HK21*P(2,22) + HK22*P(0,22) - HK24*P(1,22) + HK62*P(6,22) + HK63*P(4,22) - HK64*P(5,22));
Kfusion(23) = HK72*(HK20*P(3,23) + HK21*P(2,23) + HK22*P(0,23) - HK24*P(1,23) + HK62*P(6,23) + HK63*P(4,23) - HK64*P(5,23));
src/modules/ekf2/EKF/python/ekf_derivation/main.py
-690
View File
File diff suppressed because it is too large Load Diff