mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-28 14:40:34 +08:00
ekf2: move EKF out of ecl
This commit is contained in:
Daniel Agar
@@ -0,0 +1 @@
|
||||
__pycache__
|
||||
@@ -0,0 +1,8 @@
|
||||
#!/usr/bin/env python3
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
Created on Sat Mar 14 13:02:26 2020
|
||||
|
||||
@author: roman
|
||||
"""
|
||||
|
||||
@@ -0,0 +1,49 @@
|
||||
#!/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):
|
||||
return ccode(expression, type_aliases={real:float32})
|
||||
|
||||
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:
|
||||
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)):
|
||||
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:
|
||||
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()
|
||||
+604
@@ -0,0 +1,604 @@
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <cstdlib>
|
||||
#include "../../../../../matrix/matrix/math.hpp"
|
||||
|
||||
typedef matrix::Vector<float, 24> Vector24f;
|
||||
typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
|
||||
|
||||
float sq(float in) {
|
||||
return in * in;
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
// Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations
|
||||
|
||||
float Hfusion[24];
|
||||
Vector24f H_MAG;
|
||||
Vector24f Kfusion;
|
||||
float mag_innov_var;
|
||||
|
||||
// quaternion inputs must be normalised
|
||||
float q0 = 2.0f * ((float)rand() - 0.5f);
|
||||
float q1 = 2.0f * ((float)rand() - 0.5f);
|
||||
float q2 = 2.0f * ((float)rand() - 0.5f);
|
||||
float q3 = 2.0f * ((float)rand() - 0.5f);
|
||||
const float length = sqrtf(sq(q0) + sq(q1) + sq(q2) + sq(q3));
|
||||
q0 /= length;
|
||||
q1 /= length;
|
||||
q2 /= length;
|
||||
q3 /= length;
|
||||
|
||||
const float magN = 2.0f * ((float)rand() - 0.5f);
|
||||
const float magE = 2.0f * ((float)rand() - 0.5f);
|
||||
const float magD = 2.0f * ((float)rand() - 0.5f);
|
||||
|
||||
const float R_MAG = sq(0.05f);
|
||||
const bool update_all_states = true;
|
||||
|
||||
// create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
|
||||
SquareMatrix24f P;
|
||||
for (int col=0; col<=23; col++) {
|
||||
for (int row=0; row<=col; row++) {
|
||||
if (row == col) {
|
||||
P(row,col) = (float)rand();
|
||||
} else {
|
||||
P(col,row) = P(row,col) = 2.0f * ((float)rand() - 0.5f);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// common expressions used by sympy generated equations
|
||||
// calculate intermediate variables used for X axis innovation variance, observation Jacobians and Kalman gainss
|
||||
const float HKX0 = -magD*q2 + magE*q3 + magN*q0;
|
||||
const float HKX1 = magD*q3 + magE*q2 + magN*q1;
|
||||
const float HKX2 = magE*q1;
|
||||
const float HKX3 = magD*q0;
|
||||
const float HKX4 = magN*q2;
|
||||
const float HKX5 = magD*q1 + magE*q0 - magN*q3;
|
||||
const float HKX6 = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2);
|
||||
const float HKX7 = q0*q3 + q1*q2;
|
||||
const float HKX8 = q1*q3;
|
||||
const float HKX9 = q0*q2;
|
||||
const float HKX10 = 2*HKX7;
|
||||
const float HKX11 = -2*HKX8 + 2*HKX9;
|
||||
const float HKX12 = 2*HKX1;
|
||||
const float HKX13 = 2*HKX0;
|
||||
const float HKX14 = -2*HKX2 + 2*HKX3 + 2*HKX4;
|
||||
const float HKX15 = 2*HKX5;
|
||||
const float HKX16 = HKX10*P(0,17) - HKX11*P(0,18) + HKX12*P(0,1) + HKX13*P(0,0) - HKX14*P(0,2) + HKX15*P(0,3) + HKX6*P(0,16) + P(0,19);
|
||||
const float HKX17 = HKX10*P(16,17) - HKX11*P(16,18) + HKX12*P(1,16) + HKX13*P(0,16) - HKX14*P(2,16) + HKX15*P(3,16) + HKX6*P(16,16) + P(16,19);
|
||||
const float HKX18 = HKX10*P(17,18) - HKX11*P(18,18) + HKX12*P(1,18) + HKX13*P(0,18) - HKX14*P(2,18) + HKX15*P(3,18) + HKX6*P(16,18) + P(18,19);
|
||||
const float HKX19 = HKX10*P(2,17) - HKX11*P(2,18) + HKX12*P(1,2) + HKX13*P(0,2) - HKX14*P(2,2) + HKX15*P(2,3) + HKX6*P(2,16) + P(2,19);
|
||||
const float HKX20 = HKX10*P(17,17) - HKX11*P(17,18) + HKX12*P(1,17) + HKX13*P(0,17) - HKX14*P(2,17) + HKX15*P(3,17) + HKX6*P(16,17) + P(17,19);
|
||||
const float HKX21 = HKX10*P(3,17) - HKX11*P(3,18) + HKX12*P(1,3) + HKX13*P(0,3) - HKX14*P(2,3) + HKX15*P(3,3) + HKX6*P(3,16) + P(3,19);
|
||||
const float HKX22 = HKX10*P(1,17) - HKX11*P(1,18) + HKX12*P(1,1) + HKX13*P(0,1) - HKX14*P(1,2) + HKX15*P(1,3) + HKX6*P(1,16) + P(1,19);
|
||||
const float HKX23 = HKX10*P(17,19) - HKX11*P(18,19) + HKX12*P(1,19) + HKX13*P(0,19) - HKX14*P(2,19) + HKX15*P(3,19) + HKX6*P(16,19) + P(19,19);
|
||||
const float HKX24 = 1.0F/(HKX10*HKX20 - HKX11*HKX18 + HKX12*HKX22 + HKX13*HKX16 - HKX14*HKX19 + HKX15*HKX21 + HKX17*HKX6 + HKX23 + R_MAG);
|
||||
|
||||
// common expressions used by matlab generated equations
|
||||
float SH_MAG[9];
|
||||
SH_MAG[0] = 2.0f*magD*q3 + 2.0f*magE*q2 + 2.0f*magN*q1;
|
||||
SH_MAG[1] = 2.0f*magD*q0 - 2.0f*magE*q1 + 2.0f*magN*q2;
|
||||
SH_MAG[2] = 2.0f*magD*q1 + 2.0f*magE*q0 - 2.0f*magN*q3;
|
||||
SH_MAG[3] = sq(q3);
|
||||
SH_MAG[4] = sq(q2);
|
||||
SH_MAG[5] = sq(q1);
|
||||
SH_MAG[6] = sq(q0);
|
||||
SH_MAG[7] = 2.0f*magN*q0;
|
||||
SH_MAG[8] = 2.0f*magE*q3;
|
||||
|
||||
// Compare X axis equations
|
||||
{
|
||||
mag_innov_var = (HKX10*HKX20 - HKX11*HKX18 + HKX12*HKX22 + HKX13*HKX16 - HKX14*HKX19 + HKX15*HKX21 + HKX17*HKX6 + HKX23 + R_MAG);
|
||||
float HK50 = 1.0F/mag_innov_var;
|
||||
|
||||
// Calculate X axis observation jacobians
|
||||
memset(Hfusion, 0, sizeof(Hfusion));
|
||||
Hfusion[0] = 2*HKX0;
|
||||
Hfusion[1] = 2*HKX1;
|
||||
Hfusion[2] = 2*HKX2 - 2*HKX3 - 2*HKX4;
|
||||
Hfusion[3] = 2*HKX5;
|
||||
Hfusion[16] = HKX6;
|
||||
Hfusion[17] = 2*HKX7;
|
||||
Hfusion[18] = 2*HKX8 - 2*HKX9;
|
||||
Hfusion[19] = 1;
|
||||
|
||||
// Calculate X axis Kalman gains
|
||||
if (update_all_states) {
|
||||
Kfusion(0) = HKX16*HKX24;
|
||||
Kfusion(1) = HKX22*HKX24;
|
||||
Kfusion(2) = HKX19*HKX24;
|
||||
Kfusion(3) = HKX21*HKX24;
|
||||
|
||||
for (unsigned row = 4; row <= 15; row++) {
|
||||
Kfusion(row) = HKX24*(HKX10*P(row,17) - HKX11*P(row,18) + HKX12*P(1,row) + HKX13*P(0,row) - HKX14*P(2,row) + HKX15*P(3,row) + HKX6*P(row,16) + P(row,19));
|
||||
}
|
||||
|
||||
for (unsigned row = 22; row <= 23; row++) {
|
||||
Kfusion(row) = HKX24*(HKX10*P(17,row) - HKX11*P(18,row) + HKX12*P(1,row) + HKX13*P(0,row) - HKX14*P(2,row) + HKX15*P(3,row) + HKX6*P(16,row) + P(19,row));
|
||||
}
|
||||
}
|
||||
|
||||
Kfusion(16) = HKX17*HKX24;
|
||||
Kfusion(17) = HKX20*HKX24;
|
||||
Kfusion(18) = HKX18*HKX24;
|
||||
Kfusion(19) = HKX23*HKX24;
|
||||
|
||||
for (unsigned row = 20; row <= 21; row++) {
|
||||
Kfusion(row) = HKX24*(HKX10*P(17,row) - HKX11*P(18,row) + HKX12*P(1,row) + HKX13*P(0,row) - HKX14*P(2,row) + HKX15*P(3,row) + HKX6*P(16,row) + P(19,row));
|
||||
}
|
||||
|
||||
// save output and repeat calculation using legacy matlab generated code
|
||||
float Hfusion_sympy[24];
|
||||
Vector24f Kfusion_sympy;
|
||||
for (int row=0; row<24; row++) {
|
||||
Hfusion_sympy[row] = Hfusion[row];
|
||||
Kfusion_sympy(row) = Kfusion(row);
|
||||
}
|
||||
|
||||
// repeat calculation using matlab generated equations
|
||||
// X axis innovation variance
|
||||
mag_innov_var = (P(19,19) + R_MAG + P(1,19)*SH_MAG[0] - P(2,19)*SH_MAG[1] + P(3,19)*SH_MAG[2] - P(16,19)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + (2.0f*q0*q3 + 2.0f*q1*q2)*(P(19,17) + P(1,17)*SH_MAG[0] - P(2,17)*SH_MAG[1] + P(3,17)*SH_MAG[2] - P(16,17)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,17)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,17)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,17)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (2.0f*q0*q2 - 2.0f*q1*q3)*(P(19,18) + P(1,18)*SH_MAG[0] - P(2,18)*SH_MAG[1] + P(3,18)*SH_MAG[2] - P(16,18)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,18)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,18)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,18)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P(19,0) + P(1,0)*SH_MAG[0] - P(2,0)*SH_MAG[1] + P(3,0)*SH_MAG[2] - P(16,0)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,0)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,0)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,0)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(17,19)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,19)*(2.0f*q0*q2 - 2.0f*q1*q3) + SH_MAG[0]*(P(19,1) + P(1,1)*SH_MAG[0] - P(2,1)*SH_MAG[1] + P(3,1)*SH_MAG[2] - P(16,1)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,1)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,1)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,1)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - SH_MAG[1]*(P(19,2) + P(1,2)*SH_MAG[0] - P(2,2)*SH_MAG[1] + P(3,2)*SH_MAG[2] - P(16,2)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,2)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,2)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,2)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[2]*(P(19,3) + P(1,3)*SH_MAG[0] - P(2,3)*SH_MAG[1] + P(3,3)*SH_MAG[2] - P(16,3)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,3)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,3)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,3)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P(19,16) + P(1,16)*SH_MAG[0] - P(2,16)*SH_MAG[1] + P(3,16)*SH_MAG[2] - P(16,16)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,16)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,16)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,16)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(0,19)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2));
|
||||
|
||||
// Calculate X axis observation jacobians
|
||||
H_MAG.setZero();
|
||||
H_MAG(0) = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
|
||||
H_MAG(1) = SH_MAG[0];
|
||||
H_MAG(2) = -SH_MAG[1];
|
||||
H_MAG(3) = SH_MAG[2];
|
||||
H_MAG(16) = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6];
|
||||
H_MAG(17) = 2.0f*q0*q3 + 2.0f*q1*q2;
|
||||
H_MAG(18) = 2.0f*q1*q3 - 2.0f*q0*q2;
|
||||
H_MAG(19) = 1.0f;
|
||||
|
||||
// Calculate X axis Kalman gains
|
||||
float SK_MX[5];
|
||||
SK_MX[0] = 1.0f / mag_innov_var;
|
||||
SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6];
|
||||
SK_MX[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
|
||||
SK_MX[3] = 2.0f*q0*q2 - 2.0f*q1*q3;
|
||||
SK_MX[4] = 2.0f*q0*q3 + 2.0f*q1*q2;
|
||||
|
||||
if (update_all_states) {
|
||||
Kfusion(0) = SK_MX[0]*(P(0,19) + P(0,1)*SH_MAG[0] - P(0,2)*SH_MAG[1] + P(0,3)*SH_MAG[2] + P(0,0)*SK_MX[2] - P(0,16)*SK_MX[1] + P(0,17)*SK_MX[4] - P(0,18)*SK_MX[3]);
|
||||
Kfusion(1) = SK_MX[0]*(P(1,19) + P(1,1)*SH_MAG[0] - P(1,2)*SH_MAG[1] + P(1,3)*SH_MAG[2] + P(1,0)*SK_MX[2] - P(1,16)*SK_MX[1] + P(1,17)*SK_MX[4] - P(1,18)*SK_MX[3]);
|
||||
Kfusion(2) = SK_MX[0]*(P(2,19) + P(2,1)*SH_MAG[0] - P(2,2)*SH_MAG[1] + P(2,3)*SH_MAG[2] + P(2,0)*SK_MX[2] - P(2,16)*SK_MX[1] + P(2,17)*SK_MX[4] - P(2,18)*SK_MX[3]);
|
||||
Kfusion(3) = SK_MX[0]*(P(3,19) + P(3,1)*SH_MAG[0] - P(3,2)*SH_MAG[1] + P(3,3)*SH_MAG[2] + P(3,0)*SK_MX[2] - P(3,16)*SK_MX[1] + P(3,17)*SK_MX[4] - P(3,18)*SK_MX[3]);
|
||||
Kfusion(4) = SK_MX[0]*(P(4,19) + P(4,1)*SH_MAG[0] - P(4,2)*SH_MAG[1] + P(4,3)*SH_MAG[2] + P(4,0)*SK_MX[2] - P(4,16)*SK_MX[1] + P(4,17)*SK_MX[4] - P(4,18)*SK_MX[3]);
|
||||
Kfusion(5) = SK_MX[0]*(P(5,19) + P(5,1)*SH_MAG[0] - P(5,2)*SH_MAG[1] + P(5,3)*SH_MAG[2] + P(5,0)*SK_MX[2] - P(5,16)*SK_MX[1] + P(5,17)*SK_MX[4] - P(5,18)*SK_MX[3]);
|
||||
Kfusion(6) = SK_MX[0]*(P(6,19) + P(6,1)*SH_MAG[0] - P(6,2)*SH_MAG[1] + P(6,3)*SH_MAG[2] + P(6,0)*SK_MX[2] - P(6,16)*SK_MX[1] + P(6,17)*SK_MX[4] - P(6,18)*SK_MX[3]);
|
||||
Kfusion(7) = SK_MX[0]*(P(7,19) + P(7,1)*SH_MAG[0] - P(7,2)*SH_MAG[1] + P(7,3)*SH_MAG[2] + P(7,0)*SK_MX[2] - P(7,16)*SK_MX[1] + P(7,17)*SK_MX[4] - P(7,18)*SK_MX[3]);
|
||||
Kfusion(8) = SK_MX[0]*(P(8,19) + P(8,1)*SH_MAG[0] - P(8,2)*SH_MAG[1] + P(8,3)*SH_MAG[2] + P(8,0)*SK_MX[2] - P(8,16)*SK_MX[1] + P(8,17)*SK_MX[4] - P(8,18)*SK_MX[3]);
|
||||
Kfusion(9) = SK_MX[0]*(P(9,19) + P(9,1)*SH_MAG[0] - P(9,2)*SH_MAG[1] + P(9,3)*SH_MAG[2] + P(9,0)*SK_MX[2] - P(9,16)*SK_MX[1] + P(9,17)*SK_MX[4] - P(9,18)*SK_MX[3]);
|
||||
Kfusion(10) = SK_MX[0]*(P(10,19) + P(10,1)*SH_MAG[0] - P(10,2)*SH_MAG[1] + P(10,3)*SH_MAG[2] + P(10,0)*SK_MX[2] - P(10,16)*SK_MX[1] + P(10,17)*SK_MX[4] - P(10,18)*SK_MX[3]);
|
||||
Kfusion(11) = SK_MX[0]*(P(11,19) + P(11,1)*SH_MAG[0] - P(11,2)*SH_MAG[1] + P(11,3)*SH_MAG[2] + P(11,0)*SK_MX[2] - P(11,16)*SK_MX[1] + P(11,17)*SK_MX[4] - P(11,18)*SK_MX[3]);
|
||||
Kfusion(12) = SK_MX[0]*(P(12,19) + P(12,1)*SH_MAG[0] - P(12,2)*SH_MAG[1] + P(12,3)*SH_MAG[2] + P(12,0)*SK_MX[2] - P(12,16)*SK_MX[1] + P(12,17)*SK_MX[4] - P(12,18)*SK_MX[3]);
|
||||
Kfusion(13) = SK_MX[0]*(P(13,19) + P(13,1)*SH_MAG[0] - P(13,2)*SH_MAG[1] + P(13,3)*SH_MAG[2] + P(13,0)*SK_MX[2] - P(13,16)*SK_MX[1] + P(13,17)*SK_MX[4] - P(13,18)*SK_MX[3]);
|
||||
Kfusion(14) = SK_MX[0]*(P(14,19) + P(14,1)*SH_MAG[0] - P(14,2)*SH_MAG[1] + P(14,3)*SH_MAG[2] + P(14,0)*SK_MX[2] - P(14,16)*SK_MX[1] + P(14,17)*SK_MX[4] - P(14,18)*SK_MX[3]);
|
||||
Kfusion(15) = SK_MX[0]*(P(15,19) + P(15,1)*SH_MAG[0] - P(15,2)*SH_MAG[1] + P(15,3)*SH_MAG[2] + P(15,0)*SK_MX[2] - P(15,16)*SK_MX[1] + P(15,17)*SK_MX[4] - P(15,18)*SK_MX[3]);
|
||||
Kfusion(22) = SK_MX[0]*(P(22,19) + P(22,1)*SH_MAG[0] - P(22,2)*SH_MAG[1] + P(22,3)*SH_MAG[2] + P(22,0)*SK_MX[2] - P(22,16)*SK_MX[1] + P(22,17)*SK_MX[4] - P(22,18)*SK_MX[3]);
|
||||
Kfusion(23) = SK_MX[0]*(P(23,19) + P(23,1)*SH_MAG[0] - P(23,2)*SH_MAG[1] + P(23,3)*SH_MAG[2] + P(23,0)*SK_MX[2] - P(23,16)*SK_MX[1] + P(23,17)*SK_MX[4] - P(23,18)*SK_MX[3]);
|
||||
}
|
||||
|
||||
Kfusion(16) = SK_MX[0]*(P(16,19) + P(16,1)*SH_MAG[0] - P(16,2)*SH_MAG[1] + P(16,3)*SH_MAG[2] + P(16,0)*SK_MX[2] - P(16,16)*SK_MX[1] + P(16,17)*SK_MX[4] - P(16,18)*SK_MX[3]);
|
||||
Kfusion(17) = SK_MX[0]*(P(17,19) + P(17,1)*SH_MAG[0] - P(17,2)*SH_MAG[1] + P(17,3)*SH_MAG[2] + P(17,0)*SK_MX[2] - P(17,16)*SK_MX[1] + P(17,17)*SK_MX[4] - P(17,18)*SK_MX[3]);
|
||||
Kfusion(18) = SK_MX[0]*(P(18,19) + P(18,1)*SH_MAG[0] - P(18,2)*SH_MAG[1] + P(18,3)*SH_MAG[2] + P(18,0)*SK_MX[2] - P(18,16)*SK_MX[1] + P(18,17)*SK_MX[4] - P(18,18)*SK_MX[3]);
|
||||
Kfusion(19) = SK_MX[0]*(P(19,19) + P(19,1)*SH_MAG[0] - P(19,2)*SH_MAG[1] + P(19,3)*SH_MAG[2] + P(19,0)*SK_MX[2] - P(19,16)*SK_MX[1] + P(19,17)*SK_MX[4] - P(19,18)*SK_MX[3]);
|
||||
Kfusion(20) = SK_MX[0]*(P(20,19) + P(20,1)*SH_MAG[0] - P(20,2)*SH_MAG[1] + P(20,3)*SH_MAG[2] + P(20,0)*SK_MX[2] - P(20,16)*SK_MX[1] + P(20,17)*SK_MX[4] - P(20,18)*SK_MX[3]);
|
||||
Kfusion(21) = SK_MX[0]*(P(21,19) + P(21,1)*SH_MAG[0] - P(21,2)*SH_MAG[1] + P(21,3)*SH_MAG[2] + P(21,0)*SK_MX[2] - P(21,16)*SK_MX[1] + P(21,17)*SK_MX[4] - P(21,18)*SK_MX[3]);
|
||||
|
||||
// find largest observation variance difference as a fraction of the matlab value
|
||||
float max_diff_fraction = 0.0f;
|
||||
int max_row;
|
||||
float max_old, max_new;
|
||||
for (int row=0; row<24; row++) {
|
||||
float diff_fraction;
|
||||
if (H_MAG(row) != 0.0f) {
|
||||
diff_fraction = fabsf(Hfusion_sympy[row] - H_MAG(row)) / fabsf(H_MAG(row));
|
||||
} else if (Hfusion_sympy[row] != 0.0f) {
|
||||
diff_fraction = fabsf(Hfusion_sympy[row] - H_MAG(row)) / fabsf(Hfusion_sympy[row]);
|
||||
} else {
|
||||
diff_fraction = 0.0f;
|
||||
}
|
||||
if (diff_fraction > max_diff_fraction) {
|
||||
max_diff_fraction = diff_fraction;
|
||||
max_row = row;
|
||||
max_old = H_MAG(row);
|
||||
max_new = Hfusion_sympy[row];
|
||||
}
|
||||
}
|
||||
|
||||
if (max_diff_fraction > 1e-5f) {
|
||||
printf("Fail: Magnetomer X axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
|
||||
} else {
|
||||
printf("Pass: Magnetomer X axis Hfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
|
||||
// find largest Kalman gain difference as a fraction of the matlab value
|
||||
max_diff_fraction = 0.0f;
|
||||
for (int row=0; row<4; row++) {
|
||||
float diff_fraction;
|
||||
if (Kfusion(row) != 0.0f) {
|
||||
diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion(row)) / fabsf(Kfusion(row));
|
||||
} else if (Kfusion_sympy(row) != 0.0f) {
|
||||
diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion(row)) / fabsf(Kfusion_sympy(row));
|
||||
} else {
|
||||
diff_fraction = 0.0f;
|
||||
}
|
||||
if (diff_fraction > max_diff_fraction) {
|
||||
max_diff_fraction = diff_fraction;
|
||||
max_row = row;
|
||||
max_old = Kfusion(row);
|
||||
max_new = Kfusion_sympy(row);
|
||||
}
|
||||
}
|
||||
|
||||
if (max_diff_fraction > 1e-5f) {
|
||||
printf("Fail: Magnetomer X axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
|
||||
} else {
|
||||
printf("Pass: Magnetomer X axis Kfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// Compare Y axis equations
|
||||
{
|
||||
// recalculate innovation variance becasue states and covariances have changed due to previous fusion
|
||||
const float HKY0 = magD*q1 + magE*q0 - magN*q3;
|
||||
const float HKY1 = magD*q0 - magE*q1 + magN*q2;
|
||||
const float HKY2 = magD*q3 + magE*q2 + magN*q1;
|
||||
const float HKY3 = magD*q2;
|
||||
const float HKY4 = magE*q3;
|
||||
const float HKY5 = magN*q0;
|
||||
const float HKY6 = q1*q2;
|
||||
const float HKY7 = q0*q3;
|
||||
const float HKY8 = powf(q0, 2) - powf(q1, 2) + powf(q2, 2) - powf(q3, 2);
|
||||
const float HKY9 = q0*q1 + q2*q3;
|
||||
const float HKY10 = 2*HKY9;
|
||||
const float HKY11 = -2*HKY6 + 2*HKY7;
|
||||
const float HKY12 = 2*HKY2;
|
||||
const float HKY13 = 2*HKY0;
|
||||
const float HKY14 = 2*HKY1;
|
||||
const float HKY15 = -2*HKY3 + 2*HKY4 + 2*HKY5;
|
||||
const float HKY16 = HKY10*P(0,18) - HKY11*P(0,16) + HKY12*P(0,2) + HKY13*P(0,0) + HKY14*P(0,1) - HKY15*P(0,3) + HKY8*P(0,17) + P(0,20);
|
||||
const float HKY17 = HKY10*P(17,18) - HKY11*P(16,17) + HKY12*P(2,17) + HKY13*P(0,17) + HKY14*P(1,17) - HKY15*P(3,17) + HKY8*P(17,17) + P(17,20);
|
||||
const float HKY18 = HKY10*P(16,18) - HKY11*P(16,16) + HKY12*P(2,16) + HKY13*P(0,16) + HKY14*P(1,16) - HKY15*P(3,16) + HKY8*P(16,17) + P(16,20);
|
||||
const float HKY19 = HKY10*P(3,18) - HKY11*P(3,16) + HKY12*P(2,3) + HKY13*P(0,3) + HKY14*P(1,3) - HKY15*P(3,3) + HKY8*P(3,17) + P(3,20);
|
||||
const float HKY20 = HKY10*P(18,18) - HKY11*P(16,18) + HKY12*P(2,18) + HKY13*P(0,18) + HKY14*P(1,18) - HKY15*P(3,18) + HKY8*P(17,18) + P(18,20);
|
||||
const float HKY21 = HKY10*P(1,18) - HKY11*P(1,16) + HKY12*P(1,2) + HKY13*P(0,1) + HKY14*P(1,1) - HKY15*P(1,3) + HKY8*P(1,17) + P(1,20);
|
||||
const float HKY22 = HKY10*P(2,18) - HKY11*P(2,16) + HKY12*P(2,2) + HKY13*P(0,2) + HKY14*P(1,2) - HKY15*P(2,3) + HKY8*P(2,17) + P(2,20);
|
||||
const float HKY23 = HKY10*P(18,20) - HKY11*P(16,20) + HKY12*P(2,20) + HKY13*P(0,20) + HKY14*P(1,20) - HKY15*P(3,20) + HKY8*P(17,20) + P(20,20);
|
||||
const float HKY24 = 1.0F/(HKY10*HKY20 - HKY11*HKY18 + HKY12*HKY22 + HKY13*HKY16 + HKY14*HKY21 - HKY15*HKY19 + HKY17*HKY8 + HKY23 + R_MAG);
|
||||
|
||||
// Calculate Y axis observation jacobians
|
||||
memset(Hfusion, 0, sizeof(Hfusion));
|
||||
Hfusion[0] = 2*HKY0;
|
||||
Hfusion[1] = 2*HKY1;
|
||||
Hfusion[2] = 2*HKY2;
|
||||
Hfusion[3] = 2*HKY3 - 2*HKY4 - 2*HKY5;
|
||||
Hfusion[16] = 2*HKY6 - 2*HKY7;
|
||||
Hfusion[17] = HKY8;
|
||||
Hfusion[18] = 2*HKY9;
|
||||
Hfusion[20] = 1;
|
||||
|
||||
// Calculate Y axis Kalman gains
|
||||
if (update_all_states) {
|
||||
Kfusion(0) = HKY16*HKY24;
|
||||
Kfusion(1) = HKY21*HKY24;
|
||||
Kfusion(2) = HKY22*HKY24;
|
||||
Kfusion(3) = HKY19*HKY24;
|
||||
|
||||
for (unsigned row = 4; row <= 15; row++) {
|
||||
Kfusion(row) = HKY24*(HKY10*P(row,18) - HKY11*P(row,16) + HKY12*P(2,row) + HKY13*P(0,row) + HKY14*P(1,row) - HKY15*P(3,row) + HKY8*P(row,17) + P(row,20));
|
||||
}
|
||||
|
||||
for (unsigned row = 22; row <= 23; row++) {
|
||||
Kfusion(row) = HKY24*(HKY10*P(18,row) - HKY11*P(16,row) + HKY12*P(2,row) + HKY13*P(0,row) + HKY14*P(1,row) - HKY15*P(3,row) + HKY8*P(17,row) + P(20,row));
|
||||
}
|
||||
}
|
||||
|
||||
Kfusion(16) = HKY18*HKY24;
|
||||
Kfusion(17) = HKY17*HKY24;
|
||||
Kfusion(18) = HKY20*HKY24;
|
||||
Kfusion(19) = HKY24*(HKY10*P(18,19) - HKY11*P(16,19) + HKY12*P(2,19) + HKY13*P(0,19) + HKY14*P(1,19) - HKY15*P(3,19) + HKY8*P(17,19) + P(19,20));
|
||||
Kfusion(20) = HKY23*HKY24;
|
||||
Kfusion(21) = HKY24*(HKY10*P(18,21) - HKY11*P(16,21) + HKY12*P(2,21) + HKY13*P(0,21) + HKY14*P(1,21) - HKY15*P(3,21) + HKY8*P(17,21) + P(20,21));
|
||||
|
||||
// save output and repeat calculation using legacy matlab generated code
|
||||
float Hfusion_sympy[24];
|
||||
Vector24f Kfusion_sympy;
|
||||
for (int row=0; row<24; row++) {
|
||||
Hfusion_sympy[row] = Hfusion[row];
|
||||
Kfusion_sympy(row) = Kfusion(row);
|
||||
}
|
||||
|
||||
// repeat calculation using matlab generated equations
|
||||
|
||||
// Y axis innovation variance
|
||||
mag_innov_var = (P(20,20) + R_MAG + P(0,20)*SH_MAG[2] + P(1,20)*SH_MAG[1] + P(2,20)*SH_MAG[0] - P(17,20)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2.0f*q0*q3 - 2.0f*q1*q2)*(P(20,16) + P(0,16)*SH_MAG[2] + P(1,16)*SH_MAG[1] + P(2,16)*SH_MAG[0] - P(17,16)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,16)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,16)*(2.0f*q0*q1 + 2.0f*q2*q3) - P(3,16)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (2.0f*q0*q1 + 2.0f*q2*q3)*(P(20,18) + P(0,18)*SH_MAG[2] + P(1,18)*SH_MAG[1] + P(2,18)*SH_MAG[0] - P(17,18)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,18)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,18)*(2.0f*q0*q1 + 2.0f*q2*q3) - P(3,18)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P(20,3) + P(0,3)*SH_MAG[2] + P(1,3)*SH_MAG[1] + P(2,3)*SH_MAG[0] - P(17,3)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,3)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,3)*(2.0f*q0*q1 + 2.0f*q2*q3) - P(3,3)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - P(16,20)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,20)*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_MAG[2]*(P(20,0) + P(0,0)*SH_MAG[2] + P(1,0)*SH_MAG[1] + P(2,0)*SH_MAG[0] - P(17,0)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,0)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,0)*(2.0f*q0*q1 + 2.0f*q2*q3) - P(3,0)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[1]*(P(20,1) + P(0,1)*SH_MAG[2] + P(1,1)*SH_MAG[1] + P(2,1)*SH_MAG[0] - P(17,1)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,1)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,1)*(2.0f*q0*q1 + 2.0f*q2*q3) - P(3,1)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[0]*(P(20,2) + P(0,2)*SH_MAG[2] + P(1,2)*SH_MAG[1] + P(2,2)*SH_MAG[0] - P(17,2)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,2)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,2)*(2.0f*q0*q1 + 2.0f*q2*q3) - P(3,2)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P(20,17) + P(0,17)*SH_MAG[2] + P(1,17)*SH_MAG[1] + P(2,17)*SH_MAG[0] - P(17,17)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,17)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,17)*(2.0f*q0*q1 + 2.0f*q2*q3) - P(3,17)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - P(3,20)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2));
|
||||
|
||||
// Calculate Y axis observation jacobians
|
||||
H_MAG.setZero();
|
||||
H_MAG(0) = SH_MAG[2];
|
||||
H_MAG(1) = SH_MAG[1];
|
||||
H_MAG(2) = SH_MAG[0];
|
||||
H_MAG(3) = 2.0f*magD*q2 - SH_MAG[8] - SH_MAG[7];
|
||||
H_MAG(16) = 2.0f*q1*q2 - 2.0f*q0*q3;
|
||||
H_MAG(17) = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6];
|
||||
H_MAG(18) = 2.0f*q0*q1 + 2.0f*q2*q3;
|
||||
H_MAG(20) = 1.0f;
|
||||
|
||||
// Calculate Y axis Kalman gains
|
||||
float SK_MY[5];
|
||||
SK_MY[0] = 1.0f / mag_innov_var;
|
||||
SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6];
|
||||
SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
|
||||
SK_MY[3] = 2.0f*q0*q3 - 2.0f*q1*q2;
|
||||
SK_MY[4] = 2.0f*q0*q1 + 2.0f*q2*q3;
|
||||
|
||||
if (update_all_states) {
|
||||
Kfusion(0) = SK_MY[0]*(P(0,20) + P(0,0)*SH_MAG[2] + P(0,1)*SH_MAG[1] + P(0,2)*SH_MAG[0] - P(0,3)*SK_MY[2] - P(0,17)*SK_MY[1] - P(0,16)*SK_MY[3] + P(0,18)*SK_MY[4]);
|
||||
Kfusion(1) = SK_MY[0]*(P(1,20) + P(1,0)*SH_MAG[2] + P(1,1)*SH_MAG[1] + P(1,2)*SH_MAG[0] - P(1,3)*SK_MY[2] - P(1,17)*SK_MY[1] - P(1,16)*SK_MY[3] + P(1,18)*SK_MY[4]);
|
||||
Kfusion(2) = SK_MY[0]*(P(2,20) + P(2,0)*SH_MAG[2] + P(2,1)*SH_MAG[1] + P(2,2)*SH_MAG[0] - P(2,3)*SK_MY[2] - P(2,17)*SK_MY[1] - P(2,16)*SK_MY[3] + P(2,18)*SK_MY[4]);
|
||||
Kfusion(3) = SK_MY[0]*(P(3,20) + P(3,0)*SH_MAG[2] + P(3,1)*SH_MAG[1] + P(3,2)*SH_MAG[0] - P(3,3)*SK_MY[2] - P(3,17)*SK_MY[1] - P(3,16)*SK_MY[3] + P(3,18)*SK_MY[4]);
|
||||
Kfusion(4) = SK_MY[0]*(P(4,20) + P(4,0)*SH_MAG[2] + P(4,1)*SH_MAG[1] + P(4,2)*SH_MAG[0] - P(4,3)*SK_MY[2] - P(4,17)*SK_MY[1] - P(4,16)*SK_MY[3] + P(4,18)*SK_MY[4]);
|
||||
Kfusion(5) = SK_MY[0]*(P(5,20) + P(5,0)*SH_MAG[2] + P(5,1)*SH_MAG[1] + P(5,2)*SH_MAG[0] - P(5,3)*SK_MY[2] - P(5,17)*SK_MY[1] - P(5,16)*SK_MY[3] + P(5,18)*SK_MY[4]);
|
||||
Kfusion(6) = SK_MY[0]*(P(6,20) + P(6,0)*SH_MAG[2] + P(6,1)*SH_MAG[1] + P(6,2)*SH_MAG[0] - P(6,3)*SK_MY[2] - P(6,17)*SK_MY[1] - P(6,16)*SK_MY[3] + P(6,18)*SK_MY[4]);
|
||||
Kfusion(7) = SK_MY[0]*(P(7,20) + P(7,0)*SH_MAG[2] + P(7,1)*SH_MAG[1] + P(7,2)*SH_MAG[0] - P(7,3)*SK_MY[2] - P(7,17)*SK_MY[1] - P(7,16)*SK_MY[3] + P(7,18)*SK_MY[4]);
|
||||
Kfusion(8) = SK_MY[0]*(P(8,20) + P(8,0)*SH_MAG[2] + P(8,1)*SH_MAG[1] + P(8,2)*SH_MAG[0] - P(8,3)*SK_MY[2] - P(8,17)*SK_MY[1] - P(8,16)*SK_MY[3] + P(8,18)*SK_MY[4]);
|
||||
Kfusion(9) = SK_MY[0]*(P(9,20) + P(9,0)*SH_MAG[2] + P(9,1)*SH_MAG[1] + P(9,2)*SH_MAG[0] - P(9,3)*SK_MY[2] - P(9,17)*SK_MY[1] - P(9,16)*SK_MY[3] + P(9,18)*SK_MY[4]);
|
||||
Kfusion(10) = SK_MY[0]*(P(10,20) + P(10,0)*SH_MAG[2] + P(10,1)*SH_MAG[1] + P(10,2)*SH_MAG[0] - P(10,3)*SK_MY[2] - P(10,17)*SK_MY[1] - P(10,16)*SK_MY[3] + P(10,18)*SK_MY[4]);
|
||||
Kfusion(11) = SK_MY[0]*(P(11,20) + P(11,0)*SH_MAG[2] + P(11,1)*SH_MAG[1] + P(11,2)*SH_MAG[0] - P(11,3)*SK_MY[2] - P(11,17)*SK_MY[1] - P(11,16)*SK_MY[3] + P(11,18)*SK_MY[4]);
|
||||
Kfusion(12) = SK_MY[0]*(P(12,20) + P(12,0)*SH_MAG[2] + P(12,1)*SH_MAG[1] + P(12,2)*SH_MAG[0] - P(12,3)*SK_MY[2] - P(12,17)*SK_MY[1] - P(12,16)*SK_MY[3] + P(12,18)*SK_MY[4]);
|
||||
Kfusion(13) = SK_MY[0]*(P(13,20) + P(13,0)*SH_MAG[2] + P(13,1)*SH_MAG[1] + P(13,2)*SH_MAG[0] - P(13,3)*SK_MY[2] - P(13,17)*SK_MY[1] - P(13,16)*SK_MY[3] + P(13,18)*SK_MY[4]);
|
||||
Kfusion(14) = SK_MY[0]*(P(14,20) + P(14,0)*SH_MAG[2] + P(14,1)*SH_MAG[1] + P(14,2)*SH_MAG[0] - P(14,3)*SK_MY[2] - P(14,17)*SK_MY[1] - P(14,16)*SK_MY[3] + P(14,18)*SK_MY[4]);
|
||||
Kfusion(15) = SK_MY[0]*(P(15,20) + P(15,0)*SH_MAG[2] + P(15,1)*SH_MAG[1] + P(15,2)*SH_MAG[0] - P(15,3)*SK_MY[2] - P(15,17)*SK_MY[1] - P(15,16)*SK_MY[3] + P(15,18)*SK_MY[4]);
|
||||
Kfusion(22) = SK_MY[0]*(P(22,20) + P(22,0)*SH_MAG[2] + P(22,1)*SH_MAG[1] + P(22,2)*SH_MAG[0] - P(22,3)*SK_MY[2] - P(22,17)*SK_MY[1] - P(22,16)*SK_MY[3] + P(22,18)*SK_MY[4]);
|
||||
Kfusion(23) = SK_MY[0]*(P(23,20) + P(23,0)*SH_MAG[2] + P(23,1)*SH_MAG[1] + P(23,2)*SH_MAG[0] - P(23,3)*SK_MY[2] - P(23,17)*SK_MY[1] - P(23,16)*SK_MY[3] + P(23,18)*SK_MY[4]);
|
||||
}
|
||||
|
||||
Kfusion(16) = SK_MY[0]*(P(16,20) + P(16,0)*SH_MAG[2] + P(16,1)*SH_MAG[1] + P(16,2)*SH_MAG[0] - P(16,3)*SK_MY[2] - P(16,17)*SK_MY[1] - P(16,16)*SK_MY[3] + P(16,18)*SK_MY[4]);
|
||||
Kfusion(17) = SK_MY[0]*(P(17,20) + P(17,0)*SH_MAG[2] + P(17,1)*SH_MAG[1] + P(17,2)*SH_MAG[0] - P(17,3)*SK_MY[2] - P(17,17)*SK_MY[1] - P(17,16)*SK_MY[3] + P(17,18)*SK_MY[4]);
|
||||
Kfusion(18) = SK_MY[0]*(P(18,20) + P(18,0)*SH_MAG[2] + P(18,1)*SH_MAG[1] + P(18,2)*SH_MAG[0] - P(18,3)*SK_MY[2] - P(18,17)*SK_MY[1] - P(18,16)*SK_MY[3] + P(18,18)*SK_MY[4]);
|
||||
Kfusion(19) = SK_MY[0]*(P(19,20) + P(19,0)*SH_MAG[2] + P(19,1)*SH_MAG[1] + P(19,2)*SH_MAG[0] - P(19,3)*SK_MY[2] - P(19,17)*SK_MY[1] - P(19,16)*SK_MY[3] + P(19,18)*SK_MY[4]);
|
||||
Kfusion(20) = SK_MY[0]*(P(20,20) + P(20,0)*SH_MAG[2] + P(20,1)*SH_MAG[1] + P(20,2)*SH_MAG[0] - P(20,3)*SK_MY[2] - P(20,17)*SK_MY[1] - P(20,16)*SK_MY[3] + P(20,18)*SK_MY[4]);
|
||||
Kfusion(21) = SK_MY[0]*(P(21,20) + P(21,0)*SH_MAG[2] + P(21,1)*SH_MAG[1] + P(21,2)*SH_MAG[0] - P(21,3)*SK_MY[2] - P(21,17)*SK_MY[1] - P(21,16)*SK_MY[3] + P(21,18)*SK_MY[4]);
|
||||
|
||||
// find largest observation variance difference as a fraction of the matlab value
|
||||
float max_diff_fraction = 0.0f;
|
||||
int max_row;
|
||||
float max_old, max_new;
|
||||
for (int row=0; row<24; row++) {
|
||||
float diff_fraction;
|
||||
if (H_MAG(row) != 0.0f) {
|
||||
diff_fraction = fabsf(Hfusion_sympy[row] - H_MAG(row)) / fabsf(H_MAG(row));
|
||||
} else if (Hfusion_sympy[row] != 0.0f) {
|
||||
diff_fraction = fabsf(Hfusion_sympy[row] - H_MAG(row)) / fabsf(Hfusion_sympy[row]);
|
||||
} else {
|
||||
diff_fraction = 0.0f;
|
||||
}
|
||||
if (diff_fraction > max_diff_fraction) {
|
||||
max_diff_fraction = diff_fraction;
|
||||
max_row = row;
|
||||
max_old = H_MAG(row);
|
||||
max_new = Hfusion_sympy[row];
|
||||
}
|
||||
}
|
||||
|
||||
if (max_diff_fraction > 1e-5f) {
|
||||
printf("Fail: Magnetomer Y axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
|
||||
} else {
|
||||
printf("Pass: Magnetomer Y axis Hfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
|
||||
// find largest Kalman gain difference as a fraction of the matlab value
|
||||
max_diff_fraction = 0.0f;
|
||||
for (int row=0; row<4; row++) {
|
||||
float diff_fraction;
|
||||
if (Kfusion(row) != 0.0f) {
|
||||
diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion(row)) / fabsf(Kfusion(row));
|
||||
} else if (Kfusion_sympy(row) != 0.0f) {
|
||||
diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion(row)) / fabsf(Kfusion_sympy(row));
|
||||
} else {
|
||||
diff_fraction = 0.0f;
|
||||
}
|
||||
if (diff_fraction > max_diff_fraction) {
|
||||
max_diff_fraction = diff_fraction;
|
||||
max_row = row;
|
||||
max_old = Kfusion(row);
|
||||
max_new = Kfusion_sympy(row);
|
||||
}
|
||||
}
|
||||
|
||||
if (max_diff_fraction > 1e-5f) {
|
||||
printf("Fail: Magnetomer Y axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
|
||||
} else {
|
||||
printf("Pass: Magnetomer Y axis Kfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// Compare Z axis equations
|
||||
{
|
||||
// recalculate innovation variance becasue states and covariances have changed due to previous fusion
|
||||
const float HKZ0 = magD*q0 - magE*q1 + magN*q2;
|
||||
const float HKZ1 = magN*q3;
|
||||
const float HKZ2 = magD*q1;
|
||||
const float HKZ3 = magE*q0;
|
||||
const float HKZ4 = -magD*q2 + magE*q3 + magN*q0;
|
||||
const float HKZ5 = magD*q3 + magE*q2 + magN*q1;
|
||||
const float HKZ6 = q0*q2 + q1*q3;
|
||||
const float HKZ7 = q2*q3;
|
||||
const float HKZ8 = q0*q1;
|
||||
const float HKZ9 = powf(q0, 2) - powf(q1, 2) - powf(q2, 2) + powf(q3, 2);
|
||||
const float HKZ10 = 2*HKZ6;
|
||||
const float HKZ11 = -2*HKZ7 + 2*HKZ8;
|
||||
const float HKZ12 = 2*HKZ5;
|
||||
const float HKZ13 = 2*HKZ0;
|
||||
const float HKZ14 = -2*HKZ1 + 2*HKZ2 + 2*HKZ3;
|
||||
const float HKZ15 = 2*HKZ4;
|
||||
const float HKZ16 = HKZ10*P(0,16) - HKZ11*P(0,17) + HKZ12*P(0,3) + HKZ13*P(0,0) - HKZ14*P(0,1) + HKZ15*P(0,2) + HKZ9*P(0,18) + P(0,21);
|
||||
const float HKZ17 = HKZ10*P(16,18) - HKZ11*P(17,18) + HKZ12*P(3,18) + HKZ13*P(0,18) - HKZ14*P(1,18) + HKZ15*P(2,18) + HKZ9*P(18,18) + P(18,21);
|
||||
const float HKZ18 = HKZ10*P(16,17) - HKZ11*P(17,17) + HKZ12*P(3,17) + HKZ13*P(0,17) - HKZ14*P(1,17) + HKZ15*P(2,17) + HKZ9*P(17,18) + P(17,21);
|
||||
const float HKZ19 = HKZ10*P(1,16) - HKZ11*P(1,17) + HKZ12*P(1,3) + HKZ13*P(0,1) - HKZ14*P(1,1) + HKZ15*P(1,2) + HKZ9*P(1,18) + P(1,21);
|
||||
const float HKZ20 = HKZ10*P(16,16) - HKZ11*P(16,17) + HKZ12*P(3,16) + HKZ13*P(0,16) - HKZ14*P(1,16) + HKZ15*P(2,16) + HKZ9*P(16,18) + P(16,21);
|
||||
const float HKZ21 = HKZ10*P(3,16) - HKZ11*P(3,17) + HKZ12*P(3,3) + HKZ13*P(0,3) - HKZ14*P(1,3) + HKZ15*P(2,3) + HKZ9*P(3,18) + P(3,21);
|
||||
const float HKZ22 = HKZ10*P(2,16) - HKZ11*P(2,17) + HKZ12*P(2,3) + HKZ13*P(0,2) - HKZ14*P(1,2) + HKZ15*P(2,2) + HKZ9*P(2,18) + P(2,21);
|
||||
const float HKZ23 = HKZ10*P(16,21) - HKZ11*P(17,21) + HKZ12*P(3,21) + HKZ13*P(0,21) - HKZ14*P(1,21) + HKZ15*P(2,21) + HKZ9*P(18,21) + P(21,21);
|
||||
const float HKZ24 = 1.0F/(HKZ10*HKZ20 - HKZ11*HKZ18 + HKZ12*HKZ21 + HKZ13*HKZ16 - HKZ14*HKZ19 + HKZ15*HKZ22 + HKZ17*HKZ9 + HKZ23 + R_MAG);
|
||||
|
||||
// calculate Z axis observation jacobians
|
||||
memset(Hfusion, 0, sizeof(Hfusion));
|
||||
Hfusion[0] = 2*HKZ0;
|
||||
Hfusion[1] = 2*HKZ1 - 2*HKZ2 - 2*HKZ3;
|
||||
Hfusion[2] = 2*HKZ4;
|
||||
Hfusion[3] = 2*HKZ5;
|
||||
Hfusion[16] = 2*HKZ6;
|
||||
Hfusion[17] = 2*HKZ7 - 2*HKZ8;
|
||||
Hfusion[18] = HKZ9;
|
||||
Hfusion[21] = 1;
|
||||
|
||||
// Calculate Z axis Kalman gains
|
||||
if (update_all_states) {
|
||||
Kfusion(0) = HKZ16*HKZ24;
|
||||
Kfusion(1) = HKZ19*HKZ24;
|
||||
Kfusion(2) = HKZ22*HKZ24;
|
||||
Kfusion(3) = HKZ21*HKZ24;
|
||||
|
||||
for (unsigned row = 4; row <= 15; row++) {
|
||||
Kfusion(row) = HKZ24*(HKZ10*P(row,16) - HKZ11*P(row,17) + HKZ12*P(3,row) + HKZ13*P(0,row) - HKZ14*P(1,row) + HKZ15*P(2,row) + HKZ9*P(row,18) + P(row,21));
|
||||
}
|
||||
|
||||
for (unsigned row = 22; row <= 23; row++) {
|
||||
Kfusion(row) = HKZ24*(HKZ10*P(16,row) - HKZ11*P(17,row) + HKZ12*P(3,row) + HKZ13*P(0,row) - HKZ14*P(1,row) + HKZ15*P(2,row) + HKZ9*P(18,row) + P(21,row));
|
||||
}
|
||||
}
|
||||
|
||||
Kfusion(16) = HKZ20*HKZ24;
|
||||
Kfusion(17) = HKZ18*HKZ24;
|
||||
Kfusion(18) = HKZ17*HKZ24;
|
||||
|
||||
for (unsigned row = 19; row <= 20; row++) {
|
||||
Kfusion(row) = HKZ24*(HKZ10*P(16,row) - HKZ11*P(17,row) + HKZ12*P(3,row) + HKZ13*P(0,row) - HKZ14*P(1,row) + HKZ15*P(2,row) + HKZ9*P(18,row) + P(row,21));
|
||||
}
|
||||
|
||||
Kfusion(21) = HKZ23*HKZ24;
|
||||
|
||||
// save output and repeat calculation using legacy matlab generated code
|
||||
float Hfusion_sympy[24];
|
||||
Vector24f Kfusion_sympy;
|
||||
for (int row=0; row<24; row++) {
|
||||
Hfusion_sympy[row] = Hfusion[row];
|
||||
Kfusion_sympy(row) = Kfusion(row);
|
||||
}
|
||||
|
||||
// repeat calculation using matlab generated equations
|
||||
|
||||
// Z axis innovation variance
|
||||
mag_innov_var = (P(21,21) + R_MAG + P(0,21)*SH_MAG[1] - P(1,21)*SH_MAG[2] + P(3,21)*SH_MAG[0] + P(18,21)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + (2.0f*q0*q2 + 2.0f*q1*q3)*(P(21,16) + P(0,16)*SH_MAG[1] - P(1,16)*SH_MAG[2] + P(3,16)*SH_MAG[0] + P(18,16)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,16)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,16)*(2.0f*q0*q1 - 2.0f*q2*q3) + P(2,16)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (2.0f*q0*q1 - 2.0f*q2*q3)*(P(21,17) + P(0,17)*SH_MAG[1] - P(1,17)*SH_MAG[2] + P(3,17)*SH_MAG[0] + P(18,17)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,17)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,17)*(2.0f*q0*q1 - 2.0f*q2*q3) + P(2,17)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P(21,2) + P(0,2)*SH_MAG[1] - P(1,2)*SH_MAG[2] + P(3,2)*SH_MAG[0] + P(18,2)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,2)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,2)*(2.0f*q0*q1 - 2.0f*q2*q3) + P(2,2)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(16,21)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,21)*(2.0f*q0*q1 - 2.0f*q2*q3) + SH_MAG[1]*(P(21,0) + P(0,0)*SH_MAG[1] - P(1,0)*SH_MAG[2] + P(3,0)*SH_MAG[0] + P(18,0)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,0)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,0)*(2.0f*q0*q1 - 2.0f*q2*q3) + P(2,0)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - SH_MAG[2]*(P(21,1) + P(0,1)*SH_MAG[1] - P(1,1)*SH_MAG[2] + P(3,1)*SH_MAG[0] + P(18,1)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,1)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,1)*(2.0f*q0*q1 - 2.0f*q2*q3) + P(2,1)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[0]*(P(21,3) + P(0,3)*SH_MAG[1] - P(1,3)*SH_MAG[2] + P(3,3)*SH_MAG[0] + P(18,3)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,3)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,3)*(2.0f*q0*q1 - 2.0f*q2*q3) + P(2,3)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P(21,18) + P(0,18)*SH_MAG[1] - P(1,18)*SH_MAG[2] + P(3,18)*SH_MAG[0] + P(18,18)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,18)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,18)*(2.0f*q0*q1 - 2.0f*q2*q3) + P(2,18)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(2,21)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2));
|
||||
|
||||
// calculate Z axis observation jacobians
|
||||
H_MAG.setZero();
|
||||
H_MAG(0) = SH_MAG[1];
|
||||
H_MAG(1) = -SH_MAG[2];
|
||||
H_MAG(2) = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
|
||||
H_MAG(3) = SH_MAG[0];
|
||||
H_MAG(16) = 2.0f*q0*q2 + 2.0f*q1*q3;
|
||||
H_MAG(17) = 2.0f*q2*q3 - 2.0f*q0*q1;
|
||||
H_MAG(18) = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
|
||||
H_MAG(21) = 1.0f;
|
||||
|
||||
// Calculate Z axis Kalman gains
|
||||
float SK_MZ[5];
|
||||
SK_MZ[0] = 1.0f / mag_innov_var;
|
||||
SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
|
||||
SK_MZ[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
|
||||
SK_MZ[3] = 2.0f*q0*q1 - 2.0f*q2*q3;
|
||||
SK_MZ[4] = 2.0f*q0*q2 + 2.0f*q1*q3;
|
||||
|
||||
if (update_all_states) {
|
||||
Kfusion(0) = SK_MZ[0]*(P(0,21) + P(0,0)*SH_MAG[1] - P(0,1)*SH_MAG[2] + P(0,3)*SH_MAG[0] + P(0,2)*SK_MZ[2] + P(0,18)*SK_MZ[1] + P(0,16)*SK_MZ[4] - P(0,17)*SK_MZ[3]);
|
||||
Kfusion(1) = SK_MZ[0]*(P(1,21) + P(1,0)*SH_MAG[1] - P(1,1)*SH_MAG[2] + P(1,3)*SH_MAG[0] + P(1,2)*SK_MZ[2] + P(1,18)*SK_MZ[1] + P(1,16)*SK_MZ[4] - P(1,17)*SK_MZ[3]);
|
||||
Kfusion(2) = SK_MZ[0]*(P(2,21) + P(2,0)*SH_MAG[1] - P(2,1)*SH_MAG[2] + P(2,3)*SH_MAG[0] + P(2,2)*SK_MZ[2] + P(2,18)*SK_MZ[1] + P(2,16)*SK_MZ[4] - P(2,17)*SK_MZ[3]);
|
||||
Kfusion(3) = SK_MZ[0]*(P(3,21) + P(3,0)*SH_MAG[1] - P(3,1)*SH_MAG[2] + P(3,3)*SH_MAG[0] + P(3,2)*SK_MZ[2] + P(3,18)*SK_MZ[1] + P(3,16)*SK_MZ[4] - P(3,17)*SK_MZ[3]);
|
||||
Kfusion(4) = SK_MZ[0]*(P(4,21) + P(4,0)*SH_MAG[1] - P(4,1)*SH_MAG[2] + P(4,3)*SH_MAG[0] + P(4,2)*SK_MZ[2] + P(4,18)*SK_MZ[1] + P(4,16)*SK_MZ[4] - P(4,17)*SK_MZ[3]);
|
||||
Kfusion(5) = SK_MZ[0]*(P(5,21) + P(5,0)*SH_MAG[1] - P(5,1)*SH_MAG[2] + P(5,3)*SH_MAG[0] + P(5,2)*SK_MZ[2] + P(5,18)*SK_MZ[1] + P(5,16)*SK_MZ[4] - P(5,17)*SK_MZ[3]);
|
||||
Kfusion(6) = SK_MZ[0]*(P(6,21) + P(6,0)*SH_MAG[1] - P(6,1)*SH_MAG[2] + P(6,3)*SH_MAG[0] + P(6,2)*SK_MZ[2] + P(6,18)*SK_MZ[1] + P(6,16)*SK_MZ[4] - P(6,17)*SK_MZ[3]);
|
||||
Kfusion(7) = SK_MZ[0]*(P(7,21) + P(7,0)*SH_MAG[1] - P(7,1)*SH_MAG[2] + P(7,3)*SH_MAG[0] + P(7,2)*SK_MZ[2] + P(7,18)*SK_MZ[1] + P(7,16)*SK_MZ[4] - P(7,17)*SK_MZ[3]);
|
||||
Kfusion(8) = SK_MZ[0]*(P(8,21) + P(8,0)*SH_MAG[1] - P(8,1)*SH_MAG[2] + P(8,3)*SH_MAG[0] + P(8,2)*SK_MZ[2] + P(8,18)*SK_MZ[1] + P(8,16)*SK_MZ[4] - P(8,17)*SK_MZ[3]);
|
||||
Kfusion(9) = SK_MZ[0]*(P(9,21) + P(9,0)*SH_MAG[1] - P(9,1)*SH_MAG[2] + P(9,3)*SH_MAG[0] + P(9,2)*SK_MZ[2] + P(9,18)*SK_MZ[1] + P(9,16)*SK_MZ[4] - P(9,17)*SK_MZ[3]);
|
||||
Kfusion(10) = SK_MZ[0]*(P(10,21) + P(10,0)*SH_MAG[1] - P(10,1)*SH_MAG[2] + P(10,3)*SH_MAG[0] + P(10,2)*SK_MZ[2] + P(10,18)*SK_MZ[1] + P(10,16)*SK_MZ[4] - P(10,17)*SK_MZ[3]);
|
||||
Kfusion(11) = SK_MZ[0]*(P(11,21) + P(11,0)*SH_MAG[1] - P(11,1)*SH_MAG[2] + P(11,3)*SH_MAG[0] + P(11,2)*SK_MZ[2] + P(11,18)*SK_MZ[1] + P(11,16)*SK_MZ[4] - P(11,17)*SK_MZ[3]);
|
||||
Kfusion(12) = SK_MZ[0]*(P(12,21) + P(12,0)*SH_MAG[1] - P(12,1)*SH_MAG[2] + P(12,3)*SH_MAG[0] + P(12,2)*SK_MZ[2] + P(12,18)*SK_MZ[1] + P(12,16)*SK_MZ[4] - P(12,17)*SK_MZ[3]);
|
||||
Kfusion(13) = SK_MZ[0]*(P(13,21) + P(13,0)*SH_MAG[1] - P(13,1)*SH_MAG[2] + P(13,3)*SH_MAG[0] + P(13,2)*SK_MZ[2] + P(13,18)*SK_MZ[1] + P(13,16)*SK_MZ[4] - P(13,17)*SK_MZ[3]);
|
||||
Kfusion(14) = SK_MZ[0]*(P(14,21) + P(14,0)*SH_MAG[1] - P(14,1)*SH_MAG[2] + P(14,3)*SH_MAG[0] + P(14,2)*SK_MZ[2] + P(14,18)*SK_MZ[1] + P(14,16)*SK_MZ[4] - P(14,17)*SK_MZ[3]);
|
||||
Kfusion(15) = SK_MZ[0]*(P(15,21) + P(15,0)*SH_MAG[1] - P(15,1)*SH_MAG[2] + P(15,3)*SH_MAG[0] + P(15,2)*SK_MZ[2] + P(15,18)*SK_MZ[1] + P(15,16)*SK_MZ[4] - P(15,17)*SK_MZ[3]);
|
||||
Kfusion(22) = SK_MZ[0]*(P(22,21) + P(22,0)*SH_MAG[1] - P(22,1)*SH_MAG[2] + P(22,3)*SH_MAG[0] + P(22,2)*SK_MZ[2] + P(22,18)*SK_MZ[1] + P(22,16)*SK_MZ[4] - P(22,17)*SK_MZ[3]);
|
||||
Kfusion(23) = SK_MZ[0]*(P(23,21) + P(23,0)*SH_MAG[1] - P(23,1)*SH_MAG[2] + P(23,3)*SH_MAG[0] + P(23,2)*SK_MZ[2] + P(23,18)*SK_MZ[1] + P(23,16)*SK_MZ[4] - P(23,17)*SK_MZ[3]);
|
||||
}
|
||||
|
||||
Kfusion(16) = SK_MZ[0]*(P(16,21) + P(16,0)*SH_MAG[1] - P(16,1)*SH_MAG[2] + P(16,3)*SH_MAG[0] + P(16,2)*SK_MZ[2] + P(16,18)*SK_MZ[1] + P(16,16)*SK_MZ[4] - P(16,17)*SK_MZ[3]);
|
||||
Kfusion(17) = SK_MZ[0]*(P(17,21) + P(17,0)*SH_MAG[1] - P(17,1)*SH_MAG[2] + P(17,3)*SH_MAG[0] + P(17,2)*SK_MZ[2] + P(17,18)*SK_MZ[1] + P(17,16)*SK_MZ[4] - P(17,17)*SK_MZ[3]);
|
||||
Kfusion(18) = SK_MZ[0]*(P(18,21) + P(18,0)*SH_MAG[1] - P(18,1)*SH_MAG[2] + P(18,3)*SH_MAG[0] + P(18,2)*SK_MZ[2] + P(18,18)*SK_MZ[1] + P(18,16)*SK_MZ[4] - P(18,17)*SK_MZ[3]);
|
||||
Kfusion(19) = SK_MZ[0]*(P(19,21) + P(19,0)*SH_MAG[1] - P(19,1)*SH_MAG[2] + P(19,3)*SH_MAG[0] + P(19,2)*SK_MZ[2] + P(19,18)*SK_MZ[1] + P(19,16)*SK_MZ[4] - P(19,17)*SK_MZ[3]);
|
||||
Kfusion(20) = SK_MZ[0]*(P(20,21) + P(20,0)*SH_MAG[1] - P(20,1)*SH_MAG[2] + P(20,3)*SH_MAG[0] + P(20,2)*SK_MZ[2] + P(20,18)*SK_MZ[1] + P(20,16)*SK_MZ[4] - P(20,17)*SK_MZ[3]);
|
||||
Kfusion(21) = SK_MZ[0]*(P(21,21) + P(21,0)*SH_MAG[1] - P(21,1)*SH_MAG[2] + P(21,3)*SH_MAG[0] + P(21,2)*SK_MZ[2] + P(21,18)*SK_MZ[1] + P(21,16)*SK_MZ[4] - P(21,17)*SK_MZ[3]);
|
||||
|
||||
// find largest observation variance difference as a fraction of the matlab value
|
||||
float max_diff_fraction = 0.0f;
|
||||
int max_row;
|
||||
float max_old, max_new;
|
||||
for (int row=0; row<24; row++) {
|
||||
float diff_fraction;
|
||||
if (H_MAG(row) != 0.0f) {
|
||||
diff_fraction = fabsf(Hfusion_sympy[row] - H_MAG(row)) / fabsf(H_MAG(row));
|
||||
} else if (Hfusion_sympy[row] != 0.0f) {
|
||||
diff_fraction = fabsf(Hfusion_sympy[row] - H_MAG(row)) / fabsf(Hfusion_sympy[row]);
|
||||
} else {
|
||||
diff_fraction = 0.0f;
|
||||
}
|
||||
if (diff_fraction > max_diff_fraction) {
|
||||
max_diff_fraction = diff_fraction;
|
||||
max_row = row;
|
||||
max_old = H_MAG(row);
|
||||
max_new = Hfusion_sympy[row];
|
||||
}
|
||||
}
|
||||
|
||||
if (max_diff_fraction > 1e-5f) {
|
||||
printf("Fail: Magnetomer Z axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
|
||||
} else {
|
||||
printf("Pass: Magnetomer Z axis Hfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
|
||||
// find largest Kalman gain difference as a fraction of the matlab value
|
||||
max_diff_fraction = 0.0f;
|
||||
for (int row=0; row<4; row++) {
|
||||
float diff_fraction;
|
||||
if (Kfusion(row) != 0.0f) {
|
||||
diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion(row)) / fabsf(Kfusion(row));
|
||||
} else if (Kfusion_sympy(row) != 0.0f) {
|
||||
diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion(row)) / fabsf(Kfusion_sympy(row));
|
||||
} else {
|
||||
diff_fraction = 0.0f;
|
||||
}
|
||||
if (diff_fraction > max_diff_fraction) {
|
||||
max_diff_fraction = diff_fraction;
|
||||
max_row = row;
|
||||
max_old = Kfusion(row);
|
||||
max_new = Kfusion_sympy(row);
|
||||
}
|
||||
}
|
||||
|
||||
if (max_diff_fraction > 1e-5f) {
|
||||
printf("Fail: Magnetomer Z axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
|
||||
} else {
|
||||
printf("Pass: Magnetomer Z axis Kfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,344 @@
|
||||
// Axis 0 equations
|
||||
// Sub Expressions
|
||||
const float HKX0 = magE*q3;
|
||||
const float HKX1 = magD*q2;
|
||||
const float HKX2 = magD*q3 + magE*q2;
|
||||
const float HKX3 = magE*q1;
|
||||
const float HKX4 = magD*q0;
|
||||
const float HKX5 = 2*magN;
|
||||
const float HKX6 = HKX5*q2;
|
||||
const float HKX7 = -HKX5*q3 + magD*q1 + magE*q0;
|
||||
const float HKX8 = 2*powf(q2, 2);
|
||||
const float HKX9 = 2*powf(q3, 2);
|
||||
const float HKX10 = q0*q3 + q1*q2;
|
||||
const float HKX11 = q1*q3;
|
||||
const float HKX12 = q0*q2;
|
||||
const float HKX13 = 2*HKX2;
|
||||
const float HKX14 = HKX13*P(0,1);
|
||||
const float HKX15 = 2*HKX10;
|
||||
const float HKX16 = HKX15*P(0,17);
|
||||
const float HKX17 = -2*HKX0 + 2*HKX1;
|
||||
const float HKX18 = HKX17*P(0,0);
|
||||
const float HKX19 = -2*HKX11 + 2*HKX12;
|
||||
const float HKX20 = HKX19*P(0,18);
|
||||
const float HKX21 = HKX8 + HKX9 - 1;
|
||||
const float HKX22 = HKX21*P(0,16);
|
||||
const float HKX23 = 2*HKX7;
|
||||
const float HKX24 = HKX23*P(0,3);
|
||||
const float HKX25 = -2*HKX3 + 2*HKX4 + 2*HKX6;
|
||||
const float HKX26 = HKX25*P(0,2);
|
||||
const float HKX27 = HKX13*P(1,16);
|
||||
const float HKX28 = HKX15*P(16,17);
|
||||
const float HKX29 = HKX17*P(0,16);
|
||||
const float HKX30 = HKX19*P(16,18);
|
||||
const float HKX31 = HKX23*P(3,16);
|
||||
const float HKX32 = HKX21*P(16,16);
|
||||
const float HKX33 = HKX25*P(2,16);
|
||||
const float HKX34 = HKX13*P(1,1);
|
||||
const float HKX35 = HKX15*P(1,17);
|
||||
const float HKX36 = HKX17*P(0,1);
|
||||
const float HKX37 = HKX19*P(1,18);
|
||||
const float HKX38 = HKX23*P(1,3);
|
||||
const float HKX39 = HKX21*P(1,16);
|
||||
const float HKX40 = HKX25*P(1,2);
|
||||
const float HKX41 = HKX13*P(1,17);
|
||||
const float HKX42 = HKX15*P(17,17);
|
||||
const float HKX43 = HKX17*P(0,17);
|
||||
const float HKX44 = HKX19*P(17,18);
|
||||
const float HKX45 = HKX23*P(3,17);
|
||||
const float HKX46 = HKX21*P(16,17);
|
||||
const float HKX47 = HKX25*P(2,17);
|
||||
const float HKX48 = HKX13*P(1,3);
|
||||
const float HKX49 = HKX15*P(3,17);
|
||||
const float HKX50 = HKX17*P(0,3);
|
||||
const float HKX51 = HKX19*P(3,18);
|
||||
const float HKX52 = HKX23*P(3,3);
|
||||
const float HKX53 = HKX25*P(2,3);
|
||||
const float HKX54 = HKX21*P(3,16);
|
||||
const float HKX55 = HKX13*P(1,18);
|
||||
const float HKX56 = HKX15*P(17,18);
|
||||
const float HKX57 = HKX17*P(0,18);
|
||||
const float HKX58 = HKX19*P(18,18);
|
||||
const float HKX59 = HKX23*P(3,18);
|
||||
const float HKX60 = HKX21*P(16,18);
|
||||
const float HKX61 = HKX25*P(2,18);
|
||||
const float HKX62 = HKX13*P(1,2);
|
||||
const float HKX63 = HKX15*P(2,17);
|
||||
const float HKX64 = HKX17*P(0,2);
|
||||
const float HKX65 = HKX19*P(2,18);
|
||||
const float HKX66 = HKX23*P(2,3);
|
||||
const float HKX67 = HKX21*P(2,16);
|
||||
const float HKX68 = HKX25*P(2,2);
|
||||
const float HKX69 = -HKX13*P(1,19) - HKX15*P(17,19) + HKX17*P(0,19) + HKX19*P(18,19) + HKX21*P(16,19) - HKX23*P(3,19) + HKX25*P(2,19) - P(19,19);
|
||||
const float HKX70 = 1.0F/(-HKX13*(HKX34 + HKX35 - HKX36 - HKX37 + HKX38 - HKX39 - HKX40 + P(1,19)) - HKX15*(HKX41 + HKX42 - HKX43 - HKX44 + HKX45 - HKX46 - HKX47 + P(17,19)) + HKX17*(HKX14 + HKX16 - HKX18 - HKX20 - HKX22 + HKX24 - HKX26 + P(0,19)) + HKX19*(HKX55 + HKX56 - HKX57 - HKX58 + HKX59 - HKX60 - HKX61 + P(18,19)) + HKX21*(HKX27 + HKX28 - HKX29 - HKX30 + HKX31 - HKX32 - HKX33 + P(16,19)) - HKX23*(HKX48 + HKX49 - HKX50 - HKX51 + HKX52 - HKX53 - HKX54 + P(3,19)) + HKX25*(HKX62 + HKX63 - HKX64 - HKX65 + HKX66 - HKX67 - HKX68 + P(2,19)) + HKX69 - R_MAG);
|
||||
|
||||
|
||||
// Observation Jacobians
|
||||
Hfusion.at<0>() = 2*HKX0 - 2*HKX1;
|
||||
Hfusion.at<1>() = 2*HKX2;
|
||||
Hfusion.at<2>() = 2*HKX3 - 2*HKX4 - 2*HKX6;
|
||||
Hfusion.at<3>() = 2*HKX7;
|
||||
Hfusion.at<4>() = 0;
|
||||
Hfusion.at<5>() = 0;
|
||||
Hfusion.at<6>() = 0;
|
||||
Hfusion.at<7>() = 0;
|
||||
Hfusion.at<8>() = 0;
|
||||
Hfusion.at<9>() = 0;
|
||||
Hfusion.at<10>() = 0;
|
||||
Hfusion.at<11>() = 0;
|
||||
Hfusion.at<12>() = 0;
|
||||
Hfusion.at<13>() = 0;
|
||||
Hfusion.at<14>() = 0;
|
||||
Hfusion.at<15>() = 0;
|
||||
Hfusion.at<16>() = -HKX8 - HKX9 + 1;
|
||||
Hfusion.at<17>() = 2*HKX10;
|
||||
Hfusion.at<18>() = 2*HKX11 - 2*HKX12;
|
||||
Hfusion.at<19>() = 1;
|
||||
Hfusion.at<20>() = 0;
|
||||
Hfusion.at<21>() = 0;
|
||||
Hfusion.at<22>() = 0;
|
||||
Hfusion.at<23>() = 0;
|
||||
|
||||
|
||||
// Kalman gains
|
||||
Kfusion(0) = HKX70*(-HKX14 - HKX16 + HKX18 + HKX20 + HKX22 - HKX24 + HKX26 - P(0,19));
|
||||
Kfusion(1) = HKX70*(-HKX34 - HKX35 + HKX36 + HKX37 - HKX38 + HKX39 + HKX40 - P(1,19));
|
||||
Kfusion(2) = HKX70*(-HKX62 - HKX63 + HKX64 + HKX65 - HKX66 + HKX67 + HKX68 - P(2,19));
|
||||
Kfusion(3) = HKX70*(-HKX48 - HKX49 + HKX50 + HKX51 - HKX52 + HKX53 + HKX54 - P(3,19));
|
||||
Kfusion(4) = HKX70*(-HKX13*P(1,4) - HKX15*P(4,17) + HKX17*P(0,4) + HKX19*P(4,18) + HKX21*P(4,16) - HKX23*P(3,4) + HKX25*P(2,4) - P(4,19));
|
||||
Kfusion(5) = HKX70*(-HKX13*P(1,5) - HKX15*P(5,17) + HKX17*P(0,5) + HKX19*P(5,18) + HKX21*P(5,16) - HKX23*P(3,5) + HKX25*P(2,5) - P(5,19));
|
||||
Kfusion(6) = HKX70*(-HKX13*P(1,6) - HKX15*P(6,17) + HKX17*P(0,6) + HKX19*P(6,18) + HKX21*P(6,16) - HKX23*P(3,6) + HKX25*P(2,6) - P(6,19));
|
||||
Kfusion(7) = HKX70*(-HKX13*P(1,7) - HKX15*P(7,17) + HKX17*P(0,7) + HKX19*P(7,18) + HKX21*P(7,16) - HKX23*P(3,7) + HKX25*P(2,7) - P(7,19));
|
||||
Kfusion(8) = HKX70*(-HKX13*P(1,8) - HKX15*P(8,17) + HKX17*P(0,8) + HKX19*P(8,18) + HKX21*P(8,16) - HKX23*P(3,8) + HKX25*P(2,8) - P(8,19));
|
||||
Kfusion(9) = HKX70*(-HKX13*P(1,9) - HKX15*P(9,17) + HKX17*P(0,9) + HKX19*P(9,18) + HKX21*P(9,16) - HKX23*P(3,9) + HKX25*P(2,9) - P(9,19));
|
||||
Kfusion(10) = HKX70*(-HKX13*P(1,10) - HKX15*P(10,17) + HKX17*P(0,10) + HKX19*P(10,18) + HKX21*P(10,16) - HKX23*P(3,10) + HKX25*P(2,10) - P(10,19));
|
||||
Kfusion(11) = HKX70*(-HKX13*P(1,11) - HKX15*P(11,17) + HKX17*P(0,11) + HKX19*P(11,18) + HKX21*P(11,16) - HKX23*P(3,11) + HKX25*P(2,11) - P(11,19));
|
||||
Kfusion(12) = HKX70*(-HKX13*P(1,12) - HKX15*P(12,17) + HKX17*P(0,12) + HKX19*P(12,18) + HKX21*P(12,16) - HKX23*P(3,12) + HKX25*P(2,12) - P(12,19));
|
||||
Kfusion(13) = HKX70*(-HKX13*P(1,13) - HKX15*P(13,17) + HKX17*P(0,13) + HKX19*P(13,18) + HKX21*P(13,16) - HKX23*P(3,13) + HKX25*P(2,13) - P(13,19));
|
||||
Kfusion(14) = HKX70*(-HKX13*P(1,14) - HKX15*P(14,17) + HKX17*P(0,14) + HKX19*P(14,18) + HKX21*P(14,16) - HKX23*P(3,14) + HKX25*P(2,14) - P(14,19));
|
||||
Kfusion(15) = HKX70*(-HKX13*P(1,15) - HKX15*P(15,17) + HKX17*P(0,15) + HKX19*P(15,18) + HKX21*P(15,16) - HKX23*P(3,15) + HKX25*P(2,15) - P(15,19));
|
||||
Kfusion(16) = HKX70*(-HKX27 - HKX28 + HKX29 + HKX30 - HKX31 + HKX32 + HKX33 - P(16,19));
|
||||
Kfusion(17) = HKX70*(-HKX41 - HKX42 + HKX43 + HKX44 - HKX45 + HKX46 + HKX47 - P(17,19));
|
||||
Kfusion(18) = HKX70*(-HKX55 - HKX56 + HKX57 + HKX58 - HKX59 + HKX60 + HKX61 - P(18,19));
|
||||
Kfusion(19) = HKX69*HKX70;
|
||||
Kfusion(20) = HKX70*(-HKX13*P(1,20) - HKX15*P(17,20) + HKX17*P(0,20) + HKX19*P(18,20) + HKX21*P(16,20) - HKX23*P(3,20) + HKX25*P(2,20) - P(19,20));
|
||||
Kfusion(21) = HKX70*(-HKX13*P(1,21) - HKX15*P(17,21) + HKX17*P(0,21) + HKX19*P(18,21) + HKX21*P(16,21) - HKX23*P(3,21) + HKX25*P(2,21) - P(19,21));
|
||||
Kfusion(22) = HKX70*(-HKX13*P(1,22) - HKX15*P(17,22) + HKX17*P(0,22) + HKX19*P(18,22) + HKX21*P(16,22) - HKX23*P(3,22) + HKX25*P(2,22) - P(19,22));
|
||||
Kfusion(23) = HKX70*(-HKX13*P(1,23) - HKX15*P(17,23) + HKX17*P(0,23) + HKX19*P(18,23) + HKX21*P(16,23) - HKX23*P(3,23) + HKX25*P(2,23) - P(19,23));
|
||||
|
||||
|
||||
// Axis 1 equations
|
||||
// Sub Expressions
|
||||
const float HKY0 = magD*q1 - magN*q3;
|
||||
const float HKY1 = 2*magE;
|
||||
const float HKY2 = -HKY1*q1 + magD*q0 + magN*q2;
|
||||
const float HKY3 = magD*q3 + magN*q1;
|
||||
const float HKY4 = magD*q2;
|
||||
const float HKY5 = HKY1*q3;
|
||||
const float HKY6 = magN*q0;
|
||||
const float HKY7 = q1*q2;
|
||||
const float HKY8 = q0*q3;
|
||||
const float HKY9 = 2*powf(q1, 2);
|
||||
const float HKY10 = 2*powf(q3, 2);
|
||||
const float HKY11 = q0*q1 + q2*q3;
|
||||
const float HKY12 = 2*HKY11;
|
||||
const float HKY13 = 2*HKY3;
|
||||
const float HKY14 = 2*HKY0;
|
||||
const float HKY15 = -2*HKY7 + 2*HKY8;
|
||||
const float HKY16 = 2*HKY2;
|
||||
const float HKY17 = HKY10 + HKY9 - 1;
|
||||
const float HKY18 = -2*HKY4 + 2*HKY5 + 2*HKY6;
|
||||
const float HKY19 = HKY12*P(0,18) + HKY13*P(0,2) + HKY14*P(0,0) - HKY15*P(0,16) + HKY16*P(0,1) - HKY17*P(0,17) - HKY18*P(0,3) + P(0,20);
|
||||
const float HKY20 = HKY12*P(17,18) + HKY13*P(2,17) + HKY14*P(0,17) - HKY15*P(16,17) + HKY16*P(1,17) - HKY17*P(17,17) - HKY18*P(3,17) + P(17,20);
|
||||
const float HKY21 = HKY12*P(16,18) + HKY13*P(2,16) + HKY14*P(0,16) - HKY15*P(16,16) + HKY16*P(1,16) - HKY17*P(16,17) - HKY18*P(3,16) + P(16,20);
|
||||
const float HKY22 = HKY12*P(3,18) + HKY13*P(2,3) + HKY14*P(0,3) - HKY15*P(3,16) + HKY16*P(1,3) - HKY17*P(3,17) - HKY18*P(3,3) + P(3,20);
|
||||
const float HKY23 = HKY12*P(2,18) + HKY13*P(2,2) + HKY14*P(0,2) - HKY15*P(2,16) + HKY16*P(1,2) - HKY17*P(2,17) - HKY18*P(2,3) + P(2,20);
|
||||
const float HKY24 = HKY12*P(18,18) + HKY13*P(2,18) + HKY14*P(0,18) - HKY15*P(16,18) + HKY16*P(1,18) - HKY17*P(17,18) - HKY18*P(3,18) + P(18,20);
|
||||
const float HKY25 = HKY12*P(1,18) + HKY13*P(1,2) + HKY14*P(0,1) - HKY15*P(1,16) + HKY16*P(1,1) - HKY17*P(1,17) - HKY18*P(1,3) + P(1,20);
|
||||
const float HKY26 = HKY12*P(18,20) + HKY13*P(2,20) + HKY14*P(0,20) - HKY15*P(16,20) + HKY16*P(1,20) - HKY17*P(17,20) - HKY18*P(3,20) + P(20,20);
|
||||
const float HKY27 = 1.0F/(HKY12*HKY24 + HKY13*HKY23 + HKY14*HKY19 - HKY15*HKY21 + HKY16*HKY25 - HKY17*HKY20 - HKY18*HKY22 + HKY26 + R_MAG);
|
||||
|
||||
|
||||
// Observation Jacobians
|
||||
Hfusion.at<0>() = 2*HKY0;
|
||||
Hfusion.at<1>() = 2*HKY2;
|
||||
Hfusion.at<2>() = 2*HKY3;
|
||||
Hfusion.at<3>() = 2*HKY4 - 2*HKY5 - 2*HKY6;
|
||||
Hfusion.at<4>() = 0;
|
||||
Hfusion.at<5>() = 0;
|
||||
Hfusion.at<6>() = 0;
|
||||
Hfusion.at<7>() = 0;
|
||||
Hfusion.at<8>() = 0;
|
||||
Hfusion.at<9>() = 0;
|
||||
Hfusion.at<10>() = 0;
|
||||
Hfusion.at<11>() = 0;
|
||||
Hfusion.at<12>() = 0;
|
||||
Hfusion.at<13>() = 0;
|
||||
Hfusion.at<14>() = 0;
|
||||
Hfusion.at<15>() = 0;
|
||||
Hfusion.at<16>() = 2*HKY7 - 2*HKY8;
|
||||
Hfusion.at<17>() = -HKY10 - HKY9 + 1;
|
||||
Hfusion.at<18>() = 2*HKY11;
|
||||
Hfusion.at<19>() = 0;
|
||||
Hfusion.at<20>() = 1;
|
||||
Hfusion.at<21>() = 0;
|
||||
Hfusion.at<22>() = 0;
|
||||
Hfusion.at<23>() = 0;
|
||||
|
||||
|
||||
// Kalman gains
|
||||
Kfusion(0) = HKY19*HKY27;
|
||||
Kfusion(1) = HKY25*HKY27;
|
||||
Kfusion(2) = HKY23*HKY27;
|
||||
Kfusion(3) = HKY22*HKY27;
|
||||
Kfusion(4) = HKY27*(HKY12*P(4,18) + HKY13*P(2,4) + HKY14*P(0,4) - HKY15*P(4,16) + HKY16*P(1,4) - HKY17*P(4,17) - HKY18*P(3,4) + P(4,20));
|
||||
Kfusion(5) = HKY27*(HKY12*P(5,18) + HKY13*P(2,5) + HKY14*P(0,5) - HKY15*P(5,16) + HKY16*P(1,5) - HKY17*P(5,17) - HKY18*P(3,5) + P(5,20));
|
||||
Kfusion(6) = HKY27*(HKY12*P(6,18) + HKY13*P(2,6) + HKY14*P(0,6) - HKY15*P(6,16) + HKY16*P(1,6) - HKY17*P(6,17) - HKY18*P(3,6) + P(6,20));
|
||||
Kfusion(7) = HKY27*(HKY12*P(7,18) + HKY13*P(2,7) + HKY14*P(0,7) - HKY15*P(7,16) + HKY16*P(1,7) - HKY17*P(7,17) - HKY18*P(3,7) + P(7,20));
|
||||
Kfusion(8) = HKY27*(HKY12*P(8,18) + HKY13*P(2,8) + HKY14*P(0,8) - HKY15*P(8,16) + HKY16*P(1,8) - HKY17*P(8,17) - HKY18*P(3,8) + P(8,20));
|
||||
Kfusion(9) = HKY27*(HKY12*P(9,18) + HKY13*P(2,9) + HKY14*P(0,9) - HKY15*P(9,16) + HKY16*P(1,9) - HKY17*P(9,17) - HKY18*P(3,9) + P(9,20));
|
||||
Kfusion(10) = HKY27*(HKY12*P(10,18) + HKY13*P(2,10) + HKY14*P(0,10) - HKY15*P(10,16) + HKY16*P(1,10) - HKY17*P(10,17) - HKY18*P(3,10) + P(10,20));
|
||||
Kfusion(11) = HKY27*(HKY12*P(11,18) + HKY13*P(2,11) + HKY14*P(0,11) - HKY15*P(11,16) + HKY16*P(1,11) - HKY17*P(11,17) - HKY18*P(3,11) + P(11,20));
|
||||
Kfusion(12) = HKY27*(HKY12*P(12,18) + HKY13*P(2,12) + HKY14*P(0,12) - HKY15*P(12,16) + HKY16*P(1,12) - HKY17*P(12,17) - HKY18*P(3,12) + P(12,20));
|
||||
Kfusion(13) = HKY27*(HKY12*P(13,18) + HKY13*P(2,13) + HKY14*P(0,13) - HKY15*P(13,16) + HKY16*P(1,13) - HKY17*P(13,17) - HKY18*P(3,13) + P(13,20));
|
||||
Kfusion(14) = HKY27*(HKY12*P(14,18) + HKY13*P(2,14) + HKY14*P(0,14) - HKY15*P(14,16) + HKY16*P(1,14) - HKY17*P(14,17) - HKY18*P(3,14) + P(14,20));
|
||||
Kfusion(15) = HKY27*(HKY12*P(15,18) + HKY13*P(2,15) + HKY14*P(0,15) - HKY15*P(15,16) + HKY16*P(1,15) - HKY17*P(15,17) - HKY18*P(3,15) + P(15,20));
|
||||
Kfusion(16) = HKY21*HKY27;
|
||||
Kfusion(17) = HKY20*HKY27;
|
||||
Kfusion(18) = HKY24*HKY27;
|
||||
Kfusion(19) = HKY27*(HKY12*P(18,19) + HKY13*P(2,19) + HKY14*P(0,19) - HKY15*P(16,19) + HKY16*P(1,19) - HKY17*P(17,19) - HKY18*P(3,19) + P(19,20));
|
||||
Kfusion(20) = HKY26*HKY27;
|
||||
Kfusion(21) = HKY27*(HKY12*P(18,21) + HKY13*P(2,21) + HKY14*P(0,21) - HKY15*P(16,21) + HKY16*P(1,21) - HKY17*P(17,21) - HKY18*P(3,21) + P(20,21));
|
||||
Kfusion(22) = HKY27*(HKY12*P(18,22) + HKY13*P(2,22) + HKY14*P(0,22) - HKY15*P(16,22) + HKY16*P(1,22) - HKY17*P(17,22) - HKY18*P(3,22) + P(20,22));
|
||||
Kfusion(23) = HKY27*(HKY12*P(18,23) + HKY13*P(2,23) + HKY14*P(0,23) - HKY15*P(16,23) + HKY16*P(1,23) - HKY17*P(17,23) - HKY18*P(3,23) + P(20,23));
|
||||
|
||||
|
||||
// Axis 2 equations
|
||||
// Sub Expressions
|
||||
const float HKZ0 = magN*q2;
|
||||
const float HKZ1 = magE*q1;
|
||||
const float HKZ2 = magN*q3;
|
||||
const float HKZ3 = 2*magD;
|
||||
const float HKZ4 = HKZ3*q1;
|
||||
const float HKZ5 = magE*q0;
|
||||
const float HKZ6 = -HKZ3*q2 + magE*q3 + magN*q0;
|
||||
const float HKZ7 = magE*q2 + magN*q1;
|
||||
const float HKZ8 = q0*q2 + q1*q3;
|
||||
const float HKZ9 = q2*q3;
|
||||
const float HKZ10 = q0*q1;
|
||||
const float HKZ11 = 2*powf(q1, 2);
|
||||
const float HKZ12 = 2*powf(q2, 2);
|
||||
const float HKZ13 = 2*HKZ8;
|
||||
const float HKZ14 = HKZ13*P(0,16);
|
||||
const float HKZ15 = 2*HKZ7;
|
||||
const float HKZ16 = HKZ15*P(0,3);
|
||||
const float HKZ17 = -2*HKZ0 + 2*HKZ1;
|
||||
const float HKZ18 = HKZ17*P(0,0);
|
||||
const float HKZ19 = 2*HKZ10 - 2*HKZ9;
|
||||
const float HKZ20 = HKZ19*P(0,17);
|
||||
const float HKZ21 = HKZ11 + HKZ12 - 1;
|
||||
const float HKZ22 = HKZ21*P(0,18);
|
||||
const float HKZ23 = 2*HKZ6;
|
||||
const float HKZ24 = HKZ23*P(0,2);
|
||||
const float HKZ25 = -2*HKZ2 + 2*HKZ4 + 2*HKZ5;
|
||||
const float HKZ26 = HKZ25*P(0,1);
|
||||
const float HKZ27 = HKZ13*P(16,18);
|
||||
const float HKZ28 = HKZ15*P(3,18);
|
||||
const float HKZ29 = HKZ17*P(0,18);
|
||||
const float HKZ30 = HKZ19*P(17,18);
|
||||
const float HKZ31 = HKZ23*P(2,18);
|
||||
const float HKZ32 = HKZ25*P(1,18);
|
||||
const float HKZ33 = HKZ21*P(18,18);
|
||||
const float HKZ34 = HKZ13*P(3,16);
|
||||
const float HKZ35 = HKZ15*P(3,3);
|
||||
const float HKZ36 = HKZ17*P(0,3);
|
||||
const float HKZ37 = HKZ19*P(3,17);
|
||||
const float HKZ38 = HKZ23*P(2,3);
|
||||
const float HKZ39 = HKZ25*P(1,3);
|
||||
const float HKZ40 = HKZ21*P(3,18);
|
||||
const float HKZ41 = HKZ13*P(16,16);
|
||||
const float HKZ42 = HKZ15*P(3,16);
|
||||
const float HKZ43 = HKZ17*P(0,16);
|
||||
const float HKZ44 = HKZ19*P(16,17);
|
||||
const float HKZ45 = HKZ23*P(2,16);
|
||||
const float HKZ46 = HKZ25*P(1,16);
|
||||
const float HKZ47 = HKZ21*P(16,18);
|
||||
const float HKZ48 = HKZ13*P(2,16);
|
||||
const float HKZ49 = HKZ15*P(2,3);
|
||||
const float HKZ50 = HKZ17*P(0,2);
|
||||
const float HKZ51 = HKZ19*P(2,17);
|
||||
const float HKZ52 = HKZ23*P(2,2);
|
||||
const float HKZ53 = HKZ25*P(1,2);
|
||||
const float HKZ54 = HKZ21*P(2,18);
|
||||
const float HKZ55 = HKZ13*P(16,17);
|
||||
const float HKZ56 = HKZ15*P(3,17);
|
||||
const float HKZ57 = HKZ17*P(0,17);
|
||||
const float HKZ58 = HKZ19*P(17,17);
|
||||
const float HKZ59 = HKZ23*P(2,17);
|
||||
const float HKZ60 = HKZ25*P(1,17);
|
||||
const float HKZ61 = HKZ21*P(17,18);
|
||||
const float HKZ62 = HKZ13*P(1,16);
|
||||
const float HKZ63 = HKZ15*P(1,3);
|
||||
const float HKZ64 = HKZ17*P(0,1);
|
||||
const float HKZ65 = HKZ19*P(1,17);
|
||||
const float HKZ66 = HKZ23*P(1,2);
|
||||
const float HKZ67 = HKZ25*P(1,1);
|
||||
const float HKZ68 = HKZ21*P(1,18);
|
||||
const float HKZ69 = -HKZ13*P(16,21) - HKZ15*P(3,21) + HKZ17*P(0,21) + HKZ19*P(17,21) + HKZ21*P(18,21) - HKZ23*P(2,21) + HKZ25*P(1,21) - P(21,21);
|
||||
const float HKZ70 = 1.0F/(-HKZ13*(HKZ41 + HKZ42 - HKZ43 - HKZ44 + HKZ45 - HKZ46 - HKZ47 + P(16,21)) - HKZ15*(HKZ34 + HKZ35 - HKZ36 - HKZ37 + HKZ38 - HKZ39 - HKZ40 + P(3,21)) + HKZ17*(HKZ14 + HKZ16 - HKZ18 - HKZ20 - HKZ22 + HKZ24 - HKZ26 + P(0,21)) + HKZ19*(HKZ55 + HKZ56 - HKZ57 - HKZ58 + HKZ59 - HKZ60 - HKZ61 + P(17,21)) + HKZ21*(HKZ27 + HKZ28 - HKZ29 - HKZ30 + HKZ31 - HKZ32 - HKZ33 + P(18,21)) - HKZ23*(HKZ48 + HKZ49 - HKZ50 - HKZ51 + HKZ52 - HKZ53 - HKZ54 + P(2,21)) + HKZ25*(HKZ62 + HKZ63 - HKZ64 - HKZ65 + HKZ66 - HKZ67 - HKZ68 + P(1,21)) + HKZ69 - R_MAG);
|
||||
|
||||
|
||||
// Observation Jacobians
|
||||
Hfusion.at<0>() = 2*HKZ0 - 2*HKZ1;
|
||||
Hfusion.at<1>() = 2*HKZ2 - 2*HKZ4 - 2*HKZ5;
|
||||
Hfusion.at<2>() = 2*HKZ6;
|
||||
Hfusion.at<3>() = 2*HKZ7;
|
||||
Hfusion.at<4>() = 0;
|
||||
Hfusion.at<5>() = 0;
|
||||
Hfusion.at<6>() = 0;
|
||||
Hfusion.at<7>() = 0;
|
||||
Hfusion.at<8>() = 0;
|
||||
Hfusion.at<9>() = 0;
|
||||
Hfusion.at<10>() = 0;
|
||||
Hfusion.at<11>() = 0;
|
||||
Hfusion.at<12>() = 0;
|
||||
Hfusion.at<13>() = 0;
|
||||
Hfusion.at<14>() = 0;
|
||||
Hfusion.at<15>() = 0;
|
||||
Hfusion.at<16>() = 2*HKZ8;
|
||||
Hfusion.at<17>() = -2*HKZ10 + 2*HKZ9;
|
||||
Hfusion.at<18>() = -HKZ11 - HKZ12 + 1;
|
||||
Hfusion.at<19>() = 0;
|
||||
Hfusion.at<20>() = 0;
|
||||
Hfusion.at<21>() = 1;
|
||||
Hfusion.at<22>() = 0;
|
||||
Hfusion.at<23>() = 0;
|
||||
|
||||
|
||||
// Kalman gains
|
||||
Kfusion(0) = HKZ70*(-HKZ14 - HKZ16 + HKZ18 + HKZ20 + HKZ22 - HKZ24 + HKZ26 - P(0,21));
|
||||
Kfusion(1) = HKZ70*(-HKZ62 - HKZ63 + HKZ64 + HKZ65 - HKZ66 + HKZ67 + HKZ68 - P(1,21));
|
||||
Kfusion(2) = HKZ70*(-HKZ48 - HKZ49 + HKZ50 + HKZ51 - HKZ52 + HKZ53 + HKZ54 - P(2,21));
|
||||
Kfusion(3) = HKZ70*(-HKZ34 - HKZ35 + HKZ36 + HKZ37 - HKZ38 + HKZ39 + HKZ40 - P(3,21));
|
||||
Kfusion(4) = HKZ70*(-HKZ13*P(4,16) - HKZ15*P(3,4) + HKZ17*P(0,4) + HKZ19*P(4,17) + HKZ21*P(4,18) - HKZ23*P(2,4) + HKZ25*P(1,4) - P(4,21));
|
||||
Kfusion(5) = HKZ70*(-HKZ13*P(5,16) - HKZ15*P(3,5) + HKZ17*P(0,5) + HKZ19*P(5,17) + HKZ21*P(5,18) - HKZ23*P(2,5) + HKZ25*P(1,5) - P(5,21));
|
||||
Kfusion(6) = HKZ70*(-HKZ13*P(6,16) - HKZ15*P(3,6) + HKZ17*P(0,6) + HKZ19*P(6,17) + HKZ21*P(6,18) - HKZ23*P(2,6) + HKZ25*P(1,6) - P(6,21));
|
||||
Kfusion(7) = HKZ70*(-HKZ13*P(7,16) - HKZ15*P(3,7) + HKZ17*P(0,7) + HKZ19*P(7,17) + HKZ21*P(7,18) - HKZ23*P(2,7) + HKZ25*P(1,7) - P(7,21));
|
||||
Kfusion(8) = HKZ70*(-HKZ13*P(8,16) - HKZ15*P(3,8) + HKZ17*P(0,8) + HKZ19*P(8,17) + HKZ21*P(8,18) - HKZ23*P(2,8) + HKZ25*P(1,8) - P(8,21));
|
||||
Kfusion(9) = HKZ70*(-HKZ13*P(9,16) - HKZ15*P(3,9) + HKZ17*P(0,9) + HKZ19*P(9,17) + HKZ21*P(9,18) - HKZ23*P(2,9) + HKZ25*P(1,9) - P(9,21));
|
||||
Kfusion(10) = HKZ70*(-HKZ13*P(10,16) - HKZ15*P(3,10) + HKZ17*P(0,10) + HKZ19*P(10,17) + HKZ21*P(10,18) - HKZ23*P(2,10) + HKZ25*P(1,10) - P(10,21));
|
||||
Kfusion(11) = HKZ70*(-HKZ13*P(11,16) - HKZ15*P(3,11) + HKZ17*P(0,11) + HKZ19*P(11,17) + HKZ21*P(11,18) - HKZ23*P(2,11) + HKZ25*P(1,11) - P(11,21));
|
||||
Kfusion(12) = HKZ70*(-HKZ13*P(12,16) - HKZ15*P(3,12) + HKZ17*P(0,12) + HKZ19*P(12,17) + HKZ21*P(12,18) - HKZ23*P(2,12) + HKZ25*P(1,12) - P(12,21));
|
||||
Kfusion(13) = HKZ70*(-HKZ13*P(13,16) - HKZ15*P(3,13) + HKZ17*P(0,13) + HKZ19*P(13,17) + HKZ21*P(13,18) - HKZ23*P(2,13) + HKZ25*P(1,13) - P(13,21));
|
||||
Kfusion(14) = HKZ70*(-HKZ13*P(14,16) - HKZ15*P(3,14) + HKZ17*P(0,14) + HKZ19*P(14,17) + HKZ21*P(14,18) - HKZ23*P(2,14) + HKZ25*P(1,14) - P(14,21));
|
||||
Kfusion(15) = HKZ70*(-HKZ13*P(15,16) - HKZ15*P(3,15) + HKZ17*P(0,15) + HKZ19*P(15,17) + HKZ21*P(15,18) - HKZ23*P(2,15) + HKZ25*P(1,15) - P(15,21));
|
||||
Kfusion(16) = HKZ70*(-HKZ41 - HKZ42 + HKZ43 + HKZ44 - HKZ45 + HKZ46 + HKZ47 - P(16,21));
|
||||
Kfusion(17) = HKZ70*(-HKZ55 - HKZ56 + HKZ57 + HKZ58 - HKZ59 + HKZ60 + HKZ61 - P(17,21));
|
||||
Kfusion(18) = HKZ70*(-HKZ27 - HKZ28 + HKZ29 + HKZ30 - HKZ31 + HKZ32 + HKZ33 - P(18,21));
|
||||
Kfusion(19) = HKZ70*(-HKZ13*P(16,19) - HKZ15*P(3,19) + HKZ17*P(0,19) + HKZ19*P(17,19) + HKZ21*P(18,19) - HKZ23*P(2,19) + HKZ25*P(1,19) - P(19,21));
|
||||
Kfusion(20) = HKZ70*(-HKZ13*P(16,20) - HKZ15*P(3,20) + HKZ17*P(0,20) + HKZ19*P(17,20) + HKZ21*P(18,20) - HKZ23*P(2,20) + HKZ25*P(1,20) - P(20,21));
|
||||
Kfusion(21) = HKZ69*HKZ70;
|
||||
Kfusion(22) = HKZ70*(-HKZ13*P(16,22) - HKZ15*P(3,22) + HKZ17*P(0,22) + HKZ19*P(17,22) + HKZ21*P(18,22) - HKZ23*P(2,22) + HKZ25*P(1,22) - P(21,22));
|
||||
Kfusion(23) = HKZ70*(-HKZ13*P(16,23) - HKZ15*P(3,23) + HKZ17*P(0,23) + HKZ19*P(17,23) + HKZ21*P(18,23) - HKZ23*P(2,23) + HKZ25*P(1,23) - P(21,23));
|
||||
|
||||
|
||||
@@ -0,0 +1,340 @@
|
||||
// Sub Expressions
|
||||
const float HK0 = magE*q3;
|
||||
const float HK1 = magD*q2;
|
||||
const float HK2 = -HK1;
|
||||
const float HK3 = magD*q3;
|
||||
const float HK4 = magE*q2;
|
||||
const float HK5 = HK3 + HK4;
|
||||
const float HK6 = magE*q1;
|
||||
const float HK7 = magD*q0;
|
||||
const float HK8 = magN*q2;
|
||||
const float HK9 = 2*HK8;
|
||||
const float HK10 = magD*q1;
|
||||
const float HK11 = magE*q0;
|
||||
const float HK12 = magN*q3;
|
||||
const float HK13 = HK10 + HK11 - 2*HK12;
|
||||
const float HK14 = 2*powf(q2, 2);
|
||||
const float HK15 = -HK14;
|
||||
const float HK16 = 2*powf(q3, 2);
|
||||
const float HK17 = 1 - HK16;
|
||||
const float HK18 = q0*q3;
|
||||
const float HK19 = q1*q2;
|
||||
const float HK20 = HK18 + HK19;
|
||||
const float HK21 = q1*q3;
|
||||
const float HK22 = q0*q2;
|
||||
const float HK23 = 2*HK5;
|
||||
const float HK24 = HK23*P(0,1);
|
||||
const float HK25 = 2*HK20;
|
||||
const float HK26 = HK25*P(0,17);
|
||||
const float HK27 = -2*HK0 + 2*HK1;
|
||||
const float HK28 = HK27*P(0,0);
|
||||
const float HK29 = -2*HK21 + 2*HK22;
|
||||
const float HK30 = HK29*P(0,18);
|
||||
const float HK31 = HK16 - 1;
|
||||
const float HK32 = HK14 + HK31;
|
||||
const float HK33 = HK32*P(0,16);
|
||||
const float HK34 = 2*HK13;
|
||||
const float HK35 = HK34*P(0,3);
|
||||
const float HK36 = -HK6;
|
||||
const float HK37 = 2*HK36 + 2*HK7 + 2*HK9;
|
||||
const float HK38 = HK37*P(0,2);
|
||||
const float HK39 = -R_MAG;
|
||||
const float HK40 = HK23*P(1,16);
|
||||
const float HK41 = HK25*P(16,17);
|
||||
const float HK42 = HK27*P(0,16);
|
||||
const float HK43 = HK29*P(16,18);
|
||||
const float HK44 = HK34*P(3,16);
|
||||
const float HK45 = HK32*P(16,16);
|
||||
const float HK46 = HK37*P(2,16);
|
||||
const float HK47 = HK23*P(1,1);
|
||||
const float HK48 = HK25*P(1,17);
|
||||
const float HK49 = HK27*P(0,1);
|
||||
const float HK50 = HK29*P(1,18);
|
||||
const float HK51 = HK34*P(1,3);
|
||||
const float HK52 = HK32*P(1,16);
|
||||
const float HK53 = HK37*P(1,2);
|
||||
const float HK54 = HK23*P(1,17);
|
||||
const float HK55 = HK25*P(17,17);
|
||||
const float HK56 = HK27*P(0,17);
|
||||
const float HK57 = HK29*P(17,18);
|
||||
const float HK58 = HK34*P(3,17);
|
||||
const float HK59 = HK32*P(16,17);
|
||||
const float HK60 = HK37*P(2,17);
|
||||
const float HK61 = HK23*P(1,3);
|
||||
const float HK62 = HK25*P(3,17);
|
||||
const float HK63 = HK27*P(0,3);
|
||||
const float HK64 = HK29*P(3,18);
|
||||
const float HK65 = HK34*P(3,3);
|
||||
const float HK66 = HK37*P(2,3);
|
||||
const float HK67 = HK32*P(3,16);
|
||||
const float HK68 = HK23*P(1,18);
|
||||
const float HK69 = HK25*P(17,18);
|
||||
const float HK70 = HK27*P(0,18);
|
||||
const float HK71 = HK29*P(18,18);
|
||||
const float HK72 = HK34*P(3,18);
|
||||
const float HK73 = HK32*P(16,18);
|
||||
const float HK74 = HK37*P(2,18);
|
||||
const float HK75 = HK23*P(1,2);
|
||||
const float HK76 = HK25*P(2,17);
|
||||
const float HK77 = HK27*P(0,2);
|
||||
const float HK78 = HK29*P(2,18);
|
||||
const float HK79 = HK34*P(2,3);
|
||||
const float HK80 = HK32*P(2,16);
|
||||
const float HK81 = HK37*P(2,2);
|
||||
const float HK82 = -HK23*P(1,19) - HK25*P(17,19) + HK27*P(0,19) + HK29*P(18,19) + HK32*P(16,19) - HK34*P(3,19) + HK37*P(2,19) - P(19,19);
|
||||
const float HK83 = 1.0F/(-HK23*(HK47 + HK48 - HK49 - HK50 + HK51 - HK52 - HK53 + P(1,19)) - HK25*(HK54 + HK55 - HK56 - HK57 + HK58 - HK59 - HK60 + P(17,19)) + HK27*(HK24 + HK26 - HK28 - HK30 - HK33 + HK35 - HK38 + P(0,19)) + HK29*(HK68 + HK69 - HK70 - HK71 + HK72 - HK73 - HK74 + P(18,19)) + HK32*(HK40 + HK41 - HK42 - HK43 + HK44 - HK45 - HK46 + P(16,19)) - HK34*(HK61 + HK62 - HK63 - HK64 + HK65 - HK66 - HK67 + P(3,19)) + HK37*(HK75 + HK76 - HK77 - HK78 + HK79 - HK80 - HK81 + P(2,19)) + HK39 + HK82);
|
||||
const float HK84 = -P(19,21);
|
||||
const float HK85 = -HK12;
|
||||
const float HK86 = HK10 + HK85;
|
||||
const float HK87 = -2*HK6 + HK7 + HK8;
|
||||
const float HK88 = magN*q1;
|
||||
const float HK89 = HK3 + HK88;
|
||||
const float HK90 = 2*HK0;
|
||||
const float HK91 = magN*q0;
|
||||
const float HK92 = 2*powf(q1, 2);
|
||||
const float HK93 = -HK92;
|
||||
const float HK94 = q0*q1;
|
||||
const float HK95 = q2*q3;
|
||||
const float HK96 = HK94 + HK95;
|
||||
const float HK97 = 2*HK96;
|
||||
const float HK98 = 2*HK89;
|
||||
const float HK99 = 2*HK86;
|
||||
const float HK100 = 2*HK18 - 2*HK19;
|
||||
const float HK101 = 2*HK87;
|
||||
const float HK102 = HK31 + HK92;
|
||||
const float HK103 = 2*HK2 + 2*HK90 + 2*HK91;
|
||||
const float HK104 = -HK100*P(0,16) + HK101*P(0,1) - HK102*P(0,17) - HK103*P(0,3) + HK97*P(0,18) + HK98*P(0,2) + HK99*P(0,0) + P(0,20);
|
||||
const float HK105 = -HK100*P(16,17) + HK101*P(1,17) - HK102*P(17,17) - HK103*P(3,17) + HK97*P(17,18) + HK98*P(2,17) + HK99*P(0,17) + P(17,20);
|
||||
const float HK106 = -HK100*P(16,16) + HK101*P(1,16) - HK102*P(16,17) - HK103*P(3,16) + HK97*P(16,18) + HK98*P(2,16) + HK99*P(0,16) + P(16,20);
|
||||
const float HK107 = -HK100*P(3,16) + HK101*P(1,3) - HK102*P(3,17) - HK103*P(3,3) + HK97*P(3,18) + HK98*P(2,3) + HK99*P(0,3) + P(3,20);
|
||||
const float HK108 = -HK100*P(2,16) + HK101*P(1,2) - HK102*P(2,17) - HK103*P(2,3) + HK97*P(2,18) + HK98*P(2,2) + HK99*P(0,2) + P(2,20);
|
||||
const float HK109 = -HK100*P(16,18) + HK101*P(1,18) - HK102*P(17,18) - HK103*P(3,18) + HK97*P(18,18) + HK98*P(2,18) + HK99*P(0,18) + P(18,20);
|
||||
const float HK110 = -HK100*P(1,16) + HK101*P(1,1) - HK102*P(1,17) - HK103*P(1,3) + HK97*P(1,18) + HK98*P(1,2) + HK99*P(0,1) + P(1,20);
|
||||
const float HK111 = -HK100*P(16,20) + HK101*P(1,20) - HK102*P(17,20) - HK103*P(3,20) + HK97*P(18,20) + HK98*P(2,20) + HK99*P(0,20) + P(20,20);
|
||||
const float HK112 = 1.0F/(-HK100*HK106 + HK101*HK110 - HK102*HK105 - HK103*HK107 + HK104*HK99 + HK108*HK98 + HK109*HK97 + HK111 + R_MAG);
|
||||
const float HK113 = 2*HK10;
|
||||
const float HK114 = HK0 - 2*HK1 + HK91;
|
||||
const float HK115 = HK4 + HK88;
|
||||
const float HK116 = HK21 + HK22;
|
||||
const float HK117 = 2*HK116;
|
||||
const float HK118 = HK117*P(0,16);
|
||||
const float HK119 = 2*HK115;
|
||||
const float HK120 = HK119*P(0,3);
|
||||
const float HK121 = 2*HK6 - 2*HK8;
|
||||
const float HK122 = HK121*P(0,0);
|
||||
const float HK123 = 2*HK94 - 2*HK95;
|
||||
const float HK124 = HK123*P(0,17);
|
||||
const float HK125 = HK14 + HK92 - 1;
|
||||
const float HK126 = HK125*P(0,18);
|
||||
const float HK127 = 2*HK114;
|
||||
const float HK128 = HK127*P(0,2);
|
||||
const float HK129 = 2*HK11 + 2*HK113 + 2*HK85;
|
||||
const float HK130 = HK129*P(0,1);
|
||||
const float HK131 = HK117*P(16,18);
|
||||
const float HK132 = HK119*P(3,18);
|
||||
const float HK133 = HK121*P(0,18);
|
||||
const float HK134 = HK123*P(17,18);
|
||||
const float HK135 = HK127*P(2,18);
|
||||
const float HK136 = HK129*P(1,18);
|
||||
const float HK137 = HK125*P(18,18);
|
||||
const float HK138 = HK117*P(3,16);
|
||||
const float HK139 = HK119*P(3,3);
|
||||
const float HK140 = HK121*P(0,3);
|
||||
const float HK141 = HK123*P(3,17);
|
||||
const float HK142 = HK127*P(2,3);
|
||||
const float HK143 = HK129*P(1,3);
|
||||
const float HK144 = HK125*P(3,18);
|
||||
const float HK145 = HK117*P(16,16);
|
||||
const float HK146 = HK119*P(3,16);
|
||||
const float HK147 = HK121*P(0,16);
|
||||
const float HK148 = HK123*P(16,17);
|
||||
const float HK149 = HK127*P(2,16);
|
||||
const float HK150 = HK129*P(1,16);
|
||||
const float HK151 = HK125*P(16,18);
|
||||
const float HK152 = HK117*P(2,16);
|
||||
const float HK153 = HK119*P(2,3);
|
||||
const float HK154 = HK121*P(0,2);
|
||||
const float HK155 = HK123*P(2,17);
|
||||
const float HK156 = HK127*P(2,2);
|
||||
const float HK157 = HK129*P(1,2);
|
||||
const float HK158 = HK125*P(2,18);
|
||||
const float HK159 = HK117*P(16,17);
|
||||
const float HK160 = HK119*P(3,17);
|
||||
const float HK161 = HK121*P(0,17);
|
||||
const float HK162 = HK123*P(17,17);
|
||||
const float HK163 = HK127*P(2,17);
|
||||
const float HK164 = HK129*P(1,17);
|
||||
const float HK165 = HK125*P(17,18);
|
||||
const float HK166 = HK117*P(1,16);
|
||||
const float HK167 = HK119*P(1,3);
|
||||
const float HK168 = HK121*P(0,1);
|
||||
const float HK169 = HK123*P(1,17);
|
||||
const float HK170 = HK127*P(1,2);
|
||||
const float HK171 = HK129*P(1,1);
|
||||
const float HK172 = HK125*P(1,18);
|
||||
const float HK173 = -HK117*P(16,21) - HK119*P(3,21) + HK121*P(0,21) + HK123*P(17,21) + HK125*P(18,21) - HK127*P(2,21) + HK129*P(1,21) - P(21,21);
|
||||
const float HK174 = 1.0F/(-HK117*(HK145 + HK146 - HK147 - HK148 + HK149 - HK150 - HK151 + P(16,21)) - HK119*(HK138 + HK139 - HK140 - HK141 + HK142 - HK143 - HK144 + P(3,21)) + HK121*(HK118 + HK120 - HK122 - HK124 - HK126 + HK128 - HK130 + P(0,21)) + HK123*(HK159 + HK160 - HK161 - HK162 + HK163 - HK164 - HK165 + P(17,21)) + HK125*(HK131 + HK132 - HK133 - HK134 + HK135 - HK136 - HK137 + P(18,21)) - HK127*(HK152 + HK153 - HK154 - HK155 + HK156 - HK157 - HK158 + P(2,21)) + HK129*(HK166 + HK167 - HK168 - HK169 + HK170 - HK171 - HK172 + P(1,21)) + HK173 + HK39);
|
||||
|
||||
|
||||
// Observation Jacobians - axis 0
|
||||
Hfusion.at<0>() = 2*HK0 + 2*HK2;
|
||||
Hfusion.at<1>() = 2*HK5;
|
||||
Hfusion.at<2>() = 2*HK6 - 2*HK7 - 2*HK9;
|
||||
Hfusion.at<3>() = 2*HK13;
|
||||
Hfusion.at<4>() = 0;
|
||||
Hfusion.at<5>() = 0;
|
||||
Hfusion.at<6>() = 0;
|
||||
Hfusion.at<7>() = 0;
|
||||
Hfusion.at<8>() = 0;
|
||||
Hfusion.at<9>() = 0;
|
||||
Hfusion.at<10>() = 0;
|
||||
Hfusion.at<11>() = 0;
|
||||
Hfusion.at<12>() = 0;
|
||||
Hfusion.at<13>() = 0;
|
||||
Hfusion.at<14>() = 0;
|
||||
Hfusion.at<15>() = 0;
|
||||
Hfusion.at<16>() = HK15 + HK17;
|
||||
Hfusion.at<17>() = 2*HK20;
|
||||
Hfusion.at<18>() = 2*HK21 - 2*HK22;
|
||||
Hfusion.at<19>() = 1;
|
||||
Hfusion.at<20>() = 0;
|
||||
Hfusion.at<21>() = 0;
|
||||
Hfusion.at<22>() = 0;
|
||||
Hfusion.at<23>() = 0;
|
||||
|
||||
|
||||
// Kalman gains - axis 0
|
||||
Kfusion(0) = HK83*(-HK24 - HK26 + HK28 + HK30 + HK33 - HK35 + HK38 - P(0,19));
|
||||
Kfusion(1) = HK83*(-HK47 - HK48 + HK49 + HK50 - HK51 + HK52 + HK53 - P(1,19));
|
||||
Kfusion(2) = HK83*(-HK75 - HK76 + HK77 + HK78 - HK79 + HK80 + HK81 - P(2,19));
|
||||
Kfusion(3) = HK83*(-HK61 - HK62 + HK63 + HK64 - HK65 + HK66 + HK67 - P(3,19));
|
||||
Kfusion(4) = HK83*(-HK23*P(1,4) - HK25*P(4,17) + HK27*P(0,4) + HK29*P(4,18) + HK32*P(4,16) - HK34*P(3,4) + HK37*P(2,4) - P(4,19));
|
||||
Kfusion(5) = HK83*(-HK23*P(1,5) - HK25*P(5,17) + HK27*P(0,5) + HK29*P(5,18) + HK32*P(5,16) - HK34*P(3,5) + HK37*P(2,5) - P(5,19));
|
||||
Kfusion(6) = HK83*(-HK23*P(1,6) - HK25*P(6,17) + HK27*P(0,6) + HK29*P(6,18) + HK32*P(6,16) - HK34*P(3,6) + HK37*P(2,6) - P(6,19));
|
||||
Kfusion(7) = HK83*(-HK23*P(1,7) - HK25*P(7,17) + HK27*P(0,7) + HK29*P(7,18) + HK32*P(7,16) - HK34*P(3,7) + HK37*P(2,7) - P(7,19));
|
||||
Kfusion(8) = HK83*(-HK23*P(1,8) - HK25*P(8,17) + HK27*P(0,8) + HK29*P(8,18) + HK32*P(8,16) - HK34*P(3,8) + HK37*P(2,8) - P(8,19));
|
||||
Kfusion(9) = HK83*(-HK23*P(1,9) - HK25*P(9,17) + HK27*P(0,9) + HK29*P(9,18) + HK32*P(9,16) - HK34*P(3,9) + HK37*P(2,9) - P(9,19));
|
||||
Kfusion(10) = HK83*(-HK23*P(1,10) - HK25*P(10,17) + HK27*P(0,10) + HK29*P(10,18) + HK32*P(10,16) - HK34*P(3,10) + HK37*P(2,10) - P(10,19));
|
||||
Kfusion(11) = HK83*(-HK23*P(1,11) - HK25*P(11,17) + HK27*P(0,11) + HK29*P(11,18) + HK32*P(11,16) - HK34*P(3,11) + HK37*P(2,11) - P(11,19));
|
||||
Kfusion(12) = HK83*(-HK23*P(1,12) - HK25*P(12,17) + HK27*P(0,12) + HK29*P(12,18) + HK32*P(12,16) - HK34*P(3,12) + HK37*P(2,12) - P(12,19));
|
||||
Kfusion(13) = HK83*(-HK23*P(1,13) - HK25*P(13,17) + HK27*P(0,13) + HK29*P(13,18) + HK32*P(13,16) - HK34*P(3,13) + HK37*P(2,13) - P(13,19));
|
||||
Kfusion(14) = HK83*(-HK23*P(1,14) - HK25*P(14,17) + HK27*P(0,14) + HK29*P(14,18) + HK32*P(14,16) - HK34*P(3,14) + HK37*P(2,14) - P(14,19));
|
||||
Kfusion(15) = HK83*(-HK23*P(1,15) - HK25*P(15,17) + HK27*P(0,15) + HK29*P(15,18) + HK32*P(15,16) - HK34*P(3,15) + HK37*P(2,15) - P(15,19));
|
||||
Kfusion(16) = HK83*(-HK40 - HK41 + HK42 + HK43 - HK44 + HK45 + HK46 - P(16,19));
|
||||
Kfusion(17) = HK83*(-HK54 - HK55 + HK56 + HK57 - HK58 + HK59 + HK60 - P(17,19));
|
||||
Kfusion(18) = HK83*(-HK68 - HK69 + HK70 + HK71 - HK72 + HK73 + HK74 - P(18,19));
|
||||
Kfusion(19) = HK82*HK83;
|
||||
Kfusion(20) = HK83*(-HK23*P(1,20) - HK25*P(17,20) + HK27*P(0,20) + HK29*P(18,20) + HK32*P(16,20) - HK34*P(3,20) + HK37*P(2,20) - P(19,20));
|
||||
Kfusion(21) = HK83*(-HK23*P(1,21) - HK25*P(17,21) + HK27*P(0,21) + HK29*P(18,21) + HK32*P(16,21) - HK34*P(3,21) + HK37*P(2,21) + HK84);
|
||||
Kfusion(22) = HK83*(-HK23*P(1,22) - HK25*P(17,22) + HK27*P(0,22) + HK29*P(18,22) + HK32*P(16,22) - HK34*P(3,22) + HK37*P(2,22) - P(19,22));
|
||||
Kfusion(23) = HK83*(-HK23*P(1,23) - HK25*P(17,23) + HK27*P(0,23) + HK29*P(18,23) + HK32*P(16,23) - HK34*P(3,23) + HK37*P(2,23) - P(19,23));
|
||||
|
||||
|
||||
// Observation Jacobians - axis 1
|
||||
Hfusion.at<0>() = 2*HK86;
|
||||
Hfusion.at<1>() = 2*HK87;
|
||||
Hfusion.at<2>() = 2*HK89;
|
||||
Hfusion.at<3>() = 2*HK1 - 2*HK90 - 2*HK91;
|
||||
Hfusion.at<4>() = 0;
|
||||
Hfusion.at<5>() = 0;
|
||||
Hfusion.at<6>() = 0;
|
||||
Hfusion.at<7>() = 0;
|
||||
Hfusion.at<8>() = 0;
|
||||
Hfusion.at<9>() = 0;
|
||||
Hfusion.at<10>() = 0;
|
||||
Hfusion.at<11>() = 0;
|
||||
Hfusion.at<12>() = 0;
|
||||
Hfusion.at<13>() = 0;
|
||||
Hfusion.at<14>() = 0;
|
||||
Hfusion.at<15>() = 0;
|
||||
Hfusion.at<16>() = -2*HK18 + 2*HK19;
|
||||
Hfusion.at<17>() = HK17 + HK93;
|
||||
Hfusion.at<18>() = 2*HK96;
|
||||
Hfusion.at<19>() = 0;
|
||||
Hfusion.at<20>() = 1;
|
||||
Hfusion.at<21>() = 0;
|
||||
Hfusion.at<22>() = 0;
|
||||
Hfusion.at<23>() = 0;
|
||||
|
||||
|
||||
// Kalman gains - axis 1
|
||||
Kfusion(0) = HK104*HK112;
|
||||
Kfusion(1) = HK110*HK112;
|
||||
Kfusion(2) = HK108*HK112;
|
||||
Kfusion(3) = HK107*HK112;
|
||||
Kfusion(4) = HK112*(-HK100*P(4,16) + HK101*P(1,4) - HK102*P(4,17) - HK103*P(3,4) + HK97*P(4,18) + HK98*P(2,4) + HK99*P(0,4) + P(4,20));
|
||||
Kfusion(5) = HK112*(-HK100*P(5,16) + HK101*P(1,5) - HK102*P(5,17) - HK103*P(3,5) + HK97*P(5,18) + HK98*P(2,5) + HK99*P(0,5) + P(5,20));
|
||||
Kfusion(6) = HK112*(-HK100*P(6,16) + HK101*P(1,6) - HK102*P(6,17) - HK103*P(3,6) + HK97*P(6,18) + HK98*P(2,6) + HK99*P(0,6) + P(6,20));
|
||||
Kfusion(7) = HK112*(-HK100*P(7,16) + HK101*P(1,7) - HK102*P(7,17) - HK103*P(3,7) + HK97*P(7,18) + HK98*P(2,7) + HK99*P(0,7) + P(7,20));
|
||||
Kfusion(8) = HK112*(-HK100*P(8,16) + HK101*P(1,8) - HK102*P(8,17) - HK103*P(3,8) + HK97*P(8,18) + HK98*P(2,8) + HK99*P(0,8) + P(8,20));
|
||||
Kfusion(9) = HK112*(-HK100*P(9,16) + HK101*P(1,9) - HK102*P(9,17) - HK103*P(3,9) + HK97*P(9,18) + HK98*P(2,9) + HK99*P(0,9) + P(9,20));
|
||||
Kfusion(10) = HK112*(-HK100*P(10,16) + HK101*P(1,10) - HK102*P(10,17) - HK103*P(3,10) + HK97*P(10,18) + HK98*P(2,10) + HK99*P(0,10) + P(10,20));
|
||||
Kfusion(11) = HK112*(-HK100*P(11,16) + HK101*P(1,11) - HK102*P(11,17) - HK103*P(3,11) + HK97*P(11,18) + HK98*P(2,11) + HK99*P(0,11) + P(11,20));
|
||||
Kfusion(12) = HK112*(-HK100*P(12,16) + HK101*P(1,12) - HK102*P(12,17) - HK103*P(3,12) + HK97*P(12,18) + HK98*P(2,12) + HK99*P(0,12) + P(12,20));
|
||||
Kfusion(13) = HK112*(-HK100*P(13,16) + HK101*P(1,13) - HK102*P(13,17) - HK103*P(3,13) + HK97*P(13,18) + HK98*P(2,13) + HK99*P(0,13) + P(13,20));
|
||||
Kfusion(14) = HK112*(-HK100*P(14,16) + HK101*P(1,14) - HK102*P(14,17) - HK103*P(3,14) + HK97*P(14,18) + HK98*P(2,14) + HK99*P(0,14) + P(14,20));
|
||||
Kfusion(15) = HK112*(-HK100*P(15,16) + HK101*P(1,15) - HK102*P(15,17) - HK103*P(3,15) + HK97*P(15,18) + HK98*P(2,15) + HK99*P(0,15) + P(15,20));
|
||||
Kfusion(16) = HK106*HK112;
|
||||
Kfusion(17) = HK105*HK112;
|
||||
Kfusion(18) = HK109*HK112;
|
||||
Kfusion(19) = HK112*(-HK100*P(16,19) + HK101*P(1,19) - HK102*P(17,19) - HK103*P(3,19) + HK97*P(18,19) + HK98*P(2,19) + HK99*P(0,19) + P(19,20));
|
||||
Kfusion(20) = HK111*HK112;
|
||||
Kfusion(21) = HK112*(-HK100*P(16,21) + HK101*P(1,21) - HK102*P(17,21) - HK103*P(3,21) + HK97*P(18,21) + HK98*P(2,21) + HK99*P(0,21) + P(20,21));
|
||||
Kfusion(22) = HK112*(-HK100*P(16,22) + HK101*P(1,22) - HK102*P(17,22) - HK103*P(3,22) + HK97*P(18,22) + HK98*P(2,22) + HK99*P(0,22) + P(20,22));
|
||||
Kfusion(23) = HK112*(-HK100*P(16,23) + HK101*P(1,23) - HK102*P(17,23) - HK103*P(3,23) + HK97*P(18,23) + HK98*P(2,23) + HK99*P(0,23) + P(20,23));
|
||||
|
||||
|
||||
// Observation Jacobians - axis 2
|
||||
Hfusion.at<0>() = 2*HK36 + 2*HK8;
|
||||
Hfusion.at<1>() = -2*HK11 - 2*HK113 + 2*HK12;
|
||||
Hfusion.at<2>() = 2*HK114;
|
||||
Hfusion.at<3>() = 2*HK115;
|
||||
Hfusion.at<4>() = 0;
|
||||
Hfusion.at<5>() = 0;
|
||||
Hfusion.at<6>() = 0;
|
||||
Hfusion.at<7>() = 0;
|
||||
Hfusion.at<8>() = 0;
|
||||
Hfusion.at<9>() = 0;
|
||||
Hfusion.at<10>() = 0;
|
||||
Hfusion.at<11>() = 0;
|
||||
Hfusion.at<12>() = 0;
|
||||
Hfusion.at<13>() = 0;
|
||||
Hfusion.at<14>() = 0;
|
||||
Hfusion.at<15>() = 0;
|
||||
Hfusion.at<16>() = 2*HK116;
|
||||
Hfusion.at<17>() = -2*HK94 + 2*HK95;
|
||||
Hfusion.at<18>() = HK15 + HK93 + 1;
|
||||
Hfusion.at<19>() = 0;
|
||||
Hfusion.at<20>() = 0;
|
||||
Hfusion.at<21>() = 1;
|
||||
Hfusion.at<22>() = 0;
|
||||
Hfusion.at<23>() = 0;
|
||||
|
||||
|
||||
// Kalman gains - axis 2
|
||||
Kfusion(0) = HK174*(-HK118 - HK120 + HK122 + HK124 + HK126 - HK128 + HK130 - P(0,21));
|
||||
Kfusion(1) = HK174*(-HK166 - HK167 + HK168 + HK169 - HK170 + HK171 + HK172 - P(1,21));
|
||||
Kfusion(2) = HK174*(-HK152 - HK153 + HK154 + HK155 - HK156 + HK157 + HK158 - P(2,21));
|
||||
Kfusion(3) = HK174*(-HK138 - HK139 + HK140 + HK141 - HK142 + HK143 + HK144 - P(3,21));
|
||||
Kfusion(4) = HK174*(-HK117*P(4,16) - HK119*P(3,4) + HK121*P(0,4) + HK123*P(4,17) + HK125*P(4,18) - HK127*P(2,4) + HK129*P(1,4) - P(4,21));
|
||||
Kfusion(5) = HK174*(-HK117*P(5,16) - HK119*P(3,5) + HK121*P(0,5) + HK123*P(5,17) + HK125*P(5,18) - HK127*P(2,5) + HK129*P(1,5) - P(5,21));
|
||||
Kfusion(6) = HK174*(-HK117*P(6,16) - HK119*P(3,6) + HK121*P(0,6) + HK123*P(6,17) + HK125*P(6,18) - HK127*P(2,6) + HK129*P(1,6) - P(6,21));
|
||||
Kfusion(7) = HK174*(-HK117*P(7,16) - HK119*P(3,7) + HK121*P(0,7) + HK123*P(7,17) + HK125*P(7,18) - HK127*P(2,7) + HK129*P(1,7) - P(7,21));
|
||||
Kfusion(8) = HK174*(-HK117*P(8,16) - HK119*P(3,8) + HK121*P(0,8) + HK123*P(8,17) + HK125*P(8,18) - HK127*P(2,8) + HK129*P(1,8) - P(8,21));
|
||||
Kfusion(9) = HK174*(-HK117*P(9,16) - HK119*P(3,9) + HK121*P(0,9) + HK123*P(9,17) + HK125*P(9,18) - HK127*P(2,9) + HK129*P(1,9) - P(9,21));
|
||||
Kfusion(10) = HK174*(-HK117*P(10,16) - HK119*P(3,10) + HK121*P(0,10) + HK123*P(10,17) + HK125*P(10,18) - HK127*P(2,10) + HK129*P(1,10) - P(10,21));
|
||||
Kfusion(11) = HK174*(-HK117*P(11,16) - HK119*P(3,11) + HK121*P(0,11) + HK123*P(11,17) + HK125*P(11,18) - HK127*P(2,11) + HK129*P(1,11) - P(11,21));
|
||||
Kfusion(12) = HK174*(-HK117*P(12,16) - HK119*P(3,12) + HK121*P(0,12) + HK123*P(12,17) + HK125*P(12,18) - HK127*P(2,12) + HK129*P(1,12) - P(12,21));
|
||||
Kfusion(13) = HK174*(-HK117*P(13,16) - HK119*P(3,13) + HK121*P(0,13) + HK123*P(13,17) + HK125*P(13,18) - HK127*P(2,13) + HK129*P(1,13) - P(13,21));
|
||||
Kfusion(14) = HK174*(-HK117*P(14,16) - HK119*P(3,14) + HK121*P(0,14) + HK123*P(14,17) + HK125*P(14,18) - HK127*P(2,14) + HK129*P(1,14) - P(14,21));
|
||||
Kfusion(15) = HK174*(-HK117*P(15,16) - HK119*P(3,15) + HK121*P(0,15) + HK123*P(15,17) + HK125*P(15,18) - HK127*P(2,15) + HK129*P(1,15) - P(15,21));
|
||||
Kfusion(16) = HK174*(-HK145 - HK146 + HK147 + HK148 - HK149 + HK150 + HK151 - P(16,21));
|
||||
Kfusion(17) = HK174*(-HK159 - HK160 + HK161 + HK162 - HK163 + HK164 + HK165 - P(17,21));
|
||||
Kfusion(18) = HK174*(-HK131 - HK132 + HK133 + HK134 - HK135 + HK136 + HK137 - P(18,21));
|
||||
Kfusion(19) = HK174*(-HK117*P(16,19) - HK119*P(3,19) + HK121*P(0,19) + HK123*P(17,19) + HK125*P(18,19) - HK127*P(2,19) + HK129*P(1,19) + HK84);
|
||||
Kfusion(20) = HK174*(-HK117*P(16,20) - HK119*P(3,20) + HK121*P(0,20) + HK123*P(17,20) + HK125*P(18,20) - HK127*P(2,20) + HK129*P(1,20) - P(20,21));
|
||||
Kfusion(21) = HK173*HK174;
|
||||
Kfusion(22) = HK174*(-HK117*P(16,22) - HK119*P(3,22) + HK121*P(0,22) + HK123*P(17,22) + HK125*P(18,22) - HK127*P(2,22) + HK129*P(1,22) - P(21,22));
|
||||
Kfusion(23) = HK174*(-HK117*P(16,23) - HK119*P(3,23) + HK121*P(0,23) + HK123*P(17,23) + HK125*P(18,23) - HK127*P(2,23) + HK129*P(1,23) - P(21,23));
|
||||
|
||||
|
||||
@@ -0,0 +1,49 @@
|
||||
// Sub Expressions
|
||||
const float IV0 = q0*q1;
|
||||
const float IV1 = q2*q3;
|
||||
const float IV2 = 2*IV0 + 2*IV1;
|
||||
const float IV3 = magN*q1;
|
||||
const float IV4 = 2*IV3 + 2*magD*q3;
|
||||
const float IV5 = magD*q1;
|
||||
const float IV6 = -magN*q3;
|
||||
const float IV7 = 2*IV5 + 2*IV6;
|
||||
const float IV8 = 2*q0*q3 - 2*q1*q2;
|
||||
const float IV9 = magN*q2;
|
||||
const float IV10 = magE*q1;
|
||||
const float IV11 = -4*IV10 + 2*IV9 + 2*magD*q0;
|
||||
const float IV12 = 2*powf(q1, 2) - 1;
|
||||
const float IV13 = IV12 + 2*powf(q3, 2);
|
||||
const float IV14 = magN*q0;
|
||||
const float IV15 = magD*q2;
|
||||
const float IV16 = magE*q3;
|
||||
const float IV17 = 2*IV14 - 2*IV15 + 4*IV16;
|
||||
const float IV18 = 2*q0*q2 + 2*q1*q3;
|
||||
const float IV19 = 2*IV3 + 2*magE*q2;
|
||||
const float IV20 = 2*IV10 - 2*IV9;
|
||||
const float IV21 = 2*IV0 - 2*IV1;
|
||||
const float IV22 = 2*IV14 - 4*IV15 + 2*IV16;
|
||||
const float IV23 = 4*IV5 + 2*IV6 + 2*magE*q0;
|
||||
const float IV24 = IV12 + 2*powf(q2, 2);
|
||||
|
||||
|
||||
// Observation Jacobians - axis 0
|
||||
Hfusion.at<0>() = R_MAG;
|
||||
Hfusion.at<1>() = IV11*P(1,20) + IV11*(IV11*P(1,1) - IV13*P(1,17) - IV17*P(1,3) + IV2*P(1,18) + IV4*P(1,2) + IV7*P(0,1) - IV8*P(1,16) + P(1,20)) - IV13*P(17,20) - IV13*(IV11*P(1,17) - IV13*P(17,17) - IV17*P(3,17) + IV2*P(17,18) + IV4*P(2,17) + IV7*P(0,17) - IV8*P(16,17) + P(17,20)) - IV17*P(3,20) - IV17*(IV11*P(1,3) - IV13*P(3,17) - IV17*P(3,3) + IV2*P(3,18) + IV4*P(2,3) + IV7*P(0,3) - IV8*P(3,16) + P(3,20)) + IV2*P(18,20) + IV2*(IV11*P(1,18) - IV13*P(17,18) - IV17*P(3,18) + IV2*P(18,18) + IV4*P(2,18) + IV7*P(0,18) - IV8*P(16,18) + P(18,20)) + IV4*P(2,20) + IV4*(IV11*P(1,2) - IV13*P(2,17) - IV17*P(2,3) + IV2*P(2,18) + IV4*P(2,2) + IV7*P(0,2) - IV8*P(2,16) + P(2,20)) + IV7*P(0,20) + IV7*(IV11*P(0,1) - IV13*P(0,17) - IV17*P(0,3) + IV2*P(0,18) + IV4*P(0,2) + IV7*P(0,0) - IV8*P(0,16) + P(0,20)) - IV8*P(16,20) - IV8*(IV11*P(1,16) - IV13*P(16,17) - IV17*P(3,16) + IV2*P(16,18) + IV4*P(2,16) + IV7*P(0,16) - IV8*P(16,16) + P(16,20)) + P(20,20) + R_MAG;
|
||||
Hfusion.at<2>() = IV18*P(16,21) + IV18*(IV18*P(16,16) + IV19*P(3,16) - IV20*P(0,16) - IV21*P(16,17) + IV22*P(2,16) - IV23*P(1,16) - IV24*P(16,18) + P(16,21)) + IV19*P(3,21) + IV19*(IV18*P(3,16) + IV19*P(3,3) - IV20*P(0,3) - IV21*P(3,17) + IV22*P(2,3) - IV23*P(1,3) - IV24*P(3,18) + P(3,21)) - IV20*P(0,21) - IV20*(IV18*P(0,16) + IV19*P(0,3) - IV20*P(0,0) - IV21*P(0,17) + IV22*P(0,2) - IV23*P(0,1) - IV24*P(0,18) + P(0,21)) - IV21*P(17,21) - IV21*(IV18*P(16,17) + IV19*P(3,17) - IV20*P(0,17) - IV21*P(17,17) + IV22*P(2,17) - IV23*P(1,17) - IV24*P(17,18) + P(17,21)) + IV22*P(2,21) + IV22*(IV18*P(2,16) + IV19*P(2,3) - IV20*P(0,2) - IV21*P(2,17) + IV22*P(2,2) - IV23*P(1,2) - IV24*P(2,18) + P(2,21)) - IV23*P(1,21) - IV23*(IV18*P(1,16) + IV19*P(1,3) - IV20*P(0,1) - IV21*P(1,17) + IV22*P(1,2) - IV23*P(1,1) - IV24*P(1,18) + P(1,21)) - IV24*P(18,21) - IV24*(IV18*P(16,18) + IV19*P(3,18) - IV20*P(0,18) - IV21*P(17,18) + IV22*P(2,18) - IV23*P(1,18) - IV24*P(18,18) + P(18,21)) + P(21,21) + R_MAG;
|
||||
|
||||
|
||||
// Kalman gains - axis 0
|
||||
|
||||
|
||||
// Observation Jacobians - axis 1
|
||||
|
||||
|
||||
// Kalman gains - axis 1
|
||||
|
||||
|
||||
// Observation Jacobians - axis 2
|
||||
|
||||
|
||||
// Kalman gains - axis 2
|
||||
|
||||
|
||||
+476
@@ -0,0 +1,476 @@
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <cstdlib>
|
||||
#include "../../../../../matrix/matrix/math.hpp"
|
||||
#include "util.h"
|
||||
|
||||
typedef matrix::Vector<float, 24> Vector24f;
|
||||
typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
|
||||
template<int ... Idxs>
|
||||
using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
|
||||
|
||||
int main()
|
||||
{
|
||||
// Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations
|
||||
|
||||
SparseVector24f<0,1,2,3,4,5,6,22,23> Hfusion;
|
||||
Vector24f H_ACC;
|
||||
Vector24f Kfusion;
|
||||
float drag_innov_var;
|
||||
|
||||
Vector24f Hfusion_sympy;
|
||||
Vector24f Kfusion_sympy;
|
||||
|
||||
Vector24f Hfusion_matlab;
|
||||
Vector24f Kfusion_matlab;
|
||||
|
||||
float SH_ACC[4] = {}; // Variable used to optimise calculations of measurement jacobian
|
||||
float SK_ACC[9] = {}; // Variable used to optimise calculations of the Kalman gain vector
|
||||
const float R_ACC = sq(2.5f); // observation noise variance in specific force drag (m/sec**2)**2
|
||||
|
||||
// quaternion inputs must be normalised
|
||||
float q0 = 2.0f * ((float)rand() - 0.5f);
|
||||
float q1 = 2.0f * ((float)rand() - 0.5f);
|
||||
float q2 = 2.0f * ((float)rand() - 0.5f);
|
||||
float q3 = 2.0f * ((float)rand() - 0.5f);
|
||||
const float length = sqrtf(sq(q0) + sq(q1) + sq(q2) + sq(q3));
|
||||
q0 /= length;
|
||||
q1 /= length;
|
||||
q2 /= length;
|
||||
q3 /= length;
|
||||
|
||||
// get latest velocity in earth frame
|
||||
const float vn = 8.0f;
|
||||
const float ve = 6.0f;
|
||||
const float vd = 1.0f;
|
||||
|
||||
// get latest wind velocity in earth frame
|
||||
const float vwn = 4.0f;
|
||||
const float vwe = 3.0f;
|
||||
|
||||
const float BC_inv_x = 1.0f / 35.0f;
|
||||
const float BC_inv_y = 1.0f / 25.0f;
|
||||
|
||||
const float airSpd = 5.0f;
|
||||
|
||||
const float rho = 1.225f;
|
||||
|
||||
// create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
|
||||
SquareMatrix24f P;
|
||||
for (int col=0; col<=23; col++) {
|
||||
for (int row=0; row<=col; row++) {
|
||||
if (row == col) {
|
||||
P(row,col) = (float)rand();
|
||||
} else {
|
||||
P(col,row) = P(row,col) = 2.0f * ((float)rand() - 0.5f);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Compare X axis equations
|
||||
{
|
||||
// Estimate the derivative of specific force wrt airspeed along the X axis
|
||||
// Limit lower value to prevent arithmetic exceptions
|
||||
const float Kaccx = fmaxf(1e-1f, rho * BC_inv_x * airSpd);
|
||||
|
||||
// intermediate variables
|
||||
const float HK0 = vn - vwn;
|
||||
const float HK1 = ve - vwe;
|
||||
const float HK2 = HK0*q0 + HK1*q3 - q2*vd;
|
||||
const float HK3 = 2*Kaccx;
|
||||
const float HK4 = HK0*q1 + HK1*q2 + q3*vd;
|
||||
const float HK5 = HK0*q2 - HK1*q1 + q0*vd;
|
||||
const float HK6 = -HK0*q3 + HK1*q0 + q1*vd;
|
||||
const float HK7 = ecl::powf(q0, 2) + ecl::powf(q1, 2) - ecl::powf(q2, 2) - ecl::powf(q3, 2);
|
||||
const float HK8 = HK7*Kaccx;
|
||||
const float HK9 = q0*q3 + q1*q2;
|
||||
const float HK10 = HK3*HK9;
|
||||
const float HK11 = q0*q2 - q1*q3;
|
||||
const float HK12 = 2*HK9;
|
||||
const float HK13 = 2*HK11;
|
||||
const float HK14 = 2*HK4;
|
||||
const float HK15 = 2*HK2;
|
||||
const float HK16 = 2*HK5;
|
||||
const float HK17 = 2*HK6;
|
||||
const float HK18 = -HK12*P(0,23) + HK12*P(0,5) - HK13*P(0,6) + HK14*P(0,1) + HK15*P(0,0) - HK16*P(0,2) + HK17*P(0,3) - HK7*P(0,22) + HK7*P(0,4);
|
||||
const float HK19 = HK12*P(5,23);
|
||||
const float HK20 = -HK12*P(23,23) - HK13*P(6,23) + HK14*P(1,23) + HK15*P(0,23) - HK16*P(2,23) + HK17*P(3,23) + HK19 - HK7*P(22,23) + HK7*P(4,23);
|
||||
const float HK21 = ecl::powf(Kaccx, 2);
|
||||
const float HK22 = HK12*HK21;
|
||||
const float HK23 = HK12*P(5,5) - HK13*P(5,6) + HK14*P(1,5) + HK15*P(0,5) - HK16*P(2,5) + HK17*P(3,5) - HK19 + HK7*P(4,5) - HK7*P(5,22);
|
||||
const float HK24 = HK12*P(5,6) - HK12*P(6,23) - HK13*P(6,6) + HK14*P(1,6) + HK15*P(0,6) - HK16*P(2,6) + HK17*P(3,6) + HK7*P(4,6) - HK7*P(6,22);
|
||||
const float HK25 = HK7*P(4,22);
|
||||
const float HK26 = -HK12*P(4,23) + HK12*P(4,5) - HK13*P(4,6) + HK14*P(1,4) + HK15*P(0,4) - HK16*P(2,4) + HK17*P(3,4) - HK25 + HK7*P(4,4);
|
||||
const float HK27 = HK21*HK7;
|
||||
const float HK28 = -HK12*P(22,23) + HK12*P(5,22) - HK13*P(6,22) + HK14*P(1,22) + HK15*P(0,22) - HK16*P(2,22) + HK17*P(3,22) + HK25 - HK7*P(22,22);
|
||||
const float HK29 = -HK12*P(1,23) + HK12*P(1,5) - HK13*P(1,6) + HK14*P(1,1) + HK15*P(0,1) - HK16*P(1,2) + HK17*P(1,3) - HK7*P(1,22) + HK7*P(1,4);
|
||||
const float HK30 = -HK12*P(2,23) + HK12*P(2,5) - HK13*P(2,6) + HK14*P(1,2) + HK15*P(0,2) - HK16*P(2,2) + HK17*P(2,3) - HK7*P(2,22) + HK7*P(2,4);
|
||||
const float HK31 = -HK12*P(3,23) + HK12*P(3,5) - HK13*P(3,6) + HK14*P(1,3) + HK15*P(0,3) - HK16*P(2,3) + HK17*P(3,3) - HK7*P(3,22) + HK7*P(3,4);
|
||||
const float HK32 = Kaccx/(-HK13*HK21*HK24 + HK14*HK21*HK29 + HK15*HK18*HK21 - HK16*HK21*HK30 + HK17*HK21*HK31 - HK20*HK22 + HK22*HK23 + HK26*HK27 - HK27*HK28 + R_ACC);
|
||||
|
||||
// Observation Jacobians
|
||||
Hfusion.at<0>() = -HK2*HK3;
|
||||
Hfusion.at<1>() = -HK3*HK4;
|
||||
Hfusion.at<2>() = HK3*HK5;
|
||||
Hfusion.at<3>() = -HK3*HK6;
|
||||
Hfusion.at<4>() = -HK8;
|
||||
Hfusion.at<5>() = -HK10;
|
||||
Hfusion.at<6>() = HK11*HK3;
|
||||
Hfusion.at<22>() = HK8;
|
||||
Hfusion.at<23>() = HK10;
|
||||
|
||||
// Kalman gains
|
||||
Kfusion(0) = -HK18*HK32;
|
||||
Kfusion(1) = -HK29*HK32;
|
||||
Kfusion(2) = -HK30*HK32;
|
||||
Kfusion(3) = -HK31*HK32;
|
||||
Kfusion(4) = -HK26*HK32;
|
||||
Kfusion(5) = -HK23*HK32;
|
||||
Kfusion(6) = -HK24*HK32;
|
||||
Kfusion(7) = -HK32*(HK12*P(5,7) - HK12*P(7,23) - HK13*P(6,7) + HK14*P(1,7) + HK15*P(0,7) - HK16*P(2,7) + HK17*P(3,7) + HK7*P(4,7) - HK7*P(7,22));
|
||||
Kfusion(8) = -HK32*(HK12*P(5,8) - HK12*P(8,23) - HK13*P(6,8) + HK14*P(1,8) + HK15*P(0,8) - HK16*P(2,8) + HK17*P(3,8) + HK7*P(4,8) - HK7*P(8,22));
|
||||
Kfusion(9) = -HK32*(HK12*P(5,9) - HK12*P(9,23) - HK13*P(6,9) + HK14*P(1,9) + HK15*P(0,9) - HK16*P(2,9) + HK17*P(3,9) + HK7*P(4,9) - HK7*P(9,22));
|
||||
Kfusion(10) = -HK32*(-HK12*P(10,23) + HK12*P(5,10) - HK13*P(6,10) + HK14*P(1,10) + HK15*P(0,10) - HK16*P(2,10) + HK17*P(3,10) - HK7*P(10,22) + HK7*P(4,10));
|
||||
Kfusion(11) = -HK32*(-HK12*P(11,23) + HK12*P(5,11) - HK13*P(6,11) + HK14*P(1,11) + HK15*P(0,11) - HK16*P(2,11) + HK17*P(3,11) - HK7*P(11,22) + HK7*P(4,11));
|
||||
Kfusion(12) = -HK32*(-HK12*P(12,23) + HK12*P(5,12) - HK13*P(6,12) + HK14*P(1,12) + HK15*P(0,12) - HK16*P(2,12) + HK17*P(3,12) - HK7*P(12,22) + HK7*P(4,12));
|
||||
Kfusion(13) = -HK32*(-HK12*P(13,23) + HK12*P(5,13) - HK13*P(6,13) + HK14*P(1,13) + HK15*P(0,13) - HK16*P(2,13) + HK17*P(3,13) - HK7*P(13,22) + HK7*P(4,13));
|
||||
Kfusion(14) = -HK32*(-HK12*P(14,23) + HK12*P(5,14) - HK13*P(6,14) + HK14*P(1,14) + HK15*P(0,14) - HK16*P(2,14) + HK17*P(3,14) - HK7*P(14,22) + HK7*P(4,14));
|
||||
Kfusion(15) = -HK32*(-HK12*P(15,23) + HK12*P(5,15) - HK13*P(6,15) + HK14*P(1,15) + HK15*P(0,15) - HK16*P(2,15) + HK17*P(3,15) - HK7*P(15,22) + HK7*P(4,15));
|
||||
Kfusion(16) = -HK32*(-HK12*P(16,23) + HK12*P(5,16) - HK13*P(6,16) + HK14*P(1,16) + HK15*P(0,16) - HK16*P(2,16) + HK17*P(3,16) - HK7*P(16,22) + HK7*P(4,16));
|
||||
Kfusion(17) = -HK32*(-HK12*P(17,23) + HK12*P(5,17) - HK13*P(6,17) + HK14*P(1,17) + HK15*P(0,17) - HK16*P(2,17) + HK17*P(3,17) - HK7*P(17,22) + HK7*P(4,17));
|
||||
Kfusion(18) = -HK32*(-HK12*P(18,23) + HK12*P(5,18) - HK13*P(6,18) + HK14*P(1,18) + HK15*P(0,18) - HK16*P(2,18) + HK17*P(3,18) - HK7*P(18,22) + HK7*P(4,18));
|
||||
Kfusion(19) = -HK32*(-HK12*P(19,23) + HK12*P(5,19) - HK13*P(6,19) + HK14*P(1,19) + HK15*P(0,19) - HK16*P(2,19) + HK17*P(3,19) - HK7*P(19,22) + HK7*P(4,19));
|
||||
Kfusion(20) = -HK32*(-HK12*P(20,23) + HK12*P(5,20) - HK13*P(6,20) + HK14*P(1,20) + HK15*P(0,20) - HK16*P(2,20) + HK17*P(3,20) - HK7*P(20,22) + HK7*P(4,20));
|
||||
Kfusion(21) = -HK32*(-HK12*P(21,23) + HK12*P(5,21) - HK13*P(6,21) + HK14*P(1,21) + HK15*P(0,21) - HK16*P(2,21) + HK17*P(3,21) - HK7*P(21,22) + HK7*P(4,21));
|
||||
Kfusion(22) = -HK28*HK32;
|
||||
Kfusion(23) = -HK20*HK32;
|
||||
|
||||
// save output
|
||||
Hfusion_sympy(0) = Hfusion.at<0>();
|
||||
Hfusion_sympy(1) = Hfusion.at<1>();
|
||||
Hfusion_sympy(2) = Hfusion.at<2>();
|
||||
Hfusion_sympy(3) = Hfusion.at<3>();
|
||||
Hfusion_sympy(4) = Hfusion.at<4>();
|
||||
Hfusion_sympy(5) = Hfusion.at<5>();
|
||||
Hfusion_sympy(6) = Hfusion.at<6>();
|
||||
Hfusion_sympy(22) = Hfusion.at<22>();
|
||||
Hfusion_sympy(23) = Hfusion.at<23>();
|
||||
Kfusion_sympy = Kfusion;
|
||||
|
||||
// repeat calculation using matlab generated equations
|
||||
|
||||
const float Kacc = Kaccx;
|
||||
|
||||
// Estimate the derivative of specific force wrt airspeed along the X axis
|
||||
SH_ACC[0] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
|
||||
SH_ACC[1] = vn - vwn;
|
||||
SH_ACC[2] = ve - vwe;
|
||||
SH_ACC[3] = 2.0f*q0*q3 + 2.0f*q1*q2;
|
||||
|
||||
H_ACC.setZero();
|
||||
H_ACC(0) = -Kacc*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd);
|
||||
H_ACC(1) = -Kacc*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd);
|
||||
H_ACC(2) = Kacc*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd);
|
||||
H_ACC(3) = -Kacc*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd);
|
||||
H_ACC(4) = -Kacc*SH_ACC[0];
|
||||
H_ACC(5) = -Kacc*SH_ACC[3];
|
||||
H_ACC(6) = Kacc*(2.0f*q0*q2 - 2.0f*q1*q3);
|
||||
H_ACC(22) = Kacc*SH_ACC[0];
|
||||
H_ACC(23) = Kacc*SH_ACC[3];
|
||||
|
||||
drag_innov_var = (R_ACC + Kacc*SH_ACC[0]*(Kacc*P(4,4)*SH_ACC[0] + Kacc*P(5,4)*SH_ACC[3] - Kacc*P(22,4)*SH_ACC[0] - Kacc*P(23,4)*SH_ACC[3] - Kacc*P(6,4)*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P(0,4)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P(1,4)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(2,4)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(3,4)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) + Kacc*SH_ACC[3]*(Kacc*P(4,5)*SH_ACC[0] + Kacc*P(5,5)*SH_ACC[3] - Kacc*P(22,5)*SH_ACC[0] - Kacc*P(23,5)*SH_ACC[3] - Kacc*P(6,5)*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P(0,5)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P(1,5)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(2,5)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(3,5)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) - Kacc*SH_ACC[0]*(Kacc*P(4,22)*SH_ACC[0] + Kacc*P(5,22)*SH_ACC[3] - Kacc*P(22,22)*SH_ACC[0] - Kacc*P(23,22)*SH_ACC[3] - Kacc*P(6,22)*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P(0,22)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P(1,22)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(2,22)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(3,22)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) - Kacc*SH_ACC[3]*(Kacc*P(4,23)*SH_ACC[0] + Kacc*P(5,23)*SH_ACC[3] - Kacc*P(22,23)*SH_ACC[0] - Kacc*P(23,23)*SH_ACC[3] - Kacc*P(6,23)*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P(0,23)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P(1,23)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(2,23)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(3,23)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) - Kacc*(2.0f*q0*q2 - 2.0f*q1*q3)*(Kacc*P(4,6)*SH_ACC[0] + Kacc*P(5,6)*SH_ACC[3] - Kacc*P(22,6)*SH_ACC[0] - Kacc*P(23,6)*SH_ACC[3] - Kacc*P(6,6)*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P(0,6)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P(1,6)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(2,6)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(3,6)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) + Kacc*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)*(Kacc*P(4,0)*SH_ACC[0] + Kacc*P(5,0)*SH_ACC[3] - Kacc*P(22,0)*SH_ACC[0] - Kacc*P(23,0)*SH_ACC[3] - Kacc*P(6,0)*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P(0,0)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P(1,0)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(2,0)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(3,0)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) + Kacc*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd)*(Kacc*P(4,1)*SH_ACC[0] + Kacc*P(5,1)*SH_ACC[3] - Kacc*P(22,1)*SH_ACC[0] - Kacc*P(23,1)*SH_ACC[3] - Kacc*P(6,1)*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P(0,1)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P(1,1)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(2,1)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(3,1)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) - Kacc*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd)*(Kacc*P(4,2)*SH_ACC[0] + Kacc*P(5,2)*SH_ACC[3] - Kacc*P(22,2)*SH_ACC[0] - Kacc*P(23,2)*SH_ACC[3] - Kacc*P(6,2)*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P(0,2)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P(1,2)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(2,2)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(3,2)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) + Kacc*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)*(Kacc*P(4,3)*SH_ACC[0] + Kacc*P(5,3)*SH_ACC[3] - Kacc*P(22,3)*SH_ACC[0] - Kacc*P(23,3)*SH_ACC[3] - Kacc*P(6,3)*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P(0,3)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P(1,3)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(2,3)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(3,3)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)));
|
||||
|
||||
SK_ACC[0] = 1.0f/drag_innov_var;
|
||||
SK_ACC[1] = 2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd;
|
||||
SK_ACC[2] = 2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd;
|
||||
SK_ACC[3] = 2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd;
|
||||
SK_ACC[4] = 2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd;
|
||||
SK_ACC[5] = 2.0f*q0*q2 - 2.0f*q1*q3;
|
||||
SK_ACC[6] = SH_ACC[3];
|
||||
|
||||
Kfusion(0) = -SK_ACC[0]*(Kacc*P(0,4)*SH_ACC[0] - Kacc*P(0,22)*SH_ACC[0] + Kacc*P(0,0)*SK_ACC[3] - Kacc*P(0,2)*SK_ACC[2] + Kacc*P(0,3)*SK_ACC[1] + Kacc*P(0,1)*SK_ACC[4] + Kacc*P(0,5)*SK_ACC[6] - Kacc*P(0,6)*SK_ACC[5] - Kacc*P(0,23)*SK_ACC[6]);
|
||||
Kfusion(1) = -SK_ACC[0]*(Kacc*P(1,4)*SH_ACC[0] - Kacc*P(1,22)*SH_ACC[0] + Kacc*P(1,0)*SK_ACC[3] - Kacc*P(1,2)*SK_ACC[2] + Kacc*P(1,3)*SK_ACC[1] + Kacc*P(1,1)*SK_ACC[4] + Kacc*P(1,5)*SK_ACC[6] - Kacc*P(1,6)*SK_ACC[5] - Kacc*P(1,23)*SK_ACC[6]);
|
||||
Kfusion(2) = -SK_ACC[0]*(Kacc*P(2,4)*SH_ACC[0] - Kacc*P(2,22)*SH_ACC[0] + Kacc*P(2,0)*SK_ACC[3] - Kacc*P(2,2)*SK_ACC[2] + Kacc*P(2,3)*SK_ACC[1] + Kacc*P(2,1)*SK_ACC[4] + Kacc*P(2,5)*SK_ACC[6] - Kacc*P(2,6)*SK_ACC[5] - Kacc*P(2,23)*SK_ACC[6]);
|
||||
Kfusion(3) = -SK_ACC[0]*(Kacc*P(3,4)*SH_ACC[0] - Kacc*P(3,22)*SH_ACC[0] + Kacc*P(3,0)*SK_ACC[3] - Kacc*P(3,2)*SK_ACC[2] + Kacc*P(3,3)*SK_ACC[1] + Kacc*P(3,1)*SK_ACC[4] + Kacc*P(3,5)*SK_ACC[6] - Kacc*P(3,6)*SK_ACC[5] - Kacc*P(3,23)*SK_ACC[6]);
|
||||
Kfusion(4) = -SK_ACC[0]*(Kacc*P(4,4)*SH_ACC[0] - Kacc*P(4,22)*SH_ACC[0] + Kacc*P(4,0)*SK_ACC[3] - Kacc*P(4,2)*SK_ACC[2] + Kacc*P(4,3)*SK_ACC[1] + Kacc*P(4,1)*SK_ACC[4] + Kacc*P(4,5)*SK_ACC[6] - Kacc*P(4,6)*SK_ACC[5] - Kacc*P(4,23)*SK_ACC[6]);
|
||||
Kfusion(5) = -SK_ACC[0]*(Kacc*P(5,4)*SH_ACC[0] - Kacc*P(5,22)*SH_ACC[0] + Kacc*P(5,0)*SK_ACC[3] - Kacc*P(5,2)*SK_ACC[2] + Kacc*P(5,3)*SK_ACC[1] + Kacc*P(5,1)*SK_ACC[4] + Kacc*P(5,5)*SK_ACC[6] - Kacc*P(5,6)*SK_ACC[5] - Kacc*P(5,23)*SK_ACC[6]);
|
||||
Kfusion(6) = -SK_ACC[0]*(Kacc*P(6,4)*SH_ACC[0] - Kacc*P(6,22)*SH_ACC[0] + Kacc*P(6,0)*SK_ACC[3] - Kacc*P(6,2)*SK_ACC[2] + Kacc*P(6,3)*SK_ACC[1] + Kacc*P(6,1)*SK_ACC[4] + Kacc*P(6,5)*SK_ACC[6] - Kacc*P(6,6)*SK_ACC[5] - Kacc*P(6,23)*SK_ACC[6]);
|
||||
Kfusion(7) = -SK_ACC[0]*(Kacc*P(7,4)*SH_ACC[0] - Kacc*P(7,22)*SH_ACC[0] + Kacc*P(7,0)*SK_ACC[3] - Kacc*P(7,2)*SK_ACC[2] + Kacc*P(7,3)*SK_ACC[1] + Kacc*P(7,1)*SK_ACC[4] + Kacc*P(7,5)*SK_ACC[6] - Kacc*P(7,6)*SK_ACC[5] - Kacc*P(7,23)*SK_ACC[6]);
|
||||
Kfusion(8) = -SK_ACC[0]*(Kacc*P(8,4)*SH_ACC[0] - Kacc*P(8,22)*SH_ACC[0] + Kacc*P(8,0)*SK_ACC[3] - Kacc*P(8,2)*SK_ACC[2] + Kacc*P(8,3)*SK_ACC[1] + Kacc*P(8,1)*SK_ACC[4] + Kacc*P(8,5)*SK_ACC[6] - Kacc*P(8,6)*SK_ACC[5] - Kacc*P(8,23)*SK_ACC[6]);
|
||||
Kfusion(9) = -SK_ACC[0]*(Kacc*P(9,4)*SH_ACC[0] - Kacc*P(9,22)*SH_ACC[0] + Kacc*P(9,0)*SK_ACC[3] - Kacc*P(9,2)*SK_ACC[2] + Kacc*P(9,3)*SK_ACC[1] + Kacc*P(9,1)*SK_ACC[4] + Kacc*P(9,5)*SK_ACC[6] - Kacc*P(9,6)*SK_ACC[5] - Kacc*P(9,23)*SK_ACC[6]);
|
||||
Kfusion(10) = -SK_ACC[0]*(Kacc*P(10,4)*SH_ACC[0] - Kacc*P(10,22)*SH_ACC[0] + Kacc*P(10,0)*SK_ACC[3] - Kacc*P(10,2)*SK_ACC[2] + Kacc*P(10,3)*SK_ACC[1] + Kacc*P(10,1)*SK_ACC[4] + Kacc*P(10,5)*SK_ACC[6] - Kacc*P(10,6)*SK_ACC[5] - Kacc*P(10,23)*SK_ACC[6]);
|
||||
Kfusion(11) = -SK_ACC[0]*(Kacc*P(11,4)*SH_ACC[0] - Kacc*P(11,22)*SH_ACC[0] + Kacc*P(11,0)*SK_ACC[3] - Kacc*P(11,2)*SK_ACC[2] + Kacc*P(11,3)*SK_ACC[1] + Kacc*P(11,1)*SK_ACC[4] + Kacc*P(11,5)*SK_ACC[6] - Kacc*P(11,6)*SK_ACC[5] - Kacc*P(11,23)*SK_ACC[6]);
|
||||
Kfusion(12) = -SK_ACC[0]*(Kacc*P(12,4)*SH_ACC[0] - Kacc*P(12,22)*SH_ACC[0] + Kacc*P(12,0)*SK_ACC[3] - Kacc*P(12,2)*SK_ACC[2] + Kacc*P(12,3)*SK_ACC[1] + Kacc*P(12,1)*SK_ACC[4] + Kacc*P(12,5)*SK_ACC[6] - Kacc*P(12,6)*SK_ACC[5] - Kacc*P(12,23)*SK_ACC[6]);
|
||||
Kfusion(13) = -SK_ACC[0]*(Kacc*P(13,4)*SH_ACC[0] - Kacc*P(13,22)*SH_ACC[0] + Kacc*P(13,0)*SK_ACC[3] - Kacc*P(13,2)*SK_ACC[2] + Kacc*P(13,3)*SK_ACC[1] + Kacc*P(13,1)*SK_ACC[4] + Kacc*P(13,5)*SK_ACC[6] - Kacc*P(13,6)*SK_ACC[5] - Kacc*P(13,23)*SK_ACC[6]);
|
||||
Kfusion(14) = -SK_ACC[0]*(Kacc*P(14,4)*SH_ACC[0] - Kacc*P(14,22)*SH_ACC[0] + Kacc*P(14,0)*SK_ACC[3] - Kacc*P(14,2)*SK_ACC[2] + Kacc*P(14,3)*SK_ACC[1] + Kacc*P(14,1)*SK_ACC[4] + Kacc*P(14,5)*SK_ACC[6] - Kacc*P(14,6)*SK_ACC[5] - Kacc*P(14,23)*SK_ACC[6]);
|
||||
Kfusion(15) = -SK_ACC[0]*(Kacc*P(15,4)*SH_ACC[0] - Kacc*P(15,22)*SH_ACC[0] + Kacc*P(15,0)*SK_ACC[3] - Kacc*P(15,2)*SK_ACC[2] + Kacc*P(15,3)*SK_ACC[1] + Kacc*P(15,1)*SK_ACC[4] + Kacc*P(15,5)*SK_ACC[6] - Kacc*P(15,6)*SK_ACC[5] - Kacc*P(15,23)*SK_ACC[6]);
|
||||
Kfusion(16) = -SK_ACC[0]*(Kacc*P(16,4)*SH_ACC[0] - Kacc*P(16,22)*SH_ACC[0] + Kacc*P(16,0)*SK_ACC[3] - Kacc*P(16,2)*SK_ACC[2] + Kacc*P(16,3)*SK_ACC[1] + Kacc*P(16,1)*SK_ACC[4] + Kacc*P(16,5)*SK_ACC[6] - Kacc*P(16,6)*SK_ACC[5] - Kacc*P(16,23)*SK_ACC[6]);
|
||||
Kfusion(17) = -SK_ACC[0]*(Kacc*P(17,4)*SH_ACC[0] - Kacc*P(17,22)*SH_ACC[0] + Kacc*P(17,0)*SK_ACC[3] - Kacc*P(17,2)*SK_ACC[2] + Kacc*P(17,3)*SK_ACC[1] + Kacc*P(17,1)*SK_ACC[4] + Kacc*P(17,5)*SK_ACC[6] - Kacc*P(17,6)*SK_ACC[5] - Kacc*P(17,23)*SK_ACC[6]);
|
||||
Kfusion(18) = -SK_ACC[0]*(Kacc*P(18,4)*SH_ACC[0] - Kacc*P(18,22)*SH_ACC[0] + Kacc*P(18,0)*SK_ACC[3] - Kacc*P(18,2)*SK_ACC[2] + Kacc*P(18,3)*SK_ACC[1] + Kacc*P(18,1)*SK_ACC[4] + Kacc*P(18,5)*SK_ACC[6] - Kacc*P(18,6)*SK_ACC[5] - Kacc*P(18,23)*SK_ACC[6]);
|
||||
Kfusion(19) = -SK_ACC[0]*(Kacc*P(19,4)*SH_ACC[0] - Kacc*P(19,22)*SH_ACC[0] + Kacc*P(19,0)*SK_ACC[3] - Kacc*P(19,2)*SK_ACC[2] + Kacc*P(19,3)*SK_ACC[1] + Kacc*P(19,1)*SK_ACC[4] + Kacc*P(19,5)*SK_ACC[6] - Kacc*P(19,6)*SK_ACC[5] - Kacc*P(19,23)*SK_ACC[6]);
|
||||
Kfusion(20) = -SK_ACC[0]*(Kacc*P(20,4)*SH_ACC[0] - Kacc*P(20,22)*SH_ACC[0] + Kacc*P(20,0)*SK_ACC[3] - Kacc*P(20,2)*SK_ACC[2] + Kacc*P(20,3)*SK_ACC[1] + Kacc*P(20,1)*SK_ACC[4] + Kacc*P(20,5)*SK_ACC[6] - Kacc*P(20,6)*SK_ACC[5] - Kacc*P(20,23)*SK_ACC[6]);
|
||||
Kfusion(21) = -SK_ACC[0]*(Kacc*P(21,4)*SH_ACC[0] - Kacc*P(21,22)*SH_ACC[0] + Kacc*P(21,0)*SK_ACC[3] - Kacc*P(21,2)*SK_ACC[2] + Kacc*P(21,3)*SK_ACC[1] + Kacc*P(21,1)*SK_ACC[4] + Kacc*P(21,5)*SK_ACC[6] - Kacc*P(21,6)*SK_ACC[5] - Kacc*P(21,23)*SK_ACC[6]);
|
||||
Kfusion(22) = -SK_ACC[0]*(Kacc*P(22,4)*SH_ACC[0] - Kacc*P(22,22)*SH_ACC[0] + Kacc*P(22,0)*SK_ACC[3] - Kacc*P(22,2)*SK_ACC[2] + Kacc*P(22,3)*SK_ACC[1] + Kacc*P(22,1)*SK_ACC[4] + Kacc*P(22,5)*SK_ACC[6] - Kacc*P(22,6)*SK_ACC[5] - Kacc*P(22,23)*SK_ACC[6]);
|
||||
Kfusion(23) = -SK_ACC[0]*(Kacc*P(23,4)*SH_ACC[0] - Kacc*P(23,22)*SH_ACC[0] + Kacc*P(23,0)*SK_ACC[3] - Kacc*P(23,2)*SK_ACC[2] + Kacc*P(23,3)*SK_ACC[1] + Kacc*P(23,1)*SK_ACC[4] + Kacc*P(23,5)*SK_ACC[6] - Kacc*P(23,6)*SK_ACC[5] - Kacc*P(23,23)*SK_ACC[6]);
|
||||
|
||||
Hfusion_matlab = H_ACC;
|
||||
Kfusion_matlab = Kfusion;
|
||||
|
||||
// find largest observation variance difference as a fraction of the matlab value
|
||||
float max_diff_fraction = 0.0f;
|
||||
int max_row;
|
||||
float max_old, max_new;
|
||||
for (int row=0; row<24; row++) {
|
||||
float diff_fraction;
|
||||
if (Hfusion_matlab(row) != 0.0f) {
|
||||
diff_fraction = fabsf(Hfusion_sympy(row) - Hfusion_matlab(row)) / fabsf(Hfusion_matlab(row));
|
||||
} else if (Hfusion_sympy(row) != 0.0f) {
|
||||
diff_fraction = fabsf(Hfusion_sympy(row) - Hfusion_matlab(row)) / fabsf(Hfusion_sympy(row));
|
||||
} else {
|
||||
diff_fraction = 0.0f;
|
||||
}
|
||||
if (diff_fraction > max_diff_fraction) {
|
||||
max_diff_fraction = diff_fraction;
|
||||
max_row = row;
|
||||
max_old = Hfusion_matlab(row);
|
||||
max_new = Hfusion_sympy(row);
|
||||
}
|
||||
}
|
||||
|
||||
if (max_diff_fraction > 1e-5f) {
|
||||
printf("Fail: Specific Force X axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
|
||||
} else {
|
||||
printf("Pass: Specific Force X axis Hfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
|
||||
// find largest Kalman gain difference as a fraction of the matlab value
|
||||
// find largest Kalman gain difference as a fraction of the matlab value
|
||||
max_diff_fraction = 0.0f;
|
||||
for (int row=0; row<24; row++) {
|
||||
float diff_fraction;
|
||||
if (Kfusion_matlab(row) != 0.0f) {
|
||||
diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion_matlab(row)) / fabsf(Kfusion_matlab(row));
|
||||
} else if (Kfusion_sympy(row) != 0.0f) {
|
||||
diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion_matlab(row)) / fabsf(Kfusion_sympy(row));
|
||||
} else {
|
||||
diff_fraction = 0.0f;
|
||||
}
|
||||
if (diff_fraction > max_diff_fraction) {
|
||||
max_diff_fraction = diff_fraction;
|
||||
max_row = row;
|
||||
max_old = Kfusion_matlab(row);
|
||||
max_new = Kfusion_sympy(row);
|
||||
}
|
||||
}
|
||||
|
||||
if (max_diff_fraction > 1e-5f) {
|
||||
printf("Fail: Specific Force X axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
|
||||
} else {
|
||||
printf("Pass: Specific Force X axis Kfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// Compare Y axis equations
|
||||
{
|
||||
const float airSpd = 10.0f * (float)rand() + 5.0f;
|
||||
|
||||
// Estimate the derivative of specific force wrt airspeed along the X axis
|
||||
// Limit lower value to prevent arithmetic exceptions
|
||||
const float Kaccy = fmaxf(1e-1f, rho * BC_inv_y * airSpd);
|
||||
|
||||
// Sub Expressions
|
||||
const float HK0 = ve - vwe;
|
||||
const float HK1 = vn - vwn;
|
||||
const float HK2 = HK0*q0 - HK1*q3 + q1*vd;
|
||||
const float HK3 = 2*Kaccy;
|
||||
const float HK4 = -HK0*q1 + HK1*q2 + q0*vd;
|
||||
const float HK5 = HK0*q2 + HK1*q1 + q3*vd;
|
||||
const float HK6 = HK0*q3 + HK1*q0 - q2*vd;
|
||||
const float HK7 = q0*q3 - q1*q2;
|
||||
const float HK8 = HK3*HK7;
|
||||
const float HK9 = ecl::powf(q0, 2) - ecl::powf(q1, 2) + ecl::powf(q2, 2) - ecl::powf(q3, 2);
|
||||
const float HK10 = HK9*Kaccy;
|
||||
const float HK11 = q0*q1 + q2*q3;
|
||||
const float HK12 = 2*HK11;
|
||||
const float HK13 = 2*HK7;
|
||||
const float HK14 = 2*HK5;
|
||||
const float HK15 = 2*HK2;
|
||||
const float HK16 = 2*HK4;
|
||||
const float HK17 = 2*HK6;
|
||||
const float HK18 = HK12*P(0,6) + HK13*P(0,22) - HK13*P(0,4) + HK14*P(0,2) + HK15*P(0,0) + HK16*P(0,1) - HK17*P(0,3) - HK9*P(0,23) + HK9*P(0,5);
|
||||
const float HK19 = ecl::powf(Kaccy, 2);
|
||||
const float HK20 = HK12*P(6,6) - HK13*P(4,6) + HK13*P(6,22) + HK14*P(2,6) + HK15*P(0,6) + HK16*P(1,6) - HK17*P(3,6) + HK9*P(5,6) - HK9*P(6,23);
|
||||
const float HK21 = HK13*P(4,22);
|
||||
const float HK22 = HK12*P(6,22) + HK13*P(22,22) + HK14*P(2,22) + HK15*P(0,22) + HK16*P(1,22) - HK17*P(3,22) - HK21 - HK9*P(22,23) + HK9*P(5,22);
|
||||
const float HK23 = HK13*HK19;
|
||||
const float HK24 = HK12*P(4,6) - HK13*P(4,4) + HK14*P(2,4) + HK15*P(0,4) + HK16*P(1,4) - HK17*P(3,4) + HK21 - HK9*P(4,23) + HK9*P(4,5);
|
||||
const float HK25 = HK9*P(5,23);
|
||||
const float HK26 = HK12*P(5,6) - HK13*P(4,5) + HK13*P(5,22) + HK14*P(2,5) + HK15*P(0,5) + HK16*P(1,5) - HK17*P(3,5) - HK25 + HK9*P(5,5);
|
||||
const float HK27 = HK19*HK9;
|
||||
const float HK28 = HK12*P(6,23) + HK13*P(22,23) - HK13*P(4,23) + HK14*P(2,23) + HK15*P(0,23) + HK16*P(1,23) - HK17*P(3,23) + HK25 - HK9*P(23,23);
|
||||
const float HK29 = HK12*P(2,6) + HK13*P(2,22) - HK13*P(2,4) + HK14*P(2,2) + HK15*P(0,2) + HK16*P(1,2) - HK17*P(2,3) - HK9*P(2,23) + HK9*P(2,5);
|
||||
const float HK30 = HK12*P(1,6) + HK13*P(1,22) - HK13*P(1,4) + HK14*P(1,2) + HK15*P(0,1) + HK16*P(1,1) - HK17*P(1,3) - HK9*P(1,23) + HK9*P(1,5);
|
||||
const float HK31 = HK12*P(3,6) + HK13*P(3,22) - HK13*P(3,4) + HK14*P(2,3) + HK15*P(0,3) + HK16*P(1,3) - HK17*P(3,3) - HK9*P(3,23) + HK9*P(3,5);
|
||||
const float HK32 = Kaccy/(HK12*HK19*HK20 + HK14*HK19*HK29 + HK15*HK18*HK19 + HK16*HK19*HK30 - HK17*HK19*HK31 + HK22*HK23 - HK23*HK24 + HK26*HK27 - HK27*HK28 + R_ACC);
|
||||
|
||||
// Observation Jacobians
|
||||
Hfusion.at<0>() = -HK2*HK3;
|
||||
Hfusion.at<1>() = -HK3*HK4;
|
||||
Hfusion.at<2>() = -HK3*HK5;
|
||||
Hfusion.at<3>() = HK3*HK6;
|
||||
Hfusion.at<4>() = HK8;
|
||||
Hfusion.at<5>() = -HK10;
|
||||
Hfusion.at<6>() = -HK11*HK3;
|
||||
Hfusion.at<22>() = -HK8;
|
||||
Hfusion.at<23>() = HK10;
|
||||
|
||||
// Kalman gains
|
||||
Kfusion(0) = -HK18*HK32;
|
||||
Kfusion(1) = -HK30*HK32;
|
||||
Kfusion(2) = -HK29*HK32;
|
||||
Kfusion(3) = -HK31*HK32;
|
||||
Kfusion(4) = -HK24*HK32;
|
||||
Kfusion(5) = -HK26*HK32;
|
||||
Kfusion(6) = -HK20*HK32;
|
||||
Kfusion(7) = -HK32*(HK12*P(6,7) - HK13*P(4,7) + HK13*P(7,22) + HK14*P(2,7) + HK15*P(0,7) + HK16*P(1,7) - HK17*P(3,7) + HK9*P(5,7) - HK9*P(7,23));
|
||||
Kfusion(8) = -HK32*(HK12*P(6,8) - HK13*P(4,8) + HK13*P(8,22) + HK14*P(2,8) + HK15*P(0,8) + HK16*P(1,8) - HK17*P(3,8) + HK9*P(5,8) - HK9*P(8,23));
|
||||
Kfusion(9) = -HK32*(HK12*P(6,9) - HK13*P(4,9) + HK13*P(9,22) + HK14*P(2,9) + HK15*P(0,9) + HK16*P(1,9) - HK17*P(3,9) + HK9*P(5,9) - HK9*P(9,23));
|
||||
Kfusion(10) = -HK32*(HK12*P(6,10) + HK13*P(10,22) - HK13*P(4,10) + HK14*P(2,10) + HK15*P(0,10) + HK16*P(1,10) - HK17*P(3,10) - HK9*P(10,23) + HK9*P(5,10));
|
||||
Kfusion(11) = -HK32*(HK12*P(6,11) + HK13*P(11,22) - HK13*P(4,11) + HK14*P(2,11) + HK15*P(0,11) + HK16*P(1,11) - HK17*P(3,11) - HK9*P(11,23) + HK9*P(5,11));
|
||||
Kfusion(12) = -HK32*(HK12*P(6,12) + HK13*P(12,22) - HK13*P(4,12) + HK14*P(2,12) + HK15*P(0,12) + HK16*P(1,12) - HK17*P(3,12) - HK9*P(12,23) + HK9*P(5,12));
|
||||
Kfusion(13) = -HK32*(HK12*P(6,13) + HK13*P(13,22) - HK13*P(4,13) + HK14*P(2,13) + HK15*P(0,13) + HK16*P(1,13) - HK17*P(3,13) - HK9*P(13,23) + HK9*P(5,13));
|
||||
Kfusion(14) = -HK32*(HK12*P(6,14) + HK13*P(14,22) - HK13*P(4,14) + HK14*P(2,14) + HK15*P(0,14) + HK16*P(1,14) - HK17*P(3,14) - HK9*P(14,23) + HK9*P(5,14));
|
||||
Kfusion(15) = -HK32*(HK12*P(6,15) + HK13*P(15,22) - HK13*P(4,15) + HK14*P(2,15) + HK15*P(0,15) + HK16*P(1,15) - HK17*P(3,15) - HK9*P(15,23) + HK9*P(5,15));
|
||||
Kfusion(16) = -HK32*(HK12*P(6,16) + HK13*P(16,22) - HK13*P(4,16) + HK14*P(2,16) + HK15*P(0,16) + HK16*P(1,16) - HK17*P(3,16) - HK9*P(16,23) + HK9*P(5,16));
|
||||
Kfusion(17) = -HK32*(HK12*P(6,17) + HK13*P(17,22) - HK13*P(4,17) + HK14*P(2,17) + HK15*P(0,17) + HK16*P(1,17) - HK17*P(3,17) - HK9*P(17,23) + HK9*P(5,17));
|
||||
Kfusion(18) = -HK32*(HK12*P(6,18) + HK13*P(18,22) - HK13*P(4,18) + HK14*P(2,18) + HK15*P(0,18) + HK16*P(1,18) - HK17*P(3,18) - HK9*P(18,23) + HK9*P(5,18));
|
||||
Kfusion(19) = -HK32*(HK12*P(6,19) + HK13*P(19,22) - HK13*P(4,19) + HK14*P(2,19) + HK15*P(0,19) + HK16*P(1,19) - HK17*P(3,19) - HK9*P(19,23) + HK9*P(5,19));
|
||||
Kfusion(20) = -HK32*(HK12*P(6,20) + HK13*P(20,22) - HK13*P(4,20) + HK14*P(2,20) + HK15*P(0,20) + HK16*P(1,20) - HK17*P(3,20) - HK9*P(20,23) + HK9*P(5,20));
|
||||
Kfusion(21) = -HK32*(HK12*P(6,21) + HK13*P(21,22) - HK13*P(4,21) + HK14*P(2,21) + HK15*P(0,21) + HK16*P(1,21) - HK17*P(3,21) - HK9*P(21,23) + HK9*P(5,21));
|
||||
Kfusion(22) = -HK22*HK32;
|
||||
Kfusion(23) = -HK28*HK32;
|
||||
|
||||
// save output
|
||||
Hfusion_sympy(0) = Hfusion.at<0>();
|
||||
Hfusion_sympy(1) = Hfusion.at<1>();
|
||||
Hfusion_sympy(2) = Hfusion.at<2>();
|
||||
Hfusion_sympy(3) = Hfusion.at<3>();
|
||||
Hfusion_sympy(4) = Hfusion.at<4>();
|
||||
Hfusion_sympy(5) = Hfusion.at<5>();
|
||||
Hfusion_sympy(6) = Hfusion.at<6>();
|
||||
Hfusion_sympy(22) = Hfusion.at<22>();
|
||||
Hfusion_sympy(23) = Hfusion.at<23>();
|
||||
Kfusion_sympy = Kfusion;
|
||||
|
||||
// repeat calculation using matlab generated equations
|
||||
|
||||
const float Kacc = Kaccy;
|
||||
|
||||
SH_ACC[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3);
|
||||
SH_ACC[1] = vn - vwn;
|
||||
SH_ACC[2] = ve - vwe;
|
||||
H_ACC(0) = -Kacc*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd);
|
||||
H_ACC(1) = -Kacc*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd);
|
||||
H_ACC(2) = -Kacc*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd);
|
||||
H_ACC(3) = Kacc*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd);
|
||||
H_ACC(4) = Kacc*(2.0f*q0*q3 - 2.0f*q1*q2);
|
||||
H_ACC(5) = -Kacc*SH_ACC[0];
|
||||
H_ACC(6) = -Kacc*(2.0f*q0*q1 + 2.0f*q2*q3);
|
||||
H_ACC(22) = -2.0f*Kacc*(q0*q3 - q1*q2);
|
||||
H_ACC(23) = Kacc*SH_ACC[0];
|
||||
drag_innov_var = (R_ACC + Kacc*SH_ACC[0]*(Kacc*P(5,5)*SH_ACC[0] - Kacc*P(23,5)*SH_ACC[0] - Kacc*P(4,5)*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P(6,5)*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P(22,5)*(q0*q3 - q1*q2) + Kacc*P(0,5)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P(1,5)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(2,5)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(3,5)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) - Kacc*SH_ACC[0]*(Kacc*P(5,23)*SH_ACC[0] - Kacc*P(23,23)*SH_ACC[0] - Kacc*P(4,23)*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P(6,23)*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P(22,23)*(q0*q3 - q1*q2) + Kacc*P(0,23)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P(1,23)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(2,23)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(3,23)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) - Kacc*(2.0f*q0*q3 - 2.0f*q1*q2)*(Kacc*P(5,4)*SH_ACC[0] - Kacc*P(23,4)*SH_ACC[0] - Kacc*P(4,4)*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P(6,4)*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P(22,4)*(q0*q3 - q1*q2) + Kacc*P(0,4)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P(1,4)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(2,4)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(3,4)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) + Kacc*(2.0f*q0*q1 + 2.0f*q2*q3)*(Kacc*P(5,6)*SH_ACC[0] - Kacc*P(23,6)*SH_ACC[0] - Kacc*P(4,6)*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P(6,6)*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P(22,6)*(q0*q3 - q1*q2) + Kacc*P(0,6)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P(1,6)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(2,6)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(3,6)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) + 2*Kacc*(q0*q3 - q1*q2)*(Kacc*P(5,22)*SH_ACC[0] - Kacc*P(23,22)*SH_ACC[0] - Kacc*P(4,22)*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P(6,22)*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P(22,22)*(q0*q3 - q1*q2) + Kacc*P(0,22)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P(1,22)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(2,22)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(3,22)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) + Kacc*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)*(Kacc*P(5,0)*SH_ACC[0] - Kacc*P(23,0)*SH_ACC[0] - Kacc*P(4,0)*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P(6,0)*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P(22,0)*(q0*q3 - q1*q2) + Kacc*P(0,0)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P(1,0)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(2,0)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(3,0)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) + Kacc*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd)*(Kacc*P(5,1)*SH_ACC[0] - Kacc*P(23,1)*SH_ACC[0] - Kacc*P(4,1)*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P(6,1)*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P(22,1)*(q0*q3 - q1*q2) + Kacc*P(0,1)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P(1,1)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(2,1)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(3,1)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) + Kacc*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd)*(Kacc*P(5,2)*SH_ACC[0] - Kacc*P(23,2)*SH_ACC[0] - Kacc*P(4,2)*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P(6,2)*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P(22,2)*(q0*q3 - q1*q2) + Kacc*P(0,2)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P(1,2)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(2,2)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(3,2)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) - Kacc*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)*(Kacc*P(5,3)*SH_ACC[0] - Kacc*P(23,3)*SH_ACC[0] - Kacc*P(4,3)*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P(6,3)*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P(22,3)*(q0*q3 - q1*q2) + Kacc*P(0,3)*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P(1,3)*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P(2,3)*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P(3,3)*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)));
|
||||
SK_ACC[0] = 1.0f/drag_innov_var;
|
||||
SK_ACC[1] = 2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd;
|
||||
SK_ACC[2] = 2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd;
|
||||
SK_ACC[3] = 2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd;
|
||||
SK_ACC[4] = 2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd;
|
||||
SK_ACC[5] = 2.0f*q0*q3 - 2.0f*q1*q2;
|
||||
SK_ACC[6] = q0*q3 - q1*q2;
|
||||
SK_ACC[7] = 2.0f*q0*q1 + 2.0f*q2*q3;
|
||||
SK_ACC[8] = SH_ACC[0];
|
||||
|
||||
Kfusion(0) = -SK_ACC[0]*(Kacc*P(0,0)*SK_ACC[3] + Kacc*P(0,1)*SK_ACC[2] - Kacc*P(0,3)*SK_ACC[1] + Kacc*P(0,2)*SK_ACC[4] - Kacc*P(0,4)*SK_ACC[5] + Kacc*P(0,5)*SK_ACC[8] + Kacc*P(0,6)*SK_ACC[7] + 2*Kacc*P(0,22)*SK_ACC[6] - Kacc*P(0,23)*SK_ACC[8]);
|
||||
Kfusion(1) = -SK_ACC[0]*(Kacc*P(1,0)*SK_ACC[3] + Kacc*P(1,1)*SK_ACC[2] - Kacc*P(1,3)*SK_ACC[1] + Kacc*P(1,2)*SK_ACC[4] - Kacc*P(1,4)*SK_ACC[5] + Kacc*P(1,5)*SK_ACC[8] + Kacc*P(1,6)*SK_ACC[7] + 2*Kacc*P(1,22)*SK_ACC[6] - Kacc*P(1,23)*SK_ACC[8]);
|
||||
Kfusion(2) = -SK_ACC[0]*(Kacc*P(2,0)*SK_ACC[3] + Kacc*P(2,1)*SK_ACC[2] - Kacc*P(2,3)*SK_ACC[1] + Kacc*P(2,2)*SK_ACC[4] - Kacc*P(2,4)*SK_ACC[5] + Kacc*P(2,5)*SK_ACC[8] + Kacc*P(2,6)*SK_ACC[7] + 2*Kacc*P(2,22)*SK_ACC[6] - Kacc*P(2,23)*SK_ACC[8]);
|
||||
Kfusion(3) = -SK_ACC[0]*(Kacc*P(3,0)*SK_ACC[3] + Kacc*P(3,1)*SK_ACC[2] - Kacc*P(3,3)*SK_ACC[1] + Kacc*P(3,2)*SK_ACC[4] - Kacc*P(3,4)*SK_ACC[5] + Kacc*P(3,5)*SK_ACC[8] + Kacc*P(3,6)*SK_ACC[7] + 2*Kacc*P(3,22)*SK_ACC[6] - Kacc*P(3,23)*SK_ACC[8]);
|
||||
Kfusion(4) = -SK_ACC[0]*(Kacc*P(4,0)*SK_ACC[3] + Kacc*P(4,1)*SK_ACC[2] - Kacc*P(4,3)*SK_ACC[1] + Kacc*P(4,2)*SK_ACC[4] - Kacc*P(4,4)*SK_ACC[5] + Kacc*P(4,5)*SK_ACC[8] + Kacc*P(4,6)*SK_ACC[7] + 2*Kacc*P(4,22)*SK_ACC[6] - Kacc*P(4,23)*SK_ACC[8]);
|
||||
Kfusion(5) = -SK_ACC[0]*(Kacc*P(5,0)*SK_ACC[3] + Kacc*P(5,1)*SK_ACC[2] - Kacc*P(5,3)*SK_ACC[1] + Kacc*P(5,2)*SK_ACC[4] - Kacc*P(5,4)*SK_ACC[5] + Kacc*P(5,5)*SK_ACC[8] + Kacc*P(5,6)*SK_ACC[7] + 2*Kacc*P(5,22)*SK_ACC[6] - Kacc*P(5,23)*SK_ACC[8]);
|
||||
Kfusion(6) = -SK_ACC[0]*(Kacc*P(6,0)*SK_ACC[3] + Kacc*P(6,1)*SK_ACC[2] - Kacc*P(6,3)*SK_ACC[1] + Kacc*P(6,2)*SK_ACC[4] - Kacc*P(6,4)*SK_ACC[5] + Kacc*P(6,5)*SK_ACC[8] + Kacc*P(6,6)*SK_ACC[7] + 2*Kacc*P(6,22)*SK_ACC[6] - Kacc*P(6,23)*SK_ACC[8]);
|
||||
Kfusion(7) = -SK_ACC[0]*(Kacc*P(7,0)*SK_ACC[3] + Kacc*P(7,1)*SK_ACC[2] - Kacc*P(7,3)*SK_ACC[1] + Kacc*P(7,2)*SK_ACC[4] - Kacc*P(7,4)*SK_ACC[5] + Kacc*P(7,5)*SK_ACC[8] + Kacc*P(7,6)*SK_ACC[7] + 2*Kacc*P(7,22)*SK_ACC[6] - Kacc*P(7,23)*SK_ACC[8]);
|
||||
Kfusion(8) = -SK_ACC[0]*(Kacc*P(8,0)*SK_ACC[3] + Kacc*P(8,1)*SK_ACC[2] - Kacc*P(8,3)*SK_ACC[1] + Kacc*P(8,2)*SK_ACC[4] - Kacc*P(8,4)*SK_ACC[5] + Kacc*P(8,5)*SK_ACC[8] + Kacc*P(8,6)*SK_ACC[7] + 2*Kacc*P(8,22)*SK_ACC[6] - Kacc*P(8,23)*SK_ACC[8]);
|
||||
Kfusion(9) = -SK_ACC[0]*(Kacc*P(9,0)*SK_ACC[3] + Kacc*P(9,1)*SK_ACC[2] - Kacc*P(9,3)*SK_ACC[1] + Kacc*P(9,2)*SK_ACC[4] - Kacc*P(9,4)*SK_ACC[5] + Kacc*P(9,5)*SK_ACC[8] + Kacc*P(9,6)*SK_ACC[7] + 2*Kacc*P(9,22)*SK_ACC[6] - Kacc*P(9,23)*SK_ACC[8]);
|
||||
Kfusion(10) = -SK_ACC[0]*(Kacc*P(10,0)*SK_ACC[3] + Kacc*P(10,1)*SK_ACC[2] - Kacc*P(10,3)*SK_ACC[1] + Kacc*P(10,2)*SK_ACC[4] - Kacc*P(10,4)*SK_ACC[5] + Kacc*P(10,5)*SK_ACC[8] + Kacc*P(10,6)*SK_ACC[7] + 2*Kacc*P(10,22)*SK_ACC[6] - Kacc*P(10,23)*SK_ACC[8]);
|
||||
Kfusion(11) = -SK_ACC[0]*(Kacc*P(11,0)*SK_ACC[3] + Kacc*P(11,1)*SK_ACC[2] - Kacc*P(11,3)*SK_ACC[1] + Kacc*P(11,2)*SK_ACC[4] - Kacc*P(11,4)*SK_ACC[5] + Kacc*P(11,5)*SK_ACC[8] + Kacc*P(11,6)*SK_ACC[7] + 2*Kacc*P(11,22)*SK_ACC[6] - Kacc*P(11,23)*SK_ACC[8]);
|
||||
Kfusion(12) = -SK_ACC[0]*(Kacc*P(12,0)*SK_ACC[3] + Kacc*P(12,1)*SK_ACC[2] - Kacc*P(12,3)*SK_ACC[1] + Kacc*P(12,2)*SK_ACC[4] - Kacc*P(12,4)*SK_ACC[5] + Kacc*P(12,5)*SK_ACC[8] + Kacc*P(12,6)*SK_ACC[7] + 2*Kacc*P(12,22)*SK_ACC[6] - Kacc*P(12,23)*SK_ACC[8]);
|
||||
Kfusion(13) = -SK_ACC[0]*(Kacc*P(13,0)*SK_ACC[3] + Kacc*P(13,1)*SK_ACC[2] - Kacc*P(13,3)*SK_ACC[1] + Kacc*P(13,2)*SK_ACC[4] - Kacc*P(13,4)*SK_ACC[5] + Kacc*P(13,5)*SK_ACC[8] + Kacc*P(13,6)*SK_ACC[7] + 2*Kacc*P(13,22)*SK_ACC[6] - Kacc*P(13,23)*SK_ACC[8]);
|
||||
Kfusion(14) = -SK_ACC[0]*(Kacc*P(14,0)*SK_ACC[3] + Kacc*P(14,1)*SK_ACC[2] - Kacc*P(14,3)*SK_ACC[1] + Kacc*P(14,2)*SK_ACC[4] - Kacc*P(14,4)*SK_ACC[5] + Kacc*P(14,5)*SK_ACC[8] + Kacc*P(14,6)*SK_ACC[7] + 2*Kacc*P(14,22)*SK_ACC[6] - Kacc*P(14,23)*SK_ACC[8]);
|
||||
Kfusion(15) = -SK_ACC[0]*(Kacc*P(15,0)*SK_ACC[3] + Kacc*P(15,1)*SK_ACC[2] - Kacc*P(15,3)*SK_ACC[1] + Kacc*P(15,2)*SK_ACC[4] - Kacc*P(15,4)*SK_ACC[5] + Kacc*P(15,5)*SK_ACC[8] + Kacc*P(15,6)*SK_ACC[7] + 2*Kacc*P(15,22)*SK_ACC[6] - Kacc*P(15,23)*SK_ACC[8]);
|
||||
Kfusion(16) = -SK_ACC[0]*(Kacc*P(16,0)*SK_ACC[3] + Kacc*P(16,1)*SK_ACC[2] - Kacc*P(16,3)*SK_ACC[1] + Kacc*P(16,2)*SK_ACC[4] - Kacc*P(16,4)*SK_ACC[5] + Kacc*P(16,5)*SK_ACC[8] + Kacc*P(16,6)*SK_ACC[7] + 2*Kacc*P(16,22)*SK_ACC[6] - Kacc*P(16,23)*SK_ACC[8]);
|
||||
Kfusion(17) = -SK_ACC[0]*(Kacc*P(17,0)*SK_ACC[3] + Kacc*P(17,1)*SK_ACC[2] - Kacc*P(17,3)*SK_ACC[1] + Kacc*P(17,2)*SK_ACC[4] - Kacc*P(17,4)*SK_ACC[5] + Kacc*P(17,5)*SK_ACC[8] + Kacc*P(17,6)*SK_ACC[7] + 2*Kacc*P(17,22)*SK_ACC[6] - Kacc*P(17,23)*SK_ACC[8]);
|
||||
Kfusion(18) = -SK_ACC[0]*(Kacc*P(18,0)*SK_ACC[3] + Kacc*P(18,1)*SK_ACC[2] - Kacc*P(18,3)*SK_ACC[1] + Kacc*P(18,2)*SK_ACC[4] - Kacc*P(18,4)*SK_ACC[5] + Kacc*P(18,5)*SK_ACC[8] + Kacc*P(18,6)*SK_ACC[7] + 2*Kacc*P(18,22)*SK_ACC[6] - Kacc*P(18,23)*SK_ACC[8]);
|
||||
Kfusion(19) = -SK_ACC[0]*(Kacc*P(19,0)*SK_ACC[3] + Kacc*P(19,1)*SK_ACC[2] - Kacc*P(19,3)*SK_ACC[1] + Kacc*P(19,2)*SK_ACC[4] - Kacc*P(19,4)*SK_ACC[5] + Kacc*P(19,5)*SK_ACC[8] + Kacc*P(19,6)*SK_ACC[7] + 2*Kacc*P(19,22)*SK_ACC[6] - Kacc*P(19,23)*SK_ACC[8]);
|
||||
Kfusion(20) = -SK_ACC[0]*(Kacc*P(20,0)*SK_ACC[3] + Kacc*P(20,1)*SK_ACC[2] - Kacc*P(20,3)*SK_ACC[1] + Kacc*P(20,2)*SK_ACC[4] - Kacc*P(20,4)*SK_ACC[5] + Kacc*P(20,5)*SK_ACC[8] + Kacc*P(20,6)*SK_ACC[7] + 2*Kacc*P(20,22)*SK_ACC[6] - Kacc*P(20,23)*SK_ACC[8]);
|
||||
Kfusion(21) = -SK_ACC[0]*(Kacc*P(21,0)*SK_ACC[3] + Kacc*P(21,1)*SK_ACC[2] - Kacc*P(21,3)*SK_ACC[1] + Kacc*P(21,2)*SK_ACC[4] - Kacc*P(21,4)*SK_ACC[5] + Kacc*P(21,5)*SK_ACC[8] + Kacc*P(21,6)*SK_ACC[7] + 2*Kacc*P(21,22)*SK_ACC[6] - Kacc*P(21,23)*SK_ACC[8]);
|
||||
Kfusion(22) = -SK_ACC[0]*(Kacc*P(22,0)*SK_ACC[3] + Kacc*P(22,1)*SK_ACC[2] - Kacc*P(22,3)*SK_ACC[1] + Kacc*P(22,2)*SK_ACC[4] - Kacc*P(22,4)*SK_ACC[5] + Kacc*P(22,5)*SK_ACC[8] + Kacc*P(22,6)*SK_ACC[7] + 2*Kacc*P(22,22)*SK_ACC[6] - Kacc*P(22,23)*SK_ACC[8]);
|
||||
Kfusion(23) = -SK_ACC[0]*(Kacc*P(23,0)*SK_ACC[3] + Kacc*P(23,1)*SK_ACC[2] - Kacc*P(23,3)*SK_ACC[1] + Kacc*P(23,2)*SK_ACC[4] - Kacc*P(23,4)*SK_ACC[5] + Kacc*P(23,5)*SK_ACC[8] + Kacc*P(23,6)*SK_ACC[7] + 2*Kacc*P(23,22)*SK_ACC[6] - Kacc*P(23,23)*SK_ACC[8]);
|
||||
|
||||
Hfusion_matlab = H_ACC;
|
||||
Kfusion_matlab = Kfusion;
|
||||
|
||||
// find largest observation variance difference as a fraction of the matlab value
|
||||
float max_diff_fraction = 0.0f;
|
||||
int max_row;
|
||||
float max_old, max_new;
|
||||
for (int row=0; row<24; row++) {
|
||||
float diff_fraction;
|
||||
if (Hfusion_matlab(row) != 0.0f) {
|
||||
diff_fraction = fabsf(Hfusion_sympy(row) - Hfusion_matlab(row)) / fabsf(Hfusion_matlab(row));
|
||||
} else if (Hfusion_sympy(row) != 0.0f) {
|
||||
diff_fraction = fabsf(Hfusion_sympy(row) - H_ACC(row)) / fabsf(Hfusion_sympy(row));
|
||||
} else {
|
||||
diff_fraction = 0.0f;
|
||||
}
|
||||
if (diff_fraction > max_diff_fraction) {
|
||||
max_diff_fraction = diff_fraction;
|
||||
max_row = row;
|
||||
max_old = H_ACC(row);
|
||||
max_new = Hfusion_sympy(row);
|
||||
}
|
||||
}
|
||||
|
||||
if (max_diff_fraction > 1e-5f) {
|
||||
printf("Fail: Specific Force Y axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
|
||||
} else {
|
||||
printf("Pass: Specific Force Y axis Hfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
|
||||
// find largest Kalman gain difference as a fraction of the matlab value
|
||||
max_diff_fraction = 0.0f;
|
||||
for (int row=0; row<4; row++) {
|
||||
float diff_fraction;
|
||||
if (Kfusion(row) != 0.0f) {
|
||||
diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion(row)) / fabsf(Kfusion(row));
|
||||
} else if (Kfusion_sympy(row) != 0.0f) {
|
||||
diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion(row)) / fabsf(Kfusion_sympy(row));
|
||||
} else {
|
||||
diff_fraction = 0.0f;
|
||||
}
|
||||
if (diff_fraction > max_diff_fraction) {
|
||||
max_diff_fraction = diff_fraction;
|
||||
max_row = row;
|
||||
max_old = Kfusion(row);
|
||||
max_new = Kfusion_sympy(row);
|
||||
}
|
||||
}
|
||||
|
||||
if (max_diff_fraction > 1e-5f) {
|
||||
printf("Fail: Specific Force Y axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
|
||||
} else {
|
||||
printf("Pass: Specific Force Y axis Kfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,195 @@
|
||||
// Axis 0 equations
|
||||
// Sub Expressions
|
||||
const float HK0 = q2*vd;
|
||||
const float HK1 = ve - vwe;
|
||||
const float HK2 = HK1*q3;
|
||||
const float HK3 = HK0 - HK2;
|
||||
const float HK4 = 2*Kaccx;
|
||||
const float HK5 = HK1*q2 + q3*vd;
|
||||
const float HK6 = q0*vd;
|
||||
const float HK7 = HK1*q1;
|
||||
const float HK8 = 2*vn - 2*vwn;
|
||||
const float HK9 = HK8*q2;
|
||||
const float HK10 = HK6 - HK7 + HK9;
|
||||
const float HK11 = HK1*q0 - HK8*q3 + q1*vd;
|
||||
const float HK12 = 2*powf(q2, 2);
|
||||
const float HK13 = 2*powf(q3, 2);
|
||||
const float HK14 = HK12 + HK13 - 1;
|
||||
const float HK15 = HK14*Kaccx;
|
||||
const float HK16 = q0*q3 + q1*q2;
|
||||
const float HK17 = HK16*HK4;
|
||||
const float HK18 = q0*q2;
|
||||
const float HK19 = q1*q3;
|
||||
const float HK20 = HK18 - HK19;
|
||||
const float HK21 = 2*HK3;
|
||||
const float HK22 = 2*HK10;
|
||||
const float HK23 = 2*HK20;
|
||||
const float HK24 = 2*HK16;
|
||||
const float HK25 = 2*HK5;
|
||||
const float HK26 = 2*HK11;
|
||||
const float HK27 = HK14*P(0,22) - HK24*P(0,23) + HK24*P(0,5) + HK25*P(0,1) + HK26*P(0,3);
|
||||
const float HK28 = -HK12 - HK13 + 1;
|
||||
const float HK29 = -2*HK0 + 2*HK2;
|
||||
const float HK30 = -2*HK6 + 2*HK7 - 2*HK9;
|
||||
const float HK31 = -2*HK18 + 2*HK19;
|
||||
const float HK32 = HK24*P(5,23);
|
||||
const float HK33 = HK14*P(22,23) - HK24*P(23,23) + HK25*P(1,23) + HK26*P(3,23) + HK32;
|
||||
const float HK34 = powf(Kaccx, 2);
|
||||
const float HK35 = HK24*HK34;
|
||||
const float HK36 = HK14*P(5,22) + HK24*P(5,5) + HK25*P(1,5) + HK26*P(3,5) - HK32;
|
||||
const float HK37 = HK14*P(6,22) + HK24*P(5,6) - HK24*P(6,23) + HK25*P(1,6) + HK26*P(3,6);
|
||||
const float HK38 = HK14*P(1,22) - HK24*P(1,23) + HK24*P(1,5) + HK25*P(1,1) + HK26*P(1,3);
|
||||
const float HK39 = HK14*P(4,22);
|
||||
const float HK40 = -HK24*P(4,23) + HK24*P(4,5) + HK25*P(1,4) + HK26*P(3,4) + HK39;
|
||||
const float HK41 = HK14*HK34;
|
||||
const float HK42 = HK14*P(22,22) - HK24*P(22,23) + HK24*P(5,22) + HK25*P(1,22) + HK26*P(3,22);
|
||||
const float HK43 = HK14*P(3,22) - HK24*P(3,23) + HK24*P(3,5) + HK25*P(1,3) + HK26*P(3,3);
|
||||
const float HK44 = HK14*P(2,22) - HK24*P(2,23) + HK24*P(2,5) + HK25*P(1,2) + HK26*P(2,3);
|
||||
const float HK45 = Kaccx/(HK21*HK34*(HK27 + HK28*P(0,4) + HK29*P(0,0) + HK30*P(0,2) + HK31*P(0,6)) + HK22*HK34*(HK28*P(2,4) + HK29*P(0,2) + HK30*P(2,2) + HK31*P(2,6) + HK44) + HK23*HK34*(HK28*P(4,6) + HK29*P(0,6) + HK30*P(2,6) + HK31*P(6,6) + HK37) - HK25*HK34*(HK28*P(1,4) + HK29*P(0,1) + HK30*P(1,2) + HK31*P(1,6) + HK38) - HK26*HK34*(HK28*P(3,4) + HK29*P(0,3) + HK30*P(2,3) + HK31*P(3,6) + HK43) + HK35*(HK28*P(4,23) + HK29*P(0,23) + HK30*P(2,23) + HK31*P(6,23) + HK33) - HK35*(HK28*P(4,5) + HK29*P(0,5) + HK30*P(2,5) + HK31*P(5,6) + HK36) - HK41*(HK28*P(4,22) + HK29*P(0,22) + HK30*P(2,22) + HK31*P(6,22) + HK42) + HK41*(HK28*P(4,4) + HK29*P(0,4) + HK30*P(2,4) + HK31*P(4,6) + HK40) - R_ACC);
|
||||
|
||||
|
||||
// Observation Jacobians
|
||||
Hfusion.at<0>() = HK3*HK4;
|
||||
Hfusion.at<1>() = -HK4*HK5;
|
||||
Hfusion.at<2>() = HK10*HK4;
|
||||
Hfusion.at<3>() = -HK11*HK4;
|
||||
Hfusion.at<4>() = HK15;
|
||||
Hfusion.at<5>() = -HK17;
|
||||
Hfusion.at<6>() = HK20*HK4;
|
||||
Hfusion.at<7>() = 0;
|
||||
Hfusion.at<8>() = 0;
|
||||
Hfusion.at<9>() = 0;
|
||||
Hfusion.at<10>() = 0;
|
||||
Hfusion.at<11>() = 0;
|
||||
Hfusion.at<12>() = 0;
|
||||
Hfusion.at<13>() = 0;
|
||||
Hfusion.at<14>() = 0;
|
||||
Hfusion.at<15>() = 0;
|
||||
Hfusion.at<16>() = 0;
|
||||
Hfusion.at<17>() = 0;
|
||||
Hfusion.at<18>() = 0;
|
||||
Hfusion.at<19>() = 0;
|
||||
Hfusion.at<20>() = 0;
|
||||
Hfusion.at<21>() = 0;
|
||||
Hfusion.at<22>() = -HK15;
|
||||
Hfusion.at<23>() = HK17;
|
||||
|
||||
|
||||
// Kalman gains
|
||||
Kfusion(0) = HK45*(-HK14*P(0,4) - HK21*P(0,0) - HK22*P(0,2) - HK23*P(0,6) + HK27);
|
||||
Kfusion(1) = HK45*(-HK14*P(1,4) - HK21*P(0,1) - HK22*P(1,2) - HK23*P(1,6) + HK38);
|
||||
Kfusion(2) = HK45*(-HK14*P(2,4) - HK21*P(0,2) - HK22*P(2,2) - HK23*P(2,6) + HK44);
|
||||
Kfusion(3) = HK45*(-HK14*P(3,4) - HK21*P(0,3) - HK22*P(2,3) - HK23*P(3,6) + HK43);
|
||||
Kfusion(4) = HK45*(-HK14*P(4,4) - HK21*P(0,4) - HK22*P(2,4) - HK23*P(4,6) + HK40);
|
||||
Kfusion(5) = HK45*(-HK14*P(4,5) - HK21*P(0,5) - HK22*P(2,5) - HK23*P(5,6) + HK36);
|
||||
Kfusion(6) = HK45*(-HK14*P(4,6) - HK21*P(0,6) - HK22*P(2,6) - HK23*P(6,6) + HK37);
|
||||
Kfusion(7) = HK45*(-HK14*P(4,7) + HK14*P(7,22) - HK21*P(0,7) - HK22*P(2,7) - HK23*P(6,7) + HK24*P(5,7) - HK24*P(7,23) + HK25*P(1,7) + HK26*P(3,7));
|
||||
Kfusion(8) = HK45*(-HK14*P(4,8) + HK14*P(8,22) - HK21*P(0,8) - HK22*P(2,8) - HK23*P(6,8) + HK24*P(5,8) - HK24*P(8,23) + HK25*P(1,8) + HK26*P(3,8));
|
||||
Kfusion(9) = HK45*(-HK14*P(4,9) + HK14*P(9,22) - HK21*P(0,9) - HK22*P(2,9) - HK23*P(6,9) + HK24*P(5,9) - HK24*P(9,23) + HK25*P(1,9) + HK26*P(3,9));
|
||||
Kfusion(10) = HK45*(HK14*P(10,22) - HK14*P(4,10) - HK21*P(0,10) - HK22*P(2,10) - HK23*P(6,10) - HK24*P(10,23) + HK24*P(5,10) + HK25*P(1,10) + HK26*P(3,10));
|
||||
Kfusion(11) = HK45*(HK14*P(11,22) - HK14*P(4,11) - HK21*P(0,11) - HK22*P(2,11) - HK23*P(6,11) - HK24*P(11,23) + HK24*P(5,11) + HK25*P(1,11) + HK26*P(3,11));
|
||||
Kfusion(12) = HK45*(HK14*P(12,22) - HK14*P(4,12) - HK21*P(0,12) - HK22*P(2,12) - HK23*P(6,12) - HK24*P(12,23) + HK24*P(5,12) + HK25*P(1,12) + HK26*P(3,12));
|
||||
Kfusion(13) = HK45*(HK14*P(13,22) - HK14*P(4,13) - HK21*P(0,13) - HK22*P(2,13) - HK23*P(6,13) - HK24*P(13,23) + HK24*P(5,13) + HK25*P(1,13) + HK26*P(3,13));
|
||||
Kfusion(14) = HK45*(HK14*P(14,22) - HK14*P(4,14) - HK21*P(0,14) - HK22*P(2,14) - HK23*P(6,14) - HK24*P(14,23) + HK24*P(5,14) + HK25*P(1,14) + HK26*P(3,14));
|
||||
Kfusion(15) = HK45*(HK14*P(15,22) - HK14*P(4,15) - HK21*P(0,15) - HK22*P(2,15) - HK23*P(6,15) - HK24*P(15,23) + HK24*P(5,15) + HK25*P(1,15) + HK26*P(3,15));
|
||||
Kfusion(16) = HK45*(HK14*P(16,22) - HK14*P(4,16) - HK21*P(0,16) - HK22*P(2,16) - HK23*P(6,16) - HK24*P(16,23) + HK24*P(5,16) + HK25*P(1,16) + HK26*P(3,16));
|
||||
Kfusion(17) = HK45*(HK14*P(17,22) - HK14*P(4,17) - HK21*P(0,17) - HK22*P(2,17) - HK23*P(6,17) - HK24*P(17,23) + HK24*P(5,17) + HK25*P(1,17) + HK26*P(3,17));
|
||||
Kfusion(18) = HK45*(HK14*P(18,22) - HK14*P(4,18) - HK21*P(0,18) - HK22*P(2,18) - HK23*P(6,18) - HK24*P(18,23) + HK24*P(5,18) + HK25*P(1,18) + HK26*P(3,18));
|
||||
Kfusion(19) = HK45*(HK14*P(19,22) - HK14*P(4,19) - HK21*P(0,19) - HK22*P(2,19) - HK23*P(6,19) - HK24*P(19,23) + HK24*P(5,19) + HK25*P(1,19) + HK26*P(3,19));
|
||||
Kfusion(20) = HK45*(HK14*P(20,22) - HK14*P(4,20) - HK21*P(0,20) - HK22*P(2,20) - HK23*P(6,20) - HK24*P(20,23) + HK24*P(5,20) + HK25*P(1,20) + HK26*P(3,20));
|
||||
Kfusion(21) = HK45*(HK14*P(21,22) - HK14*P(4,21) - HK21*P(0,21) - HK22*P(2,21) - HK23*P(6,21) - HK24*P(21,23) + HK24*P(5,21) + HK25*P(1,21) + HK26*P(3,21));
|
||||
Kfusion(22) = HK45*(-HK21*P(0,22) - HK22*P(2,22) - HK23*P(6,22) - HK39 + HK42);
|
||||
Kfusion(23) = HK45*(-HK14*P(4,23) - HK21*P(0,23) - HK22*P(2,23) - HK23*P(6,23) + HK33);
|
||||
|
||||
|
||||
// Axis 1 equations
|
||||
// Sub Expressions
|
||||
const float HK0 = vn - vwn;
|
||||
const float HK1 = -HK0*q3 + q1*vd;
|
||||
const float HK2 = 2*Kaccy;
|
||||
const float HK3 = 2*ve - 2*vwe;
|
||||
const float HK4 = HK0*q2 - HK3*q1 + q0*vd;
|
||||
const float HK5 = HK0*q1 + q3*vd;
|
||||
const float HK6 = HK0*q0 + HK3*q3 - q2*vd;
|
||||
const float HK7 = q0*q3 - q1*q2;
|
||||
const float HK8 = HK2*HK7;
|
||||
const float HK9 = 2*powf(q1, 2) + 2*powf(q3, 2) - 1;
|
||||
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*HK1;
|
||||
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 = 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 = 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 HK26 = HK9*P(5,23);
|
||||
const float HK27 = 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) - HK26 + HK9*P(23,23);
|
||||
const float HK28 = HK19*HK9;
|
||||
const float HK29 = 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) + HK26 - HK9*P(5,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*HK25 + HK15*HK18*HK19 + HK16*HK19*HK30 - HK17*HK19*HK31 + HK22*HK23 - HK23*HK24 + HK27*HK28 - HK28*HK29 + R_ACC);
|
||||
|
||||
|
||||
// Observation Jacobians
|
||||
Hfusion.at<0>() = -HK1*HK2;
|
||||
Hfusion.at<1>() = -HK2*HK4;
|
||||
Hfusion.at<2>() = -HK2*HK5;
|
||||
Hfusion.at<3>() = HK2*HK6;
|
||||
Hfusion.at<4>() = HK8;
|
||||
Hfusion.at<5>() = HK10;
|
||||
Hfusion.at<6>() = -HK11*HK2;
|
||||
Hfusion.at<7>() = 0;
|
||||
Hfusion.at<8>() = 0;
|
||||
Hfusion.at<9>() = 0;
|
||||
Hfusion.at<10>() = 0;
|
||||
Hfusion.at<11>() = 0;
|
||||
Hfusion.at<12>() = 0;
|
||||
Hfusion.at<13>() = 0;
|
||||
Hfusion.at<14>() = 0;
|
||||
Hfusion.at<15>() = 0;
|
||||
Hfusion.at<16>() = 0;
|
||||
Hfusion.at<17>() = 0;
|
||||
Hfusion.at<18>() = 0;
|
||||
Hfusion.at<19>() = 0;
|
||||
Hfusion.at<20>() = 0;
|
||||
Hfusion.at<21>() = 0;
|
||||
Hfusion.at<22>() = -HK8;
|
||||
Hfusion.at<23>() = -HK10;
|
||||
|
||||
|
||||
// Kalman gains
|
||||
Kfusion(0) = -HK18*HK32;
|
||||
Kfusion(1) = -HK30*HK32;
|
||||
Kfusion(2) = -HK25*HK32;
|
||||
Kfusion(3) = -HK31*HK32;
|
||||
Kfusion(4) = -HK24*HK32;
|
||||
Kfusion(5) = -HK29*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) = -HK27*HK32;
|
||||
|
||||
|
||||
@@ -0,0 +1,202 @@
|
||||
// Sub Expressions
|
||||
const float HK0 = q2*vd;
|
||||
const float HK1 = ve - vwe;
|
||||
const float HK2 = HK1*q3;
|
||||
const float HK3 = HK0 - HK2;
|
||||
const float HK4 = 2*Kaccx;
|
||||
const float HK5 = q3*vd;
|
||||
const float HK6 = HK1*q2 + HK5;
|
||||
const float HK7 = q0*vd;
|
||||
const float HK8 = HK1*q1;
|
||||
const float HK9 = vn - vwn;
|
||||
const float HK10 = HK9*q2;
|
||||
const float HK11 = 2*HK10;
|
||||
const float HK12 = HK11 + HK7 - HK8;
|
||||
const float HK13 = q1*vd;
|
||||
const float HK14 = HK9*q3;
|
||||
const float HK15 = HK1*q0 + HK13 - 2*HK14;
|
||||
const float HK16 = 2*powf(q2, 2);
|
||||
const float HK17 = 2*powf(q3, 2);
|
||||
const float HK18 = HK17 - 1;
|
||||
const float HK19 = HK16 + HK18;
|
||||
const float HK20 = HK19*Kaccx;
|
||||
const float HK21 = q0*q3;
|
||||
const float HK22 = q1*q2;
|
||||
const float HK23 = HK21 + HK22;
|
||||
const float HK24 = HK23*HK4;
|
||||
const float HK25 = q0*q2;
|
||||
const float HK26 = q1*q3;
|
||||
const float HK27 = HK25 - HK26;
|
||||
const float HK28 = 2*HK3;
|
||||
const float HK29 = 2*HK12;
|
||||
const float HK30 = 2*HK27;
|
||||
const float HK31 = 2*HK23;
|
||||
const float HK32 = 2*HK6;
|
||||
const float HK33 = 2*HK15;
|
||||
const float HK34 = HK19*P(0,22) - HK31*P(0,23) + HK31*P(0,5) + HK32*P(0,1) + HK33*P(0,3);
|
||||
const float HK35 = -HK16 - HK17 + 1;
|
||||
const float HK36 = -HK0;
|
||||
const float HK37 = 2*HK2 + 2*HK36;
|
||||
const float HK38 = -2*HK11 - 2*HK7 + 2*HK8;
|
||||
const float HK39 = -2*HK25 + 2*HK26;
|
||||
const float HK40 = HK31*P(5,23);
|
||||
const float HK41 = HK19*P(22,23) - HK31*P(23,23) + HK32*P(1,23) + HK33*P(3,23) + HK40;
|
||||
const float HK42 = powf(Kaccx, 2);
|
||||
const float HK43 = HK31*HK42;
|
||||
const float HK44 = HK19*P(5,22) + HK31*P(5,5) + HK32*P(1,5) + HK33*P(3,5) - HK40;
|
||||
const float HK45 = HK19*P(6,22) + HK31*P(5,6) - HK31*P(6,23) + HK32*P(1,6) + HK33*P(3,6);
|
||||
const float HK46 = HK19*P(1,22) - HK31*P(1,23) + HK31*P(1,5) + HK32*P(1,1) + HK33*P(1,3);
|
||||
const float HK47 = HK19*P(4,22);
|
||||
const float HK48 = -HK31*P(4,23) + HK31*P(4,5) + HK32*P(1,4) + HK33*P(3,4) + HK47;
|
||||
const float HK49 = HK19*HK42;
|
||||
const float HK50 = HK19*P(22,22) - HK31*P(22,23) + HK31*P(5,22) + HK32*P(1,22) + HK33*P(3,22);
|
||||
const float HK51 = HK19*P(3,22) - HK31*P(3,23) + HK31*P(3,5) + HK32*P(1,3) + HK33*P(3,3);
|
||||
const float HK52 = HK19*P(2,22) - HK31*P(2,23) + HK31*P(2,5) + HK32*P(1,2) + HK33*P(2,3);
|
||||
const float HK53 = Kaccx/(HK28*HK42*(HK34 + HK35*P(0,4) + HK37*P(0,0) + HK38*P(0,2) + HK39*P(0,6)) + HK29*HK42*(HK35*P(2,4) + HK37*P(0,2) + HK38*P(2,2) + HK39*P(2,6) + HK52) + HK30*HK42*(HK35*P(4,6) + HK37*P(0,6) + HK38*P(2,6) + HK39*P(6,6) + HK45) - HK32*HK42*(HK35*P(1,4) + HK37*P(0,1) + HK38*P(1,2) + HK39*P(1,6) + HK46) - HK33*HK42*(HK35*P(3,4) + HK37*P(0,3) + HK38*P(2,3) + HK39*P(3,6) + HK51) + HK43*(HK35*P(4,23) + HK37*P(0,23) + HK38*P(2,23) + HK39*P(6,23) + HK41) - HK43*(HK35*P(4,5) + HK37*P(0,5) + HK38*P(2,5) + HK39*P(5,6) + HK44) - HK49*(HK35*P(4,22) + HK37*P(0,22) + HK38*P(2,22) + HK39*P(6,22) + HK50) + HK49*(HK35*P(4,4) + HK37*P(0,4) + HK38*P(2,4) + HK39*P(4,6) + HK48) - R_ACC);
|
||||
const float HK54 = HK13 - HK14;
|
||||
const float HK55 = 2*Kaccy;
|
||||
const float HK56 = HK10 + HK7 - 2*HK8;
|
||||
const float HK57 = HK5 + HK9*q1;
|
||||
const float HK58 = 2*HK2 + HK36 + HK9*q0;
|
||||
const float HK59 = HK21 - HK22;
|
||||
const float HK60 = HK55*HK59;
|
||||
const float HK61 = HK18 + 2*powf(q1, 2);
|
||||
const float HK62 = HK61*Kaccy;
|
||||
const float HK63 = q0*q1 + q2*q3;
|
||||
const float HK64 = 2*HK63;
|
||||
const float HK65 = 2*HK59;
|
||||
const float HK66 = 2*HK57;
|
||||
const float HK67 = 2*HK54;
|
||||
const float HK68 = 2*HK56;
|
||||
const float HK69 = 2*HK58;
|
||||
const float HK70 = HK61*P(0,23) - HK61*P(0,5) + HK64*P(0,6) + HK65*P(0,22) - HK65*P(0,4) + HK66*P(0,2) + HK67*P(0,0) + HK68*P(0,1) - HK69*P(0,3);
|
||||
const float HK71 = powf(Kaccy, 2);
|
||||
const float HK72 = -HK61*P(5,6) + HK61*P(6,23) + HK64*P(6,6) - HK65*P(4,6) + HK65*P(6,22) + HK66*P(2,6) + HK67*P(0,6) + HK68*P(1,6) - HK69*P(3,6);
|
||||
const float HK73 = HK65*P(4,22);
|
||||
const float HK74 = HK61*P(22,23) - HK61*P(5,22) + HK64*P(6,22) + HK65*P(22,22) + HK66*P(2,22) + HK67*P(0,22) + HK68*P(1,22) - HK69*P(3,22) - HK73;
|
||||
const float HK75 = HK65*HK71;
|
||||
const float HK76 = HK61*P(4,23) - HK61*P(4,5) + HK64*P(4,6) - HK65*P(4,4) + HK66*P(2,4) + HK67*P(0,4) + HK68*P(1,4) - HK69*P(3,4) + HK73;
|
||||
const float HK77 = HK61*P(2,23) - HK61*P(2,5) + HK64*P(2,6) + HK65*P(2,22) - HK65*P(2,4) + HK66*P(2,2) + HK67*P(0,2) + HK68*P(1,2) - HK69*P(2,3);
|
||||
const float HK78 = HK61*P(5,23);
|
||||
const float HK79 = HK61*P(23,23) + HK64*P(6,23) + HK65*P(22,23) - HK65*P(4,23) + HK66*P(2,23) + HK67*P(0,23) + HK68*P(1,23) - HK69*P(3,23) - HK78;
|
||||
const float HK80 = HK61*HK71;
|
||||
const float HK81 = -HK61*P(5,5) + HK64*P(5,6) - HK65*P(4,5) + HK65*P(5,22) + HK66*P(2,5) + HK67*P(0,5) + HK68*P(1,5) - HK69*P(3,5) + HK78;
|
||||
const float HK82 = HK61*P(1,23) - HK61*P(1,5) + HK64*P(1,6) + HK65*P(1,22) - HK65*P(1,4) + HK66*P(1,2) + HK67*P(0,1) + HK68*P(1,1) - HK69*P(1,3);
|
||||
const float HK83 = HK61*P(3,23) - HK61*P(3,5) + HK64*P(3,6) + HK65*P(3,22) - HK65*P(3,4) + HK66*P(2,3) + HK67*P(0,3) + HK68*P(1,3) - HK69*P(3,3);
|
||||
const float HK84 = Kaccy/(HK64*HK71*HK72 + HK66*HK71*HK77 + HK67*HK70*HK71 + HK68*HK71*HK82 - HK69*HK71*HK83 + HK74*HK75 - HK75*HK76 + HK79*HK80 - HK80*HK81 + R_ACC);
|
||||
|
||||
|
||||
// Observation Jacobians - axis 0
|
||||
Hfusion.at<0>() = HK3*HK4;
|
||||
Hfusion.at<1>() = -HK4*HK6;
|
||||
Hfusion.at<2>() = HK12*HK4;
|
||||
Hfusion.at<3>() = -HK15*HK4;
|
||||
Hfusion.at<4>() = HK20;
|
||||
Hfusion.at<5>() = -HK24;
|
||||
Hfusion.at<6>() = HK27*HK4;
|
||||
Hfusion.at<7>() = 0;
|
||||
Hfusion.at<8>() = 0;
|
||||
Hfusion.at<9>() = 0;
|
||||
Hfusion.at<10>() = 0;
|
||||
Hfusion.at<11>() = 0;
|
||||
Hfusion.at<12>() = 0;
|
||||
Hfusion.at<13>() = 0;
|
||||
Hfusion.at<14>() = 0;
|
||||
Hfusion.at<15>() = 0;
|
||||
Hfusion.at<16>() = 0;
|
||||
Hfusion.at<17>() = 0;
|
||||
Hfusion.at<18>() = 0;
|
||||
Hfusion.at<19>() = 0;
|
||||
Hfusion.at<20>() = 0;
|
||||
Hfusion.at<21>() = 0;
|
||||
Hfusion.at<22>() = -HK20;
|
||||
Hfusion.at<23>() = HK24;
|
||||
|
||||
|
||||
// Kalman gains - axis 0
|
||||
Kfusion(0) = HK53*(-HK19*P(0,4) - HK28*P(0,0) - HK29*P(0,2) - HK30*P(0,6) + HK34);
|
||||
Kfusion(1) = HK53*(-HK19*P(1,4) - HK28*P(0,1) - HK29*P(1,2) - HK30*P(1,6) + HK46);
|
||||
Kfusion(2) = HK53*(-HK19*P(2,4) - HK28*P(0,2) - HK29*P(2,2) - HK30*P(2,6) + HK52);
|
||||
Kfusion(3) = HK53*(-HK19*P(3,4) - HK28*P(0,3) - HK29*P(2,3) - HK30*P(3,6) + HK51);
|
||||
Kfusion(4) = HK53*(-HK19*P(4,4) - HK28*P(0,4) - HK29*P(2,4) - HK30*P(4,6) + HK48);
|
||||
Kfusion(5) = HK53*(-HK19*P(4,5) - HK28*P(0,5) - HK29*P(2,5) - HK30*P(5,6) + HK44);
|
||||
Kfusion(6) = HK53*(-HK19*P(4,6) - HK28*P(0,6) - HK29*P(2,6) - HK30*P(6,6) + HK45);
|
||||
Kfusion(7) = HK53*(-HK19*P(4,7) + HK19*P(7,22) - HK28*P(0,7) - HK29*P(2,7) - HK30*P(6,7) + HK31*P(5,7) - HK31*P(7,23) + HK32*P(1,7) + HK33*P(3,7));
|
||||
Kfusion(8) = HK53*(-HK19*P(4,8) + HK19*P(8,22) - HK28*P(0,8) - HK29*P(2,8) - HK30*P(6,8) + HK31*P(5,8) - HK31*P(8,23) + HK32*P(1,8) + HK33*P(3,8));
|
||||
Kfusion(9) = HK53*(-HK19*P(4,9) + HK19*P(9,22) - HK28*P(0,9) - HK29*P(2,9) - HK30*P(6,9) + HK31*P(5,9) - HK31*P(9,23) + HK32*P(1,9) + HK33*P(3,9));
|
||||
Kfusion(10) = HK53*(HK19*P(10,22) - HK19*P(4,10) - HK28*P(0,10) - HK29*P(2,10) - HK30*P(6,10) - HK31*P(10,23) + HK31*P(5,10) + HK32*P(1,10) + HK33*P(3,10));
|
||||
Kfusion(11) = HK53*(HK19*P(11,22) - HK19*P(4,11) - HK28*P(0,11) - HK29*P(2,11) - HK30*P(6,11) - HK31*P(11,23) + HK31*P(5,11) + HK32*P(1,11) + HK33*P(3,11));
|
||||
Kfusion(12) = HK53*(HK19*P(12,22) - HK19*P(4,12) - HK28*P(0,12) - HK29*P(2,12) - HK30*P(6,12) - HK31*P(12,23) + HK31*P(5,12) + HK32*P(1,12) + HK33*P(3,12));
|
||||
Kfusion(13) = HK53*(HK19*P(13,22) - HK19*P(4,13) - HK28*P(0,13) - HK29*P(2,13) - HK30*P(6,13) - HK31*P(13,23) + HK31*P(5,13) + HK32*P(1,13) + HK33*P(3,13));
|
||||
Kfusion(14) = HK53*(HK19*P(14,22) - HK19*P(4,14) - HK28*P(0,14) - HK29*P(2,14) - HK30*P(6,14) - HK31*P(14,23) + HK31*P(5,14) + HK32*P(1,14) + HK33*P(3,14));
|
||||
Kfusion(15) = HK53*(HK19*P(15,22) - HK19*P(4,15) - HK28*P(0,15) - HK29*P(2,15) - HK30*P(6,15) - HK31*P(15,23) + HK31*P(5,15) + HK32*P(1,15) + HK33*P(3,15));
|
||||
Kfusion(16) = HK53*(HK19*P(16,22) - HK19*P(4,16) - HK28*P(0,16) - HK29*P(2,16) - HK30*P(6,16) - HK31*P(16,23) + HK31*P(5,16) + HK32*P(1,16) + HK33*P(3,16));
|
||||
Kfusion(17) = HK53*(HK19*P(17,22) - HK19*P(4,17) - HK28*P(0,17) - HK29*P(2,17) - HK30*P(6,17) - HK31*P(17,23) + HK31*P(5,17) + HK32*P(1,17) + HK33*P(3,17));
|
||||
Kfusion(18) = HK53*(HK19*P(18,22) - HK19*P(4,18) - HK28*P(0,18) - HK29*P(2,18) - HK30*P(6,18) - HK31*P(18,23) + HK31*P(5,18) + HK32*P(1,18) + HK33*P(3,18));
|
||||
Kfusion(19) = HK53*(HK19*P(19,22) - HK19*P(4,19) - HK28*P(0,19) - HK29*P(2,19) - HK30*P(6,19) - HK31*P(19,23) + HK31*P(5,19) + HK32*P(1,19) + HK33*P(3,19));
|
||||
Kfusion(20) = HK53*(HK19*P(20,22) - HK19*P(4,20) - HK28*P(0,20) - HK29*P(2,20) - HK30*P(6,20) - HK31*P(20,23) + HK31*P(5,20) + HK32*P(1,20) + HK33*P(3,20));
|
||||
Kfusion(21) = HK53*(HK19*P(21,22) - HK19*P(4,21) - HK28*P(0,21) - HK29*P(2,21) - HK30*P(6,21) - HK31*P(21,23) + HK31*P(5,21) + HK32*P(1,21) + HK33*P(3,21));
|
||||
Kfusion(22) = HK53*(-HK28*P(0,22) - HK29*P(2,22) - HK30*P(6,22) - HK47 + HK50);
|
||||
Kfusion(23) = HK53*(-HK19*P(4,23) - HK28*P(0,23) - HK29*P(2,23) - HK30*P(6,23) + HK41);
|
||||
|
||||
|
||||
// Observation Jacobians - axis 1
|
||||
Hfusion.at<0>() = -HK54*HK55;
|
||||
Hfusion.at<1>() = -HK55*HK56;
|
||||
Hfusion.at<2>() = -HK55*HK57;
|
||||
Hfusion.at<3>() = HK55*HK58;
|
||||
Hfusion.at<4>() = HK60;
|
||||
Hfusion.at<5>() = HK62;
|
||||
Hfusion.at<6>() = -HK55*HK63;
|
||||
Hfusion.at<7>() = 0;
|
||||
Hfusion.at<8>() = 0;
|
||||
Hfusion.at<9>() = 0;
|
||||
Hfusion.at<10>() = 0;
|
||||
Hfusion.at<11>() = 0;
|
||||
Hfusion.at<12>() = 0;
|
||||
Hfusion.at<13>() = 0;
|
||||
Hfusion.at<14>() = 0;
|
||||
Hfusion.at<15>() = 0;
|
||||
Hfusion.at<16>() = 0;
|
||||
Hfusion.at<17>() = 0;
|
||||
Hfusion.at<18>() = 0;
|
||||
Hfusion.at<19>() = 0;
|
||||
Hfusion.at<20>() = 0;
|
||||
Hfusion.at<21>() = 0;
|
||||
Hfusion.at<22>() = -HK60;
|
||||
Hfusion.at<23>() = -HK62;
|
||||
|
||||
|
||||
// Kalman gains - axis 1
|
||||
Kfusion(0) = -HK70*HK84;
|
||||
Kfusion(1) = -HK82*HK84;
|
||||
Kfusion(2) = -HK77*HK84;
|
||||
Kfusion(3) = -HK83*HK84;
|
||||
Kfusion(4) = -HK76*HK84;
|
||||
Kfusion(5) = -HK81*HK84;
|
||||
Kfusion(6) = -HK72*HK84;
|
||||
Kfusion(7) = -HK84*(-HK61*P(5,7) + HK61*P(7,23) + HK64*P(6,7) - HK65*P(4,7) + HK65*P(7,22) + HK66*P(2,7) + HK67*P(0,7) + HK68*P(1,7) - HK69*P(3,7));
|
||||
Kfusion(8) = -HK84*(-HK61*P(5,8) + HK61*P(8,23) + HK64*P(6,8) - HK65*P(4,8) + HK65*P(8,22) + HK66*P(2,8) + HK67*P(0,8) + HK68*P(1,8) - HK69*P(3,8));
|
||||
Kfusion(9) = -HK84*(-HK61*P(5,9) + HK61*P(9,23) + HK64*P(6,9) - HK65*P(4,9) + HK65*P(9,22) + HK66*P(2,9) + HK67*P(0,9) + HK68*P(1,9) - HK69*P(3,9));
|
||||
Kfusion(10) = -HK84*(HK61*P(10,23) - HK61*P(5,10) + HK64*P(6,10) + HK65*P(10,22) - HK65*P(4,10) + HK66*P(2,10) + HK67*P(0,10) + HK68*P(1,10) - HK69*P(3,10));
|
||||
Kfusion(11) = -HK84*(HK61*P(11,23) - HK61*P(5,11) + HK64*P(6,11) + HK65*P(11,22) - HK65*P(4,11) + HK66*P(2,11) + HK67*P(0,11) + HK68*P(1,11) - HK69*P(3,11));
|
||||
Kfusion(12) = -HK84*(HK61*P(12,23) - HK61*P(5,12) + HK64*P(6,12) + HK65*P(12,22) - HK65*P(4,12) + HK66*P(2,12) + HK67*P(0,12) + HK68*P(1,12) - HK69*P(3,12));
|
||||
Kfusion(13) = -HK84*(HK61*P(13,23) - HK61*P(5,13) + HK64*P(6,13) + HK65*P(13,22) - HK65*P(4,13) + HK66*P(2,13) + HK67*P(0,13) + HK68*P(1,13) - HK69*P(3,13));
|
||||
Kfusion(14) = -HK84*(HK61*P(14,23) - HK61*P(5,14) + HK64*P(6,14) + HK65*P(14,22) - HK65*P(4,14) + HK66*P(2,14) + HK67*P(0,14) + HK68*P(1,14) - HK69*P(3,14));
|
||||
Kfusion(15) = -HK84*(HK61*P(15,23) - HK61*P(5,15) + HK64*P(6,15) + HK65*P(15,22) - HK65*P(4,15) + HK66*P(2,15) + HK67*P(0,15) + HK68*P(1,15) - HK69*P(3,15));
|
||||
Kfusion(16) = -HK84*(HK61*P(16,23) - HK61*P(5,16) + HK64*P(6,16) + HK65*P(16,22) - HK65*P(4,16) + HK66*P(2,16) + HK67*P(0,16) + HK68*P(1,16) - HK69*P(3,16));
|
||||
Kfusion(17) = -HK84*(HK61*P(17,23) - HK61*P(5,17) + HK64*P(6,17) + HK65*P(17,22) - HK65*P(4,17) + HK66*P(2,17) + HK67*P(0,17) + HK68*P(1,17) - HK69*P(3,17));
|
||||
Kfusion(18) = -HK84*(HK61*P(18,23) - HK61*P(5,18) + HK64*P(6,18) + HK65*P(18,22) - HK65*P(4,18) + HK66*P(2,18) + HK67*P(0,18) + HK68*P(1,18) - HK69*P(3,18));
|
||||
Kfusion(19) = -HK84*(HK61*P(19,23) - HK61*P(5,19) + HK64*P(6,19) + HK65*P(19,22) - HK65*P(4,19) + HK66*P(2,19) + HK67*P(0,19) + HK68*P(1,19) - HK69*P(3,19));
|
||||
Kfusion(20) = -HK84*(HK61*P(20,23) - HK61*P(5,20) + HK64*P(6,20) + HK65*P(20,22) - HK65*P(4,20) + HK66*P(2,20) + HK67*P(0,20) + HK68*P(1,20) - HK69*P(3,20));
|
||||
Kfusion(21) = -HK84*(HK61*P(21,23) - HK61*P(5,21) + HK64*P(6,21) + HK65*P(21,22) - HK65*P(4,21) + HK66*P(2,21) + HK67*P(0,21) + HK68*P(1,21) - HK69*P(3,21));
|
||||
Kfusion(22) = -HK74*HK84;
|
||||
Kfusion(23) = -HK79*HK84;
|
||||
|
||||
|
||||
// Observation Jacobians - axis 2
|
||||
|
||||
|
||||
// Kalman gains - axis 2
|
||||
|
||||
|
||||
+333
File diff suppressed because one or more lines are too long
@@ -0,0 +1,113 @@
|
||||
// Sub Expressions
|
||||
const float HK0 = q1*vd;
|
||||
const float HK1 = vn - vwn;
|
||||
const float HK2 = HK1*q3;
|
||||
const float HK3 = q2*vd;
|
||||
const float HK4 = ve - vwe;
|
||||
const float HK5 = HK4*q3;
|
||||
const float HK6 = q0*q2 - q1*q3;
|
||||
const float HK7 = 2*vd;
|
||||
const float HK8 = HK6*HK7;
|
||||
const float HK9 = q0*q3;
|
||||
const float HK10 = q1*q2;
|
||||
const float HK11 = 2*HK10 + 2*HK9;
|
||||
const float HK12 = HK11*HK4;
|
||||
const float HK13 = 2*powf(q3, 2) - 1;
|
||||
const float HK14 = HK13 + 2*powf(q2, 2);
|
||||
const float HK15 = HK1*HK14;
|
||||
const float HK16 = 1.0F/(-HK12 + HK15 + HK8);
|
||||
const float HK17 = q0*q1 + q2*q3;
|
||||
const float HK18 = HK17*HK7;
|
||||
const float HK19 = 2*HK1*(-HK10 + HK9);
|
||||
const float HK20 = HK13 + 2*powf(q1, 2);
|
||||
const float HK21 = HK20*HK4;
|
||||
const float HK22 = HK16*(-HK18 + HK19 + HK21);
|
||||
const float HK23 = 2*HK16;
|
||||
const float HK24 = HK23*(HK0 - HK2 + HK22*(HK3 - HK5));
|
||||
const float HK25 = q0*vd;
|
||||
const float HK26 = HK1*q2;
|
||||
const float HK27 = HK4*q1;
|
||||
const float HK28 = 2*HK27;
|
||||
const float HK29 = q3*vd;
|
||||
const float HK30 = 1.0F/(HK12 - HK15 - HK8);
|
||||
const float HK31 = HK30*(HK18 - HK19 - HK21);
|
||||
const float HK32 = HK31*(HK29 + HK4*q2);
|
||||
const float HK33 = 2*HK30;
|
||||
const float HK34 = HK23*(HK1*q1 + HK22*(HK25 + 2*HK26 - HK27) + HK29);
|
||||
const float HK35 = HK33*(HK1*q0 - HK3 + HK31*(HK0 - 2*HK2 + HK4*q0) + 2*HK5);
|
||||
const float HK36 = HK14*HK31;
|
||||
const float HK37 = 2*HK9;
|
||||
const float HK38 = 2*HK10;
|
||||
const float HK39 = -HK37 + HK38;
|
||||
const float HK40 = HK30*(HK11*HK31 + HK20);
|
||||
const float HK41 = HK23*(HK17 + HK22*HK6);
|
||||
const float HK42 = HK16*(HK14*HK22 + HK39);
|
||||
const float HK43 = HK16*(HK11*HK22 + HK20);
|
||||
const float HK44 = HK30*(-HK36 + HK37 - HK38);
|
||||
const float HK45 = HK33*(-HK25 - HK26 + HK28 + HK32);
|
||||
const float HK46 = -HK24*P(0,0) - HK34*P(0,2) - HK35*P(0,3) - HK40*P(0,5) - HK41*P(0,6) + HK42*P(0,22) - HK43*P(0,23) - HK44*P(0,4) - HK45*P(0,1);
|
||||
const float HK47 = -HK24*P(0,6) - HK34*P(2,6) - HK35*P(3,6) - HK40*P(5,6) - HK41*P(6,6) + HK42*P(6,22) - HK43*P(6,23) - HK44*P(4,6) - HK45*P(1,6);
|
||||
const float HK48 = -HK24*P(0,22) - HK34*P(2,22) - HK35*P(3,22) - HK40*P(5,22) - HK41*P(6,22) + HK42*P(22,22) - HK43*P(22,23) - HK44*P(4,22) - HK45*P(1,22);
|
||||
const float HK49 = -HK24*P(0,23) - HK34*P(2,23) - HK35*P(3,23) - HK40*P(5,23) - HK41*P(6,23) + HK42*P(22,23) - HK43*P(23,23) - HK44*P(4,23) - HK45*P(1,23);
|
||||
const float HK50 = -HK24*P(0,5) - HK34*P(2,5) - HK35*P(3,5) - HK40*P(5,5) - HK41*P(5,6) + HK42*P(5,22) - HK43*P(5,23) - HK44*P(4,5) - HK45*P(1,5);
|
||||
const float HK51 = -HK24*P(0,4) - HK34*P(2,4) - HK35*P(3,4) - HK40*P(4,5) - HK41*P(4,6) + HK42*P(4,22) - HK43*P(4,23) - HK44*P(4,4) - HK45*P(1,4);
|
||||
const float HK52 = -HK24*P(0,2) - HK34*P(2,2) - HK35*P(2,3) - HK40*P(2,5) - HK41*P(2,6) + HK42*P(2,22) - HK43*P(2,23) - HK44*P(2,4) - HK45*P(1,2);
|
||||
const float HK53 = -HK24*P(0,1) - HK34*P(1,2) - HK35*P(1,3) - HK40*P(1,5) - HK41*P(1,6) + HK42*P(1,22) - HK43*P(1,23) - HK44*P(1,4) - HK45*P(1,1);
|
||||
const float HK54 = -HK24*P(0,3) - HK34*P(2,3) - HK35*P(3,3) - HK40*P(3,5) - HK41*P(3,6) + HK42*P(3,22) - HK43*P(3,23) - HK44*P(3,4) - HK45*P(1,3);
|
||||
const float HK55 = 1.0F/(-HK24*HK46 - HK34*HK52 - HK35*HK54 - HK40*HK50 - HK41*HK47 + HK42*HK48 - HK43*HK49 - HK44*HK51 - HK45*HK53 + R_BETA);
|
||||
|
||||
|
||||
// Observation Jacobians
|
||||
Hfusion.at<0>() = -HK24;
|
||||
Hfusion.at<1>() = HK33*(HK25 + HK26 - HK28 - HK32);
|
||||
Hfusion.at<2>() = -HK34;
|
||||
Hfusion.at<3>() = -HK35;
|
||||
Hfusion.at<4>() = HK30*(HK36 + HK39);
|
||||
Hfusion.at<5>() = -HK40;
|
||||
Hfusion.at<6>() = -HK41;
|
||||
Hfusion.at<7>() = 0;
|
||||
Hfusion.at<8>() = 0;
|
||||
Hfusion.at<9>() = 0;
|
||||
Hfusion.at<10>() = 0;
|
||||
Hfusion.at<11>() = 0;
|
||||
Hfusion.at<12>() = 0;
|
||||
Hfusion.at<13>() = 0;
|
||||
Hfusion.at<14>() = 0;
|
||||
Hfusion.at<15>() = 0;
|
||||
Hfusion.at<16>() = 0;
|
||||
Hfusion.at<17>() = 0;
|
||||
Hfusion.at<18>() = 0;
|
||||
Hfusion.at<19>() = 0;
|
||||
Hfusion.at<20>() = 0;
|
||||
Hfusion.at<21>() = 0;
|
||||
Hfusion.at<22>() = HK42;
|
||||
Hfusion.at<23>() = -HK43;
|
||||
|
||||
|
||||
// Kalman gains
|
||||
Kfusion(0) = HK46*HK55;
|
||||
Kfusion(1) = HK53*HK55;
|
||||
Kfusion(2) = HK52*HK55;
|
||||
Kfusion(3) = HK54*HK55;
|
||||
Kfusion(4) = HK51*HK55;
|
||||
Kfusion(5) = HK50*HK55;
|
||||
Kfusion(6) = HK47*HK55;
|
||||
Kfusion(7) = HK55*(-HK24*P(0,7) - HK34*P(2,7) - HK35*P(3,7) - HK40*P(5,7) - HK41*P(6,7) + HK42*P(7,22) - HK43*P(7,23) - HK44*P(4,7) - HK45*P(1,7));
|
||||
Kfusion(8) = HK55*(-HK24*P(0,8) - HK34*P(2,8) - HK35*P(3,8) - HK40*P(5,8) - HK41*P(6,8) + HK42*P(8,22) - HK43*P(8,23) - HK44*P(4,8) - HK45*P(1,8));
|
||||
Kfusion(9) = HK55*(-HK24*P(0,9) - HK34*P(2,9) - HK35*P(3,9) - HK40*P(5,9) - HK41*P(6,9) + HK42*P(9,22) - HK43*P(9,23) - HK44*P(4,9) - HK45*P(1,9));
|
||||
Kfusion(10) = HK55*(-HK24*P(0,10) - HK34*P(2,10) - HK35*P(3,10) - HK40*P(5,10) - HK41*P(6,10) + HK42*P(10,22) - HK43*P(10,23) - HK44*P(4,10) - HK45*P(1,10));
|
||||
Kfusion(11) = HK55*(-HK24*P(0,11) - HK34*P(2,11) - HK35*P(3,11) - HK40*P(5,11) - HK41*P(6,11) + HK42*P(11,22) - HK43*P(11,23) - HK44*P(4,11) - HK45*P(1,11));
|
||||
Kfusion(12) = HK55*(-HK24*P(0,12) - HK34*P(2,12) - HK35*P(3,12) - HK40*P(5,12) - HK41*P(6,12) + HK42*P(12,22) - HK43*P(12,23) - HK44*P(4,12) - HK45*P(1,12));
|
||||
Kfusion(13) = HK55*(-HK24*P(0,13) - HK34*P(2,13) - HK35*P(3,13) - HK40*P(5,13) - HK41*P(6,13) + HK42*P(13,22) - HK43*P(13,23) - HK44*P(4,13) - HK45*P(1,13));
|
||||
Kfusion(14) = HK55*(-HK24*P(0,14) - HK34*P(2,14) - HK35*P(3,14) - HK40*P(5,14) - HK41*P(6,14) + HK42*P(14,22) - HK43*P(14,23) - HK44*P(4,14) - HK45*P(1,14));
|
||||
Kfusion(15) = HK55*(-HK24*P(0,15) - HK34*P(2,15) - HK35*P(3,15) - HK40*P(5,15) - HK41*P(6,15) + HK42*P(15,22) - HK43*P(15,23) - HK44*P(4,15) - HK45*P(1,15));
|
||||
Kfusion(16) = HK55*(-HK24*P(0,16) - HK34*P(2,16) - HK35*P(3,16) - HK40*P(5,16) - HK41*P(6,16) + HK42*P(16,22) - HK43*P(16,23) - HK44*P(4,16) - HK45*P(1,16));
|
||||
Kfusion(17) = HK55*(-HK24*P(0,17) - HK34*P(2,17) - HK35*P(3,17) - HK40*P(5,17) - HK41*P(6,17) + HK42*P(17,22) - HK43*P(17,23) - HK44*P(4,17) - HK45*P(1,17));
|
||||
Kfusion(18) = HK55*(-HK24*P(0,18) - HK34*P(2,18) - HK35*P(3,18) - HK40*P(5,18) - HK41*P(6,18) + HK42*P(18,22) - HK43*P(18,23) - HK44*P(4,18) - HK45*P(1,18));
|
||||
Kfusion(19) = HK55*(-HK24*P(0,19) - HK34*P(2,19) - HK35*P(3,19) - HK40*P(5,19) - HK41*P(6,19) + HK42*P(19,22) - HK43*P(19,23) - HK44*P(4,19) - HK45*P(1,19));
|
||||
Kfusion(20) = HK55*(-HK24*P(0,20) - HK34*P(2,20) - HK35*P(3,20) - HK40*P(5,20) - HK41*P(6,20) + HK42*P(20,22) - HK43*P(20,23) - HK44*P(4,20) - HK45*P(1,20));
|
||||
Kfusion(21) = HK55*(-HK24*P(0,21) - HK34*P(2,21) - HK35*P(3,21) - HK40*P(5,21) - HK41*P(6,21) + HK42*P(21,22) - HK43*P(21,23) - HK44*P(4,21) - HK45*P(1,21));
|
||||
Kfusion(22) = HK48*HK55;
|
||||
Kfusion(23) = HK49*HK55;
|
||||
|
||||
|
||||
@@ -0,0 +1,528 @@
|
||||
// Equations for covariance matrix prediction, without process noise!
|
||||
const float PS0 = powf(q1, 2);
|
||||
const float PS1 = 0.25F*daxVar;
|
||||
const float PS2 = powf(q2, 2);
|
||||
const float PS3 = 0.25F*dayVar;
|
||||
const float PS4 = powf(q3, 2);
|
||||
const float PS5 = 0.25F*dazVar;
|
||||
const float PS6 = 0.5F*q1;
|
||||
const float PS7 = 0.5F*q2;
|
||||
const float PS8 = P(10,11)*PS7;
|
||||
const float PS9 = 0.5F*q3;
|
||||
const float PS10 = P(10,12)*PS9;
|
||||
const float PS11 = 0.5F*dax - 0.5F*dax_b;
|
||||
const float PS12 = 0.5F*day - 0.5F*day_b;
|
||||
const float PS13 = 0.5F*daz - 0.5F*daz_b;
|
||||
const float PS14 = P(0,10) - P(1,10)*PS11 + P(10,10)*PS6 - P(2,10)*PS12 - P(3,10)*PS13 + PS10 + PS8;
|
||||
const float PS15 = P(10,11)*PS6;
|
||||
const float PS16 = P(11,12)*PS9;
|
||||
const float PS17 = P(0,11) - P(1,11)*PS11 + P(11,11)*PS7 - P(2,11)*PS12 - P(3,11)*PS13 + PS15 + PS16;
|
||||
const float PS18 = P(10,12)*PS6;
|
||||
const float PS19 = P(11,12)*PS7;
|
||||
const float PS20 = P(0,12) - P(1,12)*PS11 + P(12,12)*PS9 - P(2,12)*PS12 - P(3,12)*PS13 + PS18 + PS19;
|
||||
const float PS21 = P(1,2)*PS12;
|
||||
const float PS22 = -P(1,3)*PS13;
|
||||
const float PS23 = P(0,1) - P(1,1)*PS11 + P(1,10)*PS6 + P(1,11)*PS7 + P(1,12)*PS9 - PS21 + PS22;
|
||||
const float PS24 = -P(1,2)*PS11;
|
||||
const float PS25 = P(2,3)*PS13;
|
||||
const float PS26 = P(0,2) + P(2,10)*PS6 + P(2,11)*PS7 + P(2,12)*PS9 - P(2,2)*PS12 + PS24 - PS25;
|
||||
const float PS27 = P(1,3)*PS11;
|
||||
const float PS28 = -P(2,3)*PS12;
|
||||
const float PS29 = P(0,3) + P(3,10)*PS6 + P(3,11)*PS7 + P(3,12)*PS9 - P(3,3)*PS13 - PS27 + PS28;
|
||||
const float PS30 = P(0,1)*PS11;
|
||||
const float PS31 = P(0,2)*PS12;
|
||||
const float PS32 = P(0,3)*PS13;
|
||||
const float PS33 = P(0,0) + P(0,10)*PS6 + P(0,11)*PS7 + P(0,12)*PS9 - PS30 - PS31 - PS32;
|
||||
const float PS34 = 0.5F*q0;
|
||||
const float PS35 = q2*q3;
|
||||
const float PS36 = q0*q1;
|
||||
const float PS37 = q1*q3;
|
||||
const float PS38 = q0*q2;
|
||||
const float PS39 = q1*q2;
|
||||
const float PS40 = q0*q3;
|
||||
const float PS41 = 2*PS2;
|
||||
const float PS42 = 2*PS4 - 1;
|
||||
const float PS43 = PS41 + PS42;
|
||||
const float PS44 = P(0,13) - P(1,13)*PS11 + P(10,13)*PS6 + P(11,13)*PS7 + P(12,13)*PS9 - P(2,13)*PS12 - P(3,13)*PS13;
|
||||
const float PS45 = PS37 + PS38;
|
||||
const float PS46 = P(0,15) - P(1,15)*PS11 + P(10,15)*PS6 + P(11,15)*PS7 + P(12,15)*PS9 - P(2,15)*PS12 - P(3,15)*PS13;
|
||||
const float PS47 = 2*PS46;
|
||||
const float PS48 = dvy - dvy_b;
|
||||
const float PS49 = PS48*q0;
|
||||
const float PS50 = dvz - dvz_b;
|
||||
const float PS51 = PS50*q1;
|
||||
const float PS52 = dvx - dvx_b;
|
||||
const float PS53 = PS52*q3;
|
||||
const float PS54 = PS49 - PS51 + 2*PS53;
|
||||
const float PS55 = 2*PS29;
|
||||
const float PS56 = -PS39 + PS40;
|
||||
const float PS57 = P(0,14) - P(1,14)*PS11 + P(10,14)*PS6 + P(11,14)*PS7 + P(12,14)*PS9 - P(2,14)*PS12 - P(3,14)*PS13;
|
||||
const float PS58 = 2*PS57;
|
||||
const float PS59 = PS48*q2;
|
||||
const float PS60 = PS50*q3;
|
||||
const float PS61 = PS59 + PS60;
|
||||
const float PS62 = 2*PS23;
|
||||
const float PS63 = PS50*q2;
|
||||
const float PS64 = PS48*q3;
|
||||
const float PS65 = -PS64;
|
||||
const float PS66 = PS63 + PS65;
|
||||
const float PS67 = 2*PS33;
|
||||
const float PS68 = PS50*q0;
|
||||
const float PS69 = PS48*q1;
|
||||
const float PS70 = PS52*q2;
|
||||
const float PS71 = PS68 + PS69 - 2*PS70;
|
||||
const float PS72 = 2*PS26;
|
||||
const float PS73 = P(0,4) - P(1,4)*PS11 - P(2,4)*PS12 - P(3,4)*PS13 + P(4,10)*PS6 + P(4,11)*PS7 + P(4,12)*PS9;
|
||||
const float PS74 = 2*PS0;
|
||||
const float PS75 = PS42 + PS74;
|
||||
const float PS76 = PS39 + PS40;
|
||||
const float PS77 = 2*PS44;
|
||||
const float PS78 = PS51 - PS53;
|
||||
const float PS79 = -PS70;
|
||||
const float PS80 = PS68 + 2*PS69 + PS79;
|
||||
const float PS81 = -PS35 + PS36;
|
||||
const float PS82 = PS52*q1;
|
||||
const float PS83 = PS60 + PS82;
|
||||
const float PS84 = PS52*q0;
|
||||
const float PS85 = PS63 - 2*PS64 + PS84;
|
||||
const float PS86 = P(0,5) - P(1,5)*PS11 - P(2,5)*PS12 - P(3,5)*PS13 + P(5,10)*PS6 + P(5,11)*PS7 + P(5,12)*PS9;
|
||||
const float PS87 = PS41 + PS74 - 1;
|
||||
const float PS88 = PS35 + PS36;
|
||||
const float PS89 = 2*PS63 + PS65 + PS84;
|
||||
const float PS90 = -PS37 + PS38;
|
||||
const float PS91 = PS59 + PS82;
|
||||
const float PS92 = PS69 + PS79;
|
||||
const float PS93 = PS49 - 2*PS51 + PS53;
|
||||
const float PS94 = P(0,6) - P(1,6)*PS11 - P(2,6)*PS12 - P(3,6)*PS13 + P(6,10)*PS6 + P(6,11)*PS7 + P(6,12)*PS9;
|
||||
const float PS95 = powf(q0, 2);
|
||||
const float PS96 = -P(10,11)*PS34;
|
||||
const float PS97 = P(0,11)*PS11 + P(1,11) + P(11,11)*PS9 + P(2,11)*PS13 - P(3,11)*PS12 - PS19 + PS96;
|
||||
const float PS98 = P(0,2)*PS13;
|
||||
const float PS99 = P(0,3)*PS12;
|
||||
const float PS100 = P(0,0)*PS11 + P(0,1) - P(0,10)*PS34 + P(0,11)*PS9 - P(0,12)*PS7 + PS98 - PS99;
|
||||
const float PS101 = P(0,2)*PS11;
|
||||
const float PS102 = P(1,2) - P(2,10)*PS34 + P(2,11)*PS9 - P(2,12)*PS7 + P(2,2)*PS13 + PS101 + PS28;
|
||||
const float PS103 = P(10,11)*PS9;
|
||||
const float PS104 = P(10,12)*PS7;
|
||||
const float PS105 = P(0,10)*PS11 + P(1,10) - P(10,10)*PS34 + P(2,10)*PS13 - P(3,10)*PS12 + PS103 - PS104;
|
||||
const float PS106 = -P(10,12)*PS34;
|
||||
const float PS107 = P(0,12)*PS11 + P(1,12) - P(12,12)*PS7 + P(2,12)*PS13 - P(3,12)*PS12 + PS106 + PS16;
|
||||
const float PS108 = P(0,3)*PS11;
|
||||
const float PS109 = P(1,3) - P(3,10)*PS34 + P(3,11)*PS9 - P(3,12)*PS7 - P(3,3)*PS12 + PS108 + PS25;
|
||||
const float PS110 = P(1,2)*PS13;
|
||||
const float PS111 = P(1,3)*PS12;
|
||||
const float PS112 = P(1,1) - P(1,10)*PS34 + P(1,11)*PS9 - P(1,12)*PS7 + PS110 - PS111 + PS30;
|
||||
const float PS113 = P(0,13)*PS11 + P(1,13) - P(10,13)*PS34 + P(11,13)*PS9 - P(12,13)*PS7 + P(2,13)*PS13 - P(3,13)*PS12;
|
||||
const float PS114 = P(0,15)*PS11 + P(1,15) - P(10,15)*PS34 + P(11,15)*PS9 - P(12,15)*PS7 + P(2,15)*PS13 - P(3,15)*PS12;
|
||||
const float PS115 = 2*PS114;
|
||||
const float PS116 = 2*PS109;
|
||||
const float PS117 = P(0,14)*PS11 + P(1,14) - P(10,14)*PS34 + P(11,14)*PS9 - P(12,14)*PS7 + P(2,14)*PS13 - P(3,14)*PS12;
|
||||
const float PS118 = 2*PS117;
|
||||
const float PS119 = 2*PS112;
|
||||
const float PS120 = 2*PS100;
|
||||
const float PS121 = 2*PS102;
|
||||
const float PS122 = P(0,4)*PS11 + P(1,4) + P(2,4)*PS13 - P(3,4)*PS12 - P(4,10)*PS34 + P(4,11)*PS9 - P(4,12)*PS7;
|
||||
const float PS123 = 2*PS113;
|
||||
const float PS124 = P(0,5)*PS11 + P(1,5) + P(2,5)*PS13 - P(3,5)*PS12 - P(5,10)*PS34 + P(5,11)*PS9 - P(5,12)*PS7;
|
||||
const float PS125 = P(0,6)*PS11 + P(1,6) + P(2,6)*PS13 - P(3,6)*PS12 - P(6,10)*PS34 + P(6,11)*PS9 - P(6,12)*PS7;
|
||||
const float PS126 = -P(11,12)*PS34;
|
||||
const float PS127 = P(0,12)*PS12 - P(1,12)*PS13 + P(12,12)*PS6 + P(2,12) + P(3,12)*PS11 - PS10 + PS126;
|
||||
const float PS128 = P(2,3) - P(3,10)*PS9 - P(3,11)*PS34 + P(3,12)*PS6 + P(3,3)*PS11 + PS22 + PS99;
|
||||
const float PS129 = P(0,1)*PS13;
|
||||
const float PS130 = P(0,0)*PS12 - P(0,10)*PS9 - P(0,11)*PS34 + P(0,12)*PS6 + P(0,2) + PS108 - PS129;
|
||||
const float PS131 = P(11,12)*PS6;
|
||||
const float PS132 = P(0,11)*PS12 - P(1,11)*PS13 - P(11,11)*PS34 + P(2,11) + P(3,11)*PS11 - PS103 + PS131;
|
||||
const float PS133 = P(0,10)*PS12 - P(1,10)*PS13 - P(10,10)*PS9 + P(2,10) + P(3,10)*PS11 + PS18 + PS96;
|
||||
const float PS134 = P(0,1)*PS12;
|
||||
const float PS135 = -P(1,1)*PS13 - P(1,10)*PS9 - P(1,11)*PS34 + P(1,12)*PS6 + P(1,2) + PS134 + PS27;
|
||||
const float PS136 = P(2,3)*PS11;
|
||||
const float PS137 = -P(2,10)*PS9 - P(2,11)*PS34 + P(2,12)*PS6 + P(2,2) - PS110 + PS136 + PS31;
|
||||
const float PS138 = P(0,13)*PS12 - P(1,13)*PS13 - P(10,13)*PS9 - P(11,13)*PS34 + P(12,13)*PS6 + P(2,13) + P(3,13)*PS11;
|
||||
const float PS139 = P(0,15)*PS12 - P(1,15)*PS13 - P(10,15)*PS9 - P(11,15)*PS34 + P(12,15)*PS6 + P(2,15) + P(3,15)*PS11;
|
||||
const float PS140 = 2*PS139;
|
||||
const float PS141 = 2*PS128;
|
||||
const float PS142 = P(0,14)*PS12 - P(1,14)*PS13 - P(10,14)*PS9 - P(11,14)*PS34 + P(12,14)*PS6 + P(2,14) + P(3,14)*PS11;
|
||||
const float PS143 = 2*PS142;
|
||||
const float PS144 = 2*PS135;
|
||||
const float PS145 = 2*PS130;
|
||||
const float PS146 = 2*PS137;
|
||||
const float PS147 = P(0,4)*PS12 - P(1,4)*PS13 + P(2,4) + P(3,4)*PS11 - P(4,10)*PS9 - P(4,11)*PS34 + P(4,12)*PS6;
|
||||
const float PS148 = 2*PS138;
|
||||
const float PS149 = P(0,5)*PS12 - P(1,5)*PS13 + P(2,5) + P(3,5)*PS11 - P(5,10)*PS9 - P(5,11)*PS34 + P(5,12)*PS6;
|
||||
const float PS150 = P(0,6)*PS12 - P(1,6)*PS13 + P(2,6) + P(3,6)*PS11 - P(6,10)*PS9 - P(6,11)*PS34 + P(6,12)*PS6;
|
||||
const float PS151 = P(0,10)*PS13 + P(1,10)*PS12 + P(10,10)*PS7 - P(2,10)*PS11 + P(3,10) + PS106 - PS15;
|
||||
const float PS152 = P(1,1)*PS12 + P(1,10)*PS7 - P(1,11)*PS6 - P(1,12)*PS34 + P(1,3) + PS129 + PS24;
|
||||
const float PS153 = P(0,0)*PS13 + P(0,10)*PS7 - P(0,11)*PS6 - P(0,12)*PS34 + P(0,3) - PS101 + PS134;
|
||||
const float PS154 = P(0,12)*PS13 + P(1,12)*PS12 - P(12,12)*PS34 - P(2,12)*PS11 + P(3,12) + PS104 - PS131;
|
||||
const float PS155 = P(0,11)*PS13 + P(1,11)*PS12 - P(11,11)*PS6 - P(2,11)*PS11 + P(3,11) + PS126 + PS8;
|
||||
const float PS156 = P(2,10)*PS7 - P(2,11)*PS6 - P(2,12)*PS34 - P(2,2)*PS11 + P(2,3) + PS21 + PS98;
|
||||
const float PS157 = P(3,10)*PS7 - P(3,11)*PS6 - P(3,12)*PS34 + P(3,3) + PS111 - PS136 + PS32;
|
||||
const float PS158 = P(0,13)*PS13 + P(1,13)*PS12 + P(10,13)*PS7 - P(11,13)*PS6 - P(12,13)*PS34 - P(2,13)*PS11 + P(3,13);
|
||||
const float PS159 = P(0,15)*PS13 + P(1,15)*PS12 + P(10,15)*PS7 - P(11,15)*PS6 - P(12,15)*PS34 - P(2,15)*PS11 + P(3,15);
|
||||
const float PS160 = 2*PS159;
|
||||
const float PS161 = 2*PS157;
|
||||
const float PS162 = P(0,14)*PS13 + P(1,14)*PS12 + P(10,14)*PS7 - P(11,14)*PS6 - P(12,14)*PS34 - P(2,14)*PS11 + P(3,14);
|
||||
const float PS163 = 2*PS162;
|
||||
const float PS164 = 2*PS152;
|
||||
const float PS165 = 2*PS153;
|
||||
const float PS166 = 2*PS156;
|
||||
const float PS167 = P(0,4)*PS13 + P(1,4)*PS12 - P(2,4)*PS11 + P(3,4) + P(4,10)*PS7 - P(4,11)*PS6 - P(4,12)*PS34;
|
||||
const float PS168 = 2*PS158;
|
||||
const float PS169 = P(0,5)*PS13 + P(1,5)*PS12 - P(2,5)*PS11 + P(3,5) + P(5,10)*PS7 - P(5,11)*PS6 - P(5,12)*PS34;
|
||||
const float PS170 = P(0,6)*PS13 + P(1,6)*PS12 - P(2,6)*PS11 + P(3,6) + P(6,10)*PS7 - P(6,11)*PS6 - P(6,12)*PS34;
|
||||
const float PS171 = 2*PS45;
|
||||
const float PS172 = 2*PS56;
|
||||
const float PS173 = 2*PS61;
|
||||
const float PS174 = 2*PS66;
|
||||
const float PS175 = 2*PS71;
|
||||
const float PS176 = 2*PS54;
|
||||
const float PS177 = P(0,13)*PS174 + P(1,13)*PS173 + P(13,13)*PS43 + P(13,14)*PS172 - P(13,15)*PS171 + P(2,13)*PS175 - P(3,13)*PS176 + P(4,13);
|
||||
const float PS178 = P(0,15)*PS174 + P(1,15)*PS173 + P(13,15)*PS43 + P(14,15)*PS172 - P(15,15)*PS171 + P(2,15)*PS175 - P(3,15)*PS176 + P(4,15);
|
||||
const float PS179 = P(0,3)*PS174 + P(1,3)*PS173 + P(2,3)*PS175 + P(3,13)*PS43 + P(3,14)*PS172 - P(3,15)*PS171 - P(3,3)*PS176 + P(3,4);
|
||||
const float PS180 = P(0,14)*PS174 + P(1,14)*PS173 + P(13,14)*PS43 + P(14,14)*PS172 - P(14,15)*PS171 + P(2,14)*PS175 - P(3,14)*PS176 + P(4,14);
|
||||
const float PS181 = P(0,1)*PS174 + P(1,1)*PS173 + P(1,13)*PS43 + P(1,14)*PS172 - P(1,15)*PS171 + P(1,2)*PS175 - P(1,3)*PS176 + P(1,4);
|
||||
const float PS182 = P(0,0)*PS174 + P(0,1)*PS173 + P(0,13)*PS43 + P(0,14)*PS172 - P(0,15)*PS171 + P(0,2)*PS175 - P(0,3)*PS176 + P(0,4);
|
||||
const float PS183 = P(0,2)*PS174 + P(1,2)*PS173 + P(2,13)*PS43 + P(2,14)*PS172 - P(2,15)*PS171 + P(2,2)*PS175 - P(2,3)*PS176 + P(2,4);
|
||||
const float PS184 = 4*dvyVar;
|
||||
const float PS185 = 4*dvzVar;
|
||||
const float PS186 = P(0,4)*PS174 + P(1,4)*PS173 + P(2,4)*PS175 - P(3,4)*PS176 + P(4,13)*PS43 + P(4,14)*PS172 - P(4,15)*PS171 + P(4,4);
|
||||
const float PS187 = 2*PS177;
|
||||
const float PS188 = 2*PS182;
|
||||
const float PS189 = 2*PS181;
|
||||
const float PS190 = 2*PS81;
|
||||
const float PS191 = 2*PS183;
|
||||
const float PS192 = 2*PS179;
|
||||
const float PS193 = 2*PS76;
|
||||
const float PS194 = PS43*dvxVar;
|
||||
const float PS195 = PS75*dvyVar;
|
||||
const float PS196 = P(0,5)*PS174 + P(1,5)*PS173 + P(2,5)*PS175 - P(3,5)*PS176 + P(4,5) + P(5,13)*PS43 + P(5,14)*PS172 - P(5,15)*PS171;
|
||||
const float PS197 = 2*PS88;
|
||||
const float PS198 = PS87*dvzVar;
|
||||
const float PS199 = 2*PS90;
|
||||
const float PS200 = P(0,6)*PS174 + P(1,6)*PS173 + P(2,6)*PS175 - P(3,6)*PS176 + P(4,6) + P(6,13)*PS43 + P(6,14)*PS172 - P(6,15)*PS171;
|
||||
const float PS201 = 2*PS83;
|
||||
const float PS202 = 2*PS78;
|
||||
const float PS203 = 2*PS85;
|
||||
const float PS204 = 2*PS80;
|
||||
const float PS205 = -P(0,14)*PS202 - P(1,14)*PS204 - P(13,14)*PS193 + P(14,14)*PS75 + P(14,15)*PS190 + P(2,14)*PS201 + P(3,14)*PS203 + P(5,14);
|
||||
const float PS206 = -P(0,13)*PS202 - P(1,13)*PS204 - P(13,13)*PS193 + P(13,14)*PS75 + P(13,15)*PS190 + P(2,13)*PS201 + P(3,13)*PS203 + P(5,13);
|
||||
const float PS207 = -P(0,0)*PS202 - P(0,1)*PS204 - P(0,13)*PS193 + P(0,14)*PS75 + P(0,15)*PS190 + P(0,2)*PS201 + P(0,3)*PS203 + P(0,5);
|
||||
const float PS208 = -P(0,1)*PS202 - P(1,1)*PS204 - P(1,13)*PS193 + P(1,14)*PS75 + P(1,15)*PS190 + P(1,2)*PS201 + P(1,3)*PS203 + P(1,5);
|
||||
const float PS209 = -P(0,15)*PS202 - P(1,15)*PS204 - P(13,15)*PS193 + P(14,15)*PS75 + P(15,15)*PS190 + P(2,15)*PS201 + P(3,15)*PS203 + P(5,15);
|
||||
const float PS210 = -P(0,2)*PS202 - P(1,2)*PS204 - P(2,13)*PS193 + P(2,14)*PS75 + P(2,15)*PS190 + P(2,2)*PS201 + P(2,3)*PS203 + P(2,5);
|
||||
const float PS211 = -P(0,3)*PS202 - P(1,3)*PS204 + P(2,3)*PS201 - P(3,13)*PS193 + P(3,14)*PS75 + P(3,15)*PS190 + P(3,3)*PS203 + P(3,5);
|
||||
const float PS212 = 4*dvxVar;
|
||||
const float PS213 = -P(0,5)*PS202 - P(1,5)*PS204 + P(2,5)*PS201 + P(3,5)*PS203 - P(5,13)*PS193 + P(5,14)*PS75 + P(5,15)*PS190 + P(5,5);
|
||||
const float PS214 = 2*PS89;
|
||||
const float PS215 = 2*PS91;
|
||||
const float PS216 = 2*PS92;
|
||||
const float PS217 = 2*PS93;
|
||||
const float PS218 = -P(0,6)*PS202 - P(1,6)*PS204 + P(2,6)*PS201 + P(3,6)*PS203 + P(5,6) - P(6,13)*PS193 + P(6,14)*PS75 + P(6,15)*PS190;
|
||||
const float PS219 = P(0,15)*PS216 + P(1,15)*PS217 + P(13,15)*PS199 - P(14,15)*PS197 + P(15,15)*PS87 - P(2,15)*PS214 + P(3,15)*PS215 + P(6,15);
|
||||
const float PS220 = P(0,14)*PS216 + P(1,14)*PS217 + P(13,14)*PS199 - P(14,14)*PS197 + P(14,15)*PS87 - P(2,14)*PS214 + P(3,14)*PS215 + P(6,14);
|
||||
const float PS221 = P(0,13)*PS216 + P(1,13)*PS217 + P(13,13)*PS199 - P(13,14)*PS197 + P(13,15)*PS87 - P(2,13)*PS214 + P(3,13)*PS215 + P(6,13);
|
||||
const float PS222 = P(0,6)*PS216 + P(1,6)*PS217 - P(2,6)*PS214 + P(3,6)*PS215 + P(6,13)*PS199 - P(6,14)*PS197 + P(6,15)*PS87 + P(6,6);
|
||||
|
||||
|
||||
nextP(0,0) = PS0*PS1 - PS11*PS23 - PS12*PS26 - PS13*PS29 + PS14*PS6 + PS17*PS7 + PS2*PS3 + PS20*PS9 + PS33 + PS4*PS5;
|
||||
nextP(0,1) = -PS1*PS36 + PS11*PS33 - PS12*PS29 + PS13*PS26 - PS14*PS34 + PS17*PS9 - PS20*PS7 + PS23 + PS3*PS35 - PS35*PS5;
|
||||
nextP(1,1) = PS1*PS95 + PS100*PS11 + PS102*PS13 - PS105*PS34 - PS107*PS7 - PS109*PS12 + PS112 + PS2*PS5 + PS3*PS4 + PS9*PS97;
|
||||
nextP(0,2) = -PS1*PS37 + PS11*PS29 + PS12*PS33 - PS13*PS23 - PS14*PS9 - PS17*PS34 + PS20*PS6 + PS26 - PS3*PS38 + PS37*PS5;
|
||||
nextP(1,2) = PS1*PS40 + PS100*PS12 + PS102 - PS105*PS9 + PS107*PS6 + PS109*PS11 - PS112*PS13 - PS3*PS40 - PS34*PS97 - PS39*PS5;
|
||||
nextP(2,2) = PS0*PS5 + PS1*PS4 + PS11*PS128 + PS12*PS130 + PS127*PS6 - PS13*PS135 - PS132*PS34 - PS133*PS9 + PS137 + PS3*PS95;
|
||||
nextP(0,3) = PS1*PS39 - PS11*PS26 + PS12*PS23 + PS13*PS33 + PS14*PS7 - PS17*PS6 - PS20*PS34 + PS29 - PS3*PS39 - PS40*PS5;
|
||||
nextP(1,3) = -PS1*PS38 + PS100*PS13 - PS102*PS11 + PS105*PS7 - PS107*PS34 + PS109 + PS112*PS12 - PS3*PS37 + PS38*PS5 - PS6*PS97;
|
||||
nextP(2,3) = -PS1*PS35 - PS11*PS137 + PS12*PS135 - PS127*PS34 + PS128 + PS13*PS130 - PS132*PS6 + PS133*PS7 + PS3*PS36 - PS36*PS5;
|
||||
nextP(3,3) = PS0*PS3 + PS1*PS2 - PS11*PS156 + PS12*PS152 + PS13*PS153 + PS151*PS7 - PS154*PS34 - PS155*PS6 + PS157 + PS5*PS95;
|
||||
nextP(0,4) = PS43*PS44 - PS45*PS47 - PS54*PS55 + PS56*PS58 + PS61*PS62 + PS66*PS67 + PS71*PS72 + PS73;
|
||||
nextP(1,4) = PS113*PS43 - PS115*PS45 - PS116*PS54 + PS118*PS56 + PS119*PS61 + PS120*PS66 + PS121*PS71 + PS122;
|
||||
nextP(2,4) = PS138*PS43 - PS140*PS45 - PS141*PS54 + PS143*PS56 + PS144*PS61 + PS145*PS66 + PS146*PS71 + PS147;
|
||||
nextP(3,4) = PS158*PS43 - PS160*PS45 - PS161*PS54 + PS163*PS56 + PS164*PS61 + PS165*PS66 + PS166*PS71 + PS167;
|
||||
nextP(4,4) = -PS171*PS178 + PS172*PS180 + PS173*PS181 + PS174*PS182 + PS175*PS183 - PS176*PS179 + PS177*PS43 + PS184*powf(PS56, 2) + PS185*powf(PS45, 2) + PS186 + powf(PS43, 2)*dvxVar;
|
||||
nextP(0,5) = PS47*PS81 + PS55*PS85 + PS57*PS75 - PS62*PS80 - PS67*PS78 + PS72*PS83 - PS76*PS77 + PS86;
|
||||
nextP(1,5) = PS115*PS81 + PS116*PS85 + PS117*PS75 - PS119*PS80 - PS120*PS78 + PS121*PS83 - PS123*PS76 + PS124;
|
||||
nextP(2,5) = PS140*PS81 + PS141*PS85 + PS142*PS75 - PS144*PS80 - PS145*PS78 + PS146*PS83 - PS148*PS76 + PS149;
|
||||
nextP(3,5) = PS160*PS81 + PS161*PS85 + PS162*PS75 - PS164*PS80 - PS165*PS78 + PS166*PS83 - PS168*PS76 + PS169;
|
||||
nextP(4,5) = PS172*PS195 + PS178*PS190 + PS180*PS75 - PS185*PS45*PS81 - PS187*PS76 - PS188*PS78 - PS189*PS80 + PS191*PS83 + PS192*PS85 - PS193*PS194 + PS196;
|
||||
nextP(5,5) = PS185*powf(PS81, 2) + PS190*PS209 - PS193*PS206 + PS201*PS210 - PS202*PS207 + PS203*PS211 - PS204*PS208 + PS205*PS75 + PS212*powf(PS76, 2) + PS213 + powf(PS75, 2)*dvyVar;
|
||||
nextP(0,6) = PS46*PS87 + PS55*PS91 - PS58*PS88 + PS62*PS93 + PS67*PS92 - PS72*PS89 + PS77*PS90 + PS94;
|
||||
nextP(1,6) = PS114*PS87 + PS116*PS91 - PS118*PS88 + PS119*PS93 + PS120*PS92 - PS121*PS89 + PS123*PS90 + PS125;
|
||||
nextP(2,6) = PS139*PS87 + PS141*PS91 - PS143*PS88 + PS144*PS93 + PS145*PS92 - PS146*PS89 + PS148*PS90 + PS150;
|
||||
nextP(3,6) = PS159*PS87 + PS161*PS91 - PS163*PS88 + PS164*PS93 + PS165*PS92 - PS166*PS89 + PS168*PS90 + PS170;
|
||||
nextP(4,6) = -PS171*PS198 + PS178*PS87 - PS180*PS197 - PS184*PS56*PS88 + PS187*PS90 + PS188*PS92 + PS189*PS93 - PS191*PS89 + PS192*PS91 + PS194*PS199 + PS200;
|
||||
nextP(5,6) = PS190*PS198 - PS195*PS197 - PS197*PS205 + PS199*PS206 + PS207*PS216 + PS208*PS217 + PS209*PS87 - PS210*PS214 + PS211*PS215 - PS212*PS76*PS90 + PS218;
|
||||
nextP(6,6) = PS184*powf(PS88, 2) - PS197*PS220 + PS199*PS221 + PS212*powf(PS90, 2) - PS214*(P(0,2)*PS216 + P(1,2)*PS217 + P(2,13)*PS199 - P(2,14)*PS197 + P(2,15)*PS87 - P(2,2)*PS214 + P(2,3)*PS215 + P(2,6)) + PS215*(P(0,3)*PS216 + P(1,3)*PS217 - P(2,3)*PS214 + P(3,13)*PS199 - P(3,14)*PS197 + P(3,15)*PS87 + P(3,3)*PS215 + P(3,6)) + PS216*(P(0,0)*PS216 + P(0,1)*PS217 + P(0,13)*PS199 - P(0,14)*PS197 + P(0,15)*PS87 - P(0,2)*PS214 + P(0,3)*PS215 + P(0,6)) + PS217*(P(0,1)*PS216 + P(1,1)*PS217 + P(1,13)*PS199 - P(1,14)*PS197 + P(1,15)*PS87 - P(1,2)*PS214 + P(1,3)*PS215 + P(1,6)) + PS219*PS87 + PS222 + powf(PS87, 2)*dvzVar;
|
||||
nextP(0,7) = P(0,7) - P(1,7)*PS11 - P(2,7)*PS12 - P(3,7)*PS13 + P(7,10)*PS6 + P(7,11)*PS7 + P(7,12)*PS9 + PS73*dt;
|
||||
nextP(1,7) = P(0,7)*PS11 + P(1,7) + P(2,7)*PS13 - P(3,7)*PS12 - P(7,10)*PS34 + P(7,11)*PS9 - P(7,12)*PS7 + PS122*dt;
|
||||
nextP(2,7) = P(0,7)*PS12 - P(1,7)*PS13 + P(2,7) + P(3,7)*PS11 - P(7,10)*PS9 - P(7,11)*PS34 + P(7,12)*PS6 + PS147*dt;
|
||||
nextP(3,7) = P(0,7)*PS13 + P(1,7)*PS12 - P(2,7)*PS11 + P(3,7) + P(7,10)*PS7 - P(7,11)*PS6 - P(7,12)*PS34 + PS167*dt;
|
||||
nextP(4,7) = P(0,7)*PS174 + P(1,7)*PS173 + P(2,7)*PS175 - P(3,7)*PS176 + P(4,7) + P(7,13)*PS43 + P(7,14)*PS172 - P(7,15)*PS171 + PS186*dt;
|
||||
nextP(5,7) = -P(0,7)*PS202 - P(1,7)*PS204 + P(2,7)*PS201 + P(3,7)*PS203 + P(5,7) - P(7,13)*PS193 + P(7,14)*PS75 + P(7,15)*PS190 + dt*(-P(0,4)*PS202 - P(1,4)*PS204 + P(2,4)*PS201 + P(3,4)*PS203 - P(4,13)*PS193 + P(4,14)*PS75 + P(4,15)*PS190 + P(4,5));
|
||||
nextP(6,7) = P(0,7)*PS216 + P(1,7)*PS217 - P(2,7)*PS214 + P(3,7)*PS215 + P(6,7) + P(7,13)*PS199 - P(7,14)*PS197 + P(7,15)*PS87 + dt*(P(0,4)*PS216 + P(1,4)*PS217 - P(2,4)*PS214 + P(3,4)*PS215 + P(4,13)*PS199 - P(4,14)*PS197 + P(4,15)*PS87 + P(4,6));
|
||||
nextP(7,7) = P(4,7)*dt + P(7,7) + dt*(P(4,4)*dt + P(4,7));
|
||||
nextP(0,8) = P(0,8) - P(1,8)*PS11 - P(2,8)*PS12 - P(3,8)*PS13 + P(8,10)*PS6 + P(8,11)*PS7 + P(8,12)*PS9 + PS86*dt;
|
||||
nextP(1,8) = P(0,8)*PS11 + P(1,8) + P(2,8)*PS13 - P(3,8)*PS12 - P(8,10)*PS34 + P(8,11)*PS9 - P(8,12)*PS7 + PS124*dt;
|
||||
nextP(2,8) = P(0,8)*PS12 - P(1,8)*PS13 + P(2,8) + P(3,8)*PS11 - P(8,10)*PS9 - P(8,11)*PS34 + P(8,12)*PS6 + PS149*dt;
|
||||
nextP(3,8) = P(0,8)*PS13 + P(1,8)*PS12 - P(2,8)*PS11 + P(3,8) + P(8,10)*PS7 - P(8,11)*PS6 - P(8,12)*PS34 + PS169*dt;
|
||||
nextP(4,8) = P(0,8)*PS174 + P(1,8)*PS173 + P(2,8)*PS175 - P(3,8)*PS176 + P(4,8) + P(8,13)*PS43 + P(8,14)*PS172 - P(8,15)*PS171 + PS196*dt;
|
||||
nextP(5,8) = -P(0,8)*PS202 - P(1,8)*PS204 + P(2,8)*PS201 + P(3,8)*PS203 + P(5,8) - P(8,13)*PS193 + P(8,14)*PS75 + P(8,15)*PS190 + PS213*dt;
|
||||
nextP(6,8) = P(0,8)*PS216 + P(1,8)*PS217 - P(2,8)*PS214 + P(3,8)*PS215 + P(6,8) + P(8,13)*PS199 - P(8,14)*PS197 + P(8,15)*PS87 + dt*(P(0,5)*PS216 + P(1,5)*PS217 - P(2,5)*PS214 + P(3,5)*PS215 + P(5,13)*PS199 - P(5,14)*PS197 + P(5,15)*PS87 + P(5,6));
|
||||
nextP(7,8) = P(4,8)*dt + P(7,8) + dt*(P(4,5)*dt + P(5,7));
|
||||
nextP(8,8) = P(5,8)*dt + P(8,8) + dt*(P(5,5)*dt + P(5,8));
|
||||
nextP(0,9) = P(0,9) - P(1,9)*PS11 - P(2,9)*PS12 - P(3,9)*PS13 + P(9,10)*PS6 + P(9,11)*PS7 + P(9,12)*PS9 + PS94*dt;
|
||||
nextP(1,9) = P(0,9)*PS11 + P(1,9) + P(2,9)*PS13 - P(3,9)*PS12 - P(9,10)*PS34 + P(9,11)*PS9 - P(9,12)*PS7 + PS125*dt;
|
||||
nextP(2,9) = P(0,9)*PS12 - P(1,9)*PS13 + P(2,9) + P(3,9)*PS11 - P(9,10)*PS9 - P(9,11)*PS34 + P(9,12)*PS6 + PS150*dt;
|
||||
nextP(3,9) = P(0,9)*PS13 + P(1,9)*PS12 - P(2,9)*PS11 + P(3,9) + P(9,10)*PS7 - P(9,11)*PS6 - P(9,12)*PS34 + PS170*dt;
|
||||
nextP(4,9) = P(0,9)*PS174 + P(1,9)*PS173 + P(2,9)*PS175 - P(3,9)*PS176 + P(4,9) + P(9,13)*PS43 + P(9,14)*PS172 - P(9,15)*PS171 + PS200*dt;
|
||||
nextP(5,9) = -P(0,9)*PS202 - P(1,9)*PS204 + P(2,9)*PS201 + P(3,9)*PS203 + P(5,9) - P(9,13)*PS193 + P(9,14)*PS75 + P(9,15)*PS190 + PS218*dt;
|
||||
nextP(6,9) = P(0,9)*PS216 + P(1,9)*PS217 - P(2,9)*PS214 + P(3,9)*PS215 + P(6,9) + P(9,13)*PS199 - P(9,14)*PS197 + P(9,15)*PS87 + PS222*dt;
|
||||
nextP(7,9) = P(4,9)*dt + P(7,9) + dt*(P(4,6)*dt + P(6,7));
|
||||
nextP(8,9) = P(5,9)*dt + P(8,9) + dt*(P(5,6)*dt + P(6,8));
|
||||
nextP(9,9) = P(6,9)*dt + P(9,9) + dt*(P(6,6)*dt + P(6,9));
|
||||
nextP(0,10) = PS14;
|
||||
nextP(1,10) = PS105;
|
||||
nextP(2,10) = PS133;
|
||||
nextP(3,10) = PS151;
|
||||
nextP(4,10) = P(0,10)*PS174 + P(1,10)*PS173 + P(10,13)*PS43 + P(10,14)*PS172 - P(10,15)*PS171 + P(2,10)*PS175 - P(3,10)*PS176 + P(4,10);
|
||||
nextP(5,10) = -P(0,10)*PS202 - P(1,10)*PS204 - P(10,13)*PS193 + P(10,14)*PS75 + P(10,15)*PS190 + P(2,10)*PS201 + P(3,10)*PS203 + P(5,10);
|
||||
nextP(6,10) = P(0,10)*PS216 + P(1,10)*PS217 + P(10,13)*PS199 - P(10,14)*PS197 + P(10,15)*PS87 - P(2,10)*PS214 + P(3,10)*PS215 + P(6,10);
|
||||
nextP(7,10) = P(4,10)*dt + P(7,10);
|
||||
nextP(8,10) = P(5,10)*dt + P(8,10);
|
||||
nextP(9,10) = P(6,10)*dt + P(9,10);
|
||||
nextP(10,10) = P(10,10);
|
||||
nextP(0,11) = PS17;
|
||||
nextP(1,11) = PS97;
|
||||
nextP(2,11) = PS132;
|
||||
nextP(3,11) = PS155;
|
||||
nextP(4,11) = P(0,11)*PS174 + P(1,11)*PS173 + P(11,13)*PS43 + P(11,14)*PS172 - P(11,15)*PS171 + P(2,11)*PS175 - P(3,11)*PS176 + P(4,11);
|
||||
nextP(5,11) = -P(0,11)*PS202 - P(1,11)*PS204 - P(11,13)*PS193 + P(11,14)*PS75 + P(11,15)*PS190 + P(2,11)*PS201 + P(3,11)*PS203 + P(5,11);
|
||||
nextP(6,11) = P(0,11)*PS216 + P(1,11)*PS217 + P(11,13)*PS199 - P(11,14)*PS197 + P(11,15)*PS87 - P(2,11)*PS214 + P(3,11)*PS215 + P(6,11);
|
||||
nextP(7,11) = P(4,11)*dt + P(7,11);
|
||||
nextP(8,11) = P(5,11)*dt + P(8,11);
|
||||
nextP(9,11) = P(6,11)*dt + P(9,11);
|
||||
nextP(10,11) = P(10,11);
|
||||
nextP(11,11) = P(11,11);
|
||||
nextP(0,12) = PS20;
|
||||
nextP(1,12) = PS107;
|
||||
nextP(2,12) = PS127;
|
||||
nextP(3,12) = PS154;
|
||||
nextP(4,12) = P(0,12)*PS174 + P(1,12)*PS173 + P(12,13)*PS43 + P(12,14)*PS172 - P(12,15)*PS171 + P(2,12)*PS175 - P(3,12)*PS176 + P(4,12);
|
||||
nextP(5,12) = -P(0,12)*PS202 - P(1,12)*PS204 - P(12,13)*PS193 + P(12,14)*PS75 + P(12,15)*PS190 + P(2,12)*PS201 + P(3,12)*PS203 + P(5,12);
|
||||
nextP(6,12) = P(0,12)*PS216 + P(1,12)*PS217 + P(12,13)*PS199 - P(12,14)*PS197 + P(12,15)*PS87 - P(2,12)*PS214 + P(3,12)*PS215 + P(6,12);
|
||||
nextP(7,12) = P(4,12)*dt + P(7,12);
|
||||
nextP(8,12) = P(5,12)*dt + P(8,12);
|
||||
nextP(9,12) = P(6,12)*dt + P(9,12);
|
||||
nextP(10,12) = P(10,12);
|
||||
nextP(11,12) = P(11,12);
|
||||
nextP(12,12) = P(12,12);
|
||||
nextP(0,13) = PS44;
|
||||
nextP(1,13) = PS113;
|
||||
nextP(2,13) = PS138;
|
||||
nextP(3,13) = PS158;
|
||||
nextP(4,13) = PS177;
|
||||
nextP(5,13) = PS206;
|
||||
nextP(6,13) = PS221;
|
||||
nextP(7,13) = P(4,13)*dt + P(7,13);
|
||||
nextP(8,13) = P(5,13)*dt + P(8,13);
|
||||
nextP(9,13) = P(6,13)*dt + P(9,13);
|
||||
nextP(10,13) = P(10,13);
|
||||
nextP(11,13) = P(11,13);
|
||||
nextP(12,13) = P(12,13);
|
||||
nextP(13,13) = P(13,13);
|
||||
nextP(0,14) = PS57;
|
||||
nextP(1,14) = PS117;
|
||||
nextP(2,14) = PS142;
|
||||
nextP(3,14) = PS162;
|
||||
nextP(4,14) = PS180;
|
||||
nextP(5,14) = PS205;
|
||||
nextP(6,14) = PS220;
|
||||
nextP(7,14) = P(4,14)*dt + P(7,14);
|
||||
nextP(8,14) = P(5,14)*dt + P(8,14);
|
||||
nextP(9,14) = P(6,14)*dt + P(9,14);
|
||||
nextP(10,14) = P(10,14);
|
||||
nextP(11,14) = P(11,14);
|
||||
nextP(12,14) = P(12,14);
|
||||
nextP(13,14) = P(13,14);
|
||||
nextP(14,14) = P(14,14);
|
||||
nextP(0,15) = PS46;
|
||||
nextP(1,15) = PS114;
|
||||
nextP(2,15) = PS139;
|
||||
nextP(3,15) = PS159;
|
||||
nextP(4,15) = PS178;
|
||||
nextP(5,15) = PS209;
|
||||
nextP(6,15) = PS219;
|
||||
nextP(7,15) = P(4,15)*dt + P(7,15);
|
||||
nextP(8,15) = P(5,15)*dt + P(8,15);
|
||||
nextP(9,15) = P(6,15)*dt + P(9,15);
|
||||
nextP(10,15) = P(10,15);
|
||||
nextP(11,15) = P(11,15);
|
||||
nextP(12,15) = P(12,15);
|
||||
nextP(13,15) = P(13,15);
|
||||
nextP(14,15) = P(14,15);
|
||||
nextP(15,15) = P(15,15);
|
||||
nextP(0,16) = P(0,16) - P(1,16)*PS11 + P(10,16)*PS6 + P(11,16)*PS7 + P(12,16)*PS9 - P(2,16)*PS12 - P(3,16)*PS13;
|
||||
nextP(1,16) = P(0,16)*PS11 + P(1,16) - P(10,16)*PS34 + P(11,16)*PS9 - P(12,16)*PS7 + P(2,16)*PS13 - P(3,16)*PS12;
|
||||
nextP(2,16) = P(0,16)*PS12 - P(1,16)*PS13 - P(10,16)*PS9 - P(11,16)*PS34 + P(12,16)*PS6 + P(2,16) + P(3,16)*PS11;
|
||||
nextP(3,16) = P(0,16)*PS13 + P(1,16)*PS12 + P(10,16)*PS7 - P(11,16)*PS6 - P(12,16)*PS34 - P(2,16)*PS11 + P(3,16);
|
||||
nextP(4,16) = P(0,16)*PS174 + P(1,16)*PS173 + P(13,16)*PS43 + P(14,16)*PS172 - P(15,16)*PS171 + P(2,16)*PS175 - P(3,16)*PS176 + P(4,16);
|
||||
nextP(5,16) = -P(0,16)*PS202 - P(1,16)*PS204 - P(13,16)*PS193 + P(14,16)*PS75 + P(15,16)*PS190 + P(2,16)*PS201 + P(3,16)*PS203 + P(5,16);
|
||||
nextP(6,16) = P(0,16)*PS216 + P(1,16)*PS217 + P(13,16)*PS199 - P(14,16)*PS197 + P(15,16)*PS87 - P(2,16)*PS214 + P(3,16)*PS215 + P(6,16);
|
||||
nextP(7,16) = P(4,16)*dt + P(7,16);
|
||||
nextP(8,16) = P(5,16)*dt + P(8,16);
|
||||
nextP(9,16) = P(6,16)*dt + P(9,16);
|
||||
nextP(10,16) = P(10,16);
|
||||
nextP(11,16) = P(11,16);
|
||||
nextP(12,16) = P(12,16);
|
||||
nextP(13,16) = P(13,16);
|
||||
nextP(14,16) = P(14,16);
|
||||
nextP(15,16) = P(15,16);
|
||||
nextP(16,16) = P(16,16);
|
||||
nextP(0,17) = P(0,17) - P(1,17)*PS11 + P(10,17)*PS6 + P(11,17)*PS7 + P(12,17)*PS9 - P(2,17)*PS12 - P(3,17)*PS13;
|
||||
nextP(1,17) = P(0,17)*PS11 + P(1,17) - P(10,17)*PS34 + P(11,17)*PS9 - P(12,17)*PS7 + P(2,17)*PS13 - P(3,17)*PS12;
|
||||
nextP(2,17) = P(0,17)*PS12 - P(1,17)*PS13 - P(10,17)*PS9 - P(11,17)*PS34 + P(12,17)*PS6 + P(2,17) + P(3,17)*PS11;
|
||||
nextP(3,17) = P(0,17)*PS13 + P(1,17)*PS12 + P(10,17)*PS7 - P(11,17)*PS6 - P(12,17)*PS34 - P(2,17)*PS11 + P(3,17);
|
||||
nextP(4,17) = P(0,17)*PS174 + P(1,17)*PS173 + P(13,17)*PS43 + P(14,17)*PS172 - P(15,17)*PS171 + P(2,17)*PS175 - P(3,17)*PS176 + P(4,17);
|
||||
nextP(5,17) = -P(0,17)*PS202 - P(1,17)*PS204 - P(13,17)*PS193 + P(14,17)*PS75 + P(15,17)*PS190 + P(2,17)*PS201 + P(3,17)*PS203 + P(5,17);
|
||||
nextP(6,17) = P(0,17)*PS216 + P(1,17)*PS217 + P(13,17)*PS199 - P(14,17)*PS197 + P(15,17)*PS87 - P(2,17)*PS214 + P(3,17)*PS215 + P(6,17);
|
||||
nextP(7,17) = P(4,17)*dt + P(7,17);
|
||||
nextP(8,17) = P(5,17)*dt + P(8,17);
|
||||
nextP(9,17) = P(6,17)*dt + P(9,17);
|
||||
nextP(10,17) = P(10,17);
|
||||
nextP(11,17) = P(11,17);
|
||||
nextP(12,17) = P(12,17);
|
||||
nextP(13,17) = P(13,17);
|
||||
nextP(14,17) = P(14,17);
|
||||
nextP(15,17) = P(15,17);
|
||||
nextP(16,17) = P(16,17);
|
||||
nextP(17,17) = P(17,17);
|
||||
nextP(0,18) = P(0,18) - P(1,18)*PS11 + P(10,18)*PS6 + P(11,18)*PS7 + P(12,18)*PS9 - P(2,18)*PS12 - P(3,18)*PS13;
|
||||
nextP(1,18) = P(0,18)*PS11 + P(1,18) - P(10,18)*PS34 + P(11,18)*PS9 - P(12,18)*PS7 + P(2,18)*PS13 - P(3,18)*PS12;
|
||||
nextP(2,18) = P(0,18)*PS12 - P(1,18)*PS13 - P(10,18)*PS9 - P(11,18)*PS34 + P(12,18)*PS6 + P(2,18) + P(3,18)*PS11;
|
||||
nextP(3,18) = P(0,18)*PS13 + P(1,18)*PS12 + P(10,18)*PS7 - P(11,18)*PS6 - P(12,18)*PS34 - P(2,18)*PS11 + P(3,18);
|
||||
nextP(4,18) = P(0,18)*PS174 + P(1,18)*PS173 + P(13,18)*PS43 + P(14,18)*PS172 - P(15,18)*PS171 + P(2,18)*PS175 - P(3,18)*PS176 + P(4,18);
|
||||
nextP(5,18) = -P(0,18)*PS202 - P(1,18)*PS204 - P(13,18)*PS193 + P(14,18)*PS75 + P(15,18)*PS190 + P(2,18)*PS201 + P(3,18)*PS203 + P(5,18);
|
||||
nextP(6,18) = P(0,18)*PS216 + P(1,18)*PS217 + P(13,18)*PS199 - P(14,18)*PS197 + P(15,18)*PS87 - P(2,18)*PS214 + P(3,18)*PS215 + P(6,18);
|
||||
nextP(7,18) = P(4,18)*dt + P(7,18);
|
||||
nextP(8,18) = P(5,18)*dt + P(8,18);
|
||||
nextP(9,18) = P(6,18)*dt + P(9,18);
|
||||
nextP(10,18) = P(10,18);
|
||||
nextP(11,18) = P(11,18);
|
||||
nextP(12,18) = P(12,18);
|
||||
nextP(13,18) = P(13,18);
|
||||
nextP(14,18) = P(14,18);
|
||||
nextP(15,18) = P(15,18);
|
||||
nextP(16,18) = P(16,18);
|
||||
nextP(17,18) = P(17,18);
|
||||
nextP(18,18) = P(18,18);
|
||||
nextP(0,19) = P(0,19) - P(1,19)*PS11 + P(10,19)*PS6 + P(11,19)*PS7 + P(12,19)*PS9 - P(2,19)*PS12 - P(3,19)*PS13;
|
||||
nextP(1,19) = P(0,19)*PS11 + P(1,19) - P(10,19)*PS34 + P(11,19)*PS9 - P(12,19)*PS7 + P(2,19)*PS13 - P(3,19)*PS12;
|
||||
nextP(2,19) = P(0,19)*PS12 - P(1,19)*PS13 - P(10,19)*PS9 - P(11,19)*PS34 + P(12,19)*PS6 + P(2,19) + P(3,19)*PS11;
|
||||
nextP(3,19) = P(0,19)*PS13 + P(1,19)*PS12 + P(10,19)*PS7 - P(11,19)*PS6 - P(12,19)*PS34 - P(2,19)*PS11 + P(3,19);
|
||||
nextP(4,19) = P(0,19)*PS174 + P(1,19)*PS173 + P(13,19)*PS43 + P(14,19)*PS172 - P(15,19)*PS171 + P(2,19)*PS175 - P(3,19)*PS176 + P(4,19);
|
||||
nextP(5,19) = -P(0,19)*PS202 - P(1,19)*PS204 - P(13,19)*PS193 + P(14,19)*PS75 + P(15,19)*PS190 + P(2,19)*PS201 + P(3,19)*PS203 + P(5,19);
|
||||
nextP(6,19) = P(0,19)*PS216 + P(1,19)*PS217 + P(13,19)*PS199 - P(14,19)*PS197 + P(15,19)*PS87 - P(2,19)*PS214 + P(3,19)*PS215 + P(6,19);
|
||||
nextP(7,19) = P(4,19)*dt + P(7,19);
|
||||
nextP(8,19) = P(5,19)*dt + P(8,19);
|
||||
nextP(9,19) = P(6,19)*dt + P(9,19);
|
||||
nextP(10,19) = P(10,19);
|
||||
nextP(11,19) = P(11,19);
|
||||
nextP(12,19) = P(12,19);
|
||||
nextP(13,19) = P(13,19);
|
||||
nextP(14,19) = P(14,19);
|
||||
nextP(15,19) = P(15,19);
|
||||
nextP(16,19) = P(16,19);
|
||||
nextP(17,19) = P(17,19);
|
||||
nextP(18,19) = P(18,19);
|
||||
nextP(19,19) = P(19,19);
|
||||
nextP(0,20) = P(0,20) - P(1,20)*PS11 + P(10,20)*PS6 + P(11,20)*PS7 + P(12,20)*PS9 - P(2,20)*PS12 - P(3,20)*PS13;
|
||||
nextP(1,20) = P(0,20)*PS11 + P(1,20) - P(10,20)*PS34 + P(11,20)*PS9 - P(12,20)*PS7 + P(2,20)*PS13 - P(3,20)*PS12;
|
||||
nextP(2,20) = P(0,20)*PS12 - P(1,20)*PS13 - P(10,20)*PS9 - P(11,20)*PS34 + P(12,20)*PS6 + P(2,20) + P(3,20)*PS11;
|
||||
nextP(3,20) = P(0,20)*PS13 + P(1,20)*PS12 + P(10,20)*PS7 - P(11,20)*PS6 - P(12,20)*PS34 - P(2,20)*PS11 + P(3,20);
|
||||
nextP(4,20) = P(0,20)*PS174 + P(1,20)*PS173 + P(13,20)*PS43 + P(14,20)*PS172 - P(15,20)*PS171 + P(2,20)*PS175 - P(3,20)*PS176 + P(4,20);
|
||||
nextP(5,20) = -P(0,20)*PS202 - P(1,20)*PS204 - P(13,20)*PS193 + P(14,20)*PS75 + P(15,20)*PS190 + P(2,20)*PS201 + P(3,20)*PS203 + P(5,20);
|
||||
nextP(6,20) = P(0,20)*PS216 + P(1,20)*PS217 + P(13,20)*PS199 - P(14,20)*PS197 + P(15,20)*PS87 - P(2,20)*PS214 + P(3,20)*PS215 + P(6,20);
|
||||
nextP(7,20) = P(4,20)*dt + P(7,20);
|
||||
nextP(8,20) = P(5,20)*dt + P(8,20);
|
||||
nextP(9,20) = P(6,20)*dt + P(9,20);
|
||||
nextP(10,20) = P(10,20);
|
||||
nextP(11,20) = P(11,20);
|
||||
nextP(12,20) = P(12,20);
|
||||
nextP(13,20) = P(13,20);
|
||||
nextP(14,20) = P(14,20);
|
||||
nextP(15,20) = P(15,20);
|
||||
nextP(16,20) = P(16,20);
|
||||
nextP(17,20) = P(17,20);
|
||||
nextP(18,20) = P(18,20);
|
||||
nextP(19,20) = P(19,20);
|
||||
nextP(20,20) = P(20,20);
|
||||
nextP(0,21) = P(0,21) - P(1,21)*PS11 + P(10,21)*PS6 + P(11,21)*PS7 + P(12,21)*PS9 - P(2,21)*PS12 - P(3,21)*PS13;
|
||||
nextP(1,21) = P(0,21)*PS11 + P(1,21) - P(10,21)*PS34 + P(11,21)*PS9 - P(12,21)*PS7 + P(2,21)*PS13 - P(3,21)*PS12;
|
||||
nextP(2,21) = P(0,21)*PS12 - P(1,21)*PS13 - P(10,21)*PS9 - P(11,21)*PS34 + P(12,21)*PS6 + P(2,21) + P(3,21)*PS11;
|
||||
nextP(3,21) = P(0,21)*PS13 + P(1,21)*PS12 + P(10,21)*PS7 - P(11,21)*PS6 - P(12,21)*PS34 - P(2,21)*PS11 + P(3,21);
|
||||
nextP(4,21) = P(0,21)*PS174 + P(1,21)*PS173 + P(13,21)*PS43 + P(14,21)*PS172 - P(15,21)*PS171 + P(2,21)*PS175 - P(3,21)*PS176 + P(4,21);
|
||||
nextP(5,21) = -P(0,21)*PS202 - P(1,21)*PS204 - P(13,21)*PS193 + P(14,21)*PS75 + P(15,21)*PS190 + P(2,21)*PS201 + P(3,21)*PS203 + P(5,21);
|
||||
nextP(6,21) = P(0,21)*PS216 + P(1,21)*PS217 + P(13,21)*PS199 - P(14,21)*PS197 + P(15,21)*PS87 - P(2,21)*PS214 + P(3,21)*PS215 + P(6,21);
|
||||
nextP(7,21) = P(4,21)*dt + P(7,21);
|
||||
nextP(8,21) = P(5,21)*dt + P(8,21);
|
||||
nextP(9,21) = P(6,21)*dt + P(9,21);
|
||||
nextP(10,21) = P(10,21);
|
||||
nextP(11,21) = P(11,21);
|
||||
nextP(12,21) = P(12,21);
|
||||
nextP(13,21) = P(13,21);
|
||||
nextP(14,21) = P(14,21);
|
||||
nextP(15,21) = P(15,21);
|
||||
nextP(16,21) = P(16,21);
|
||||
nextP(17,21) = P(17,21);
|
||||
nextP(18,21) = P(18,21);
|
||||
nextP(19,21) = P(19,21);
|
||||
nextP(20,21) = P(20,21);
|
||||
nextP(21,21) = P(21,21);
|
||||
nextP(0,22) = P(0,22) - P(1,22)*PS11 + P(10,22)*PS6 + P(11,22)*PS7 + P(12,22)*PS9 - P(2,22)*PS12 - P(3,22)*PS13;
|
||||
nextP(1,22) = P(0,22)*PS11 + P(1,22) - P(10,22)*PS34 + P(11,22)*PS9 - P(12,22)*PS7 + P(2,22)*PS13 - P(3,22)*PS12;
|
||||
nextP(2,22) = P(0,22)*PS12 - P(1,22)*PS13 - P(10,22)*PS9 - P(11,22)*PS34 + P(12,22)*PS6 + P(2,22) + P(3,22)*PS11;
|
||||
nextP(3,22) = P(0,22)*PS13 + P(1,22)*PS12 + P(10,22)*PS7 - P(11,22)*PS6 - P(12,22)*PS34 - P(2,22)*PS11 + P(3,22);
|
||||
nextP(4,22) = P(0,22)*PS174 + P(1,22)*PS173 + P(13,22)*PS43 + P(14,22)*PS172 - P(15,22)*PS171 + P(2,22)*PS175 - P(3,22)*PS176 + P(4,22);
|
||||
nextP(5,22) = -P(0,22)*PS202 - P(1,22)*PS204 - P(13,22)*PS193 + P(14,22)*PS75 + P(15,22)*PS190 + P(2,22)*PS201 + P(3,22)*PS203 + P(5,22);
|
||||
nextP(6,22) = P(0,22)*PS216 + P(1,22)*PS217 + P(13,22)*PS199 - P(14,22)*PS197 + P(15,22)*PS87 - P(2,22)*PS214 + P(3,22)*PS215 + P(6,22);
|
||||
nextP(7,22) = P(4,22)*dt + P(7,22);
|
||||
nextP(8,22) = P(5,22)*dt + P(8,22);
|
||||
nextP(9,22) = P(6,22)*dt + P(9,22);
|
||||
nextP(10,22) = P(10,22);
|
||||
nextP(11,22) = P(11,22);
|
||||
nextP(12,22) = P(12,22);
|
||||
nextP(13,22) = P(13,22);
|
||||
nextP(14,22) = P(14,22);
|
||||
nextP(15,22) = P(15,22);
|
||||
nextP(16,22) = P(16,22);
|
||||
nextP(17,22) = P(17,22);
|
||||
nextP(18,22) = P(18,22);
|
||||
nextP(19,22) = P(19,22);
|
||||
nextP(20,22) = P(20,22);
|
||||
nextP(21,22) = P(21,22);
|
||||
nextP(22,22) = P(22,22);
|
||||
nextP(0,23) = P(0,23) - P(1,23)*PS11 + P(10,23)*PS6 + P(11,23)*PS7 + P(12,23)*PS9 - P(2,23)*PS12 - P(3,23)*PS13;
|
||||
nextP(1,23) = P(0,23)*PS11 + P(1,23) - P(10,23)*PS34 + P(11,23)*PS9 - P(12,23)*PS7 + P(2,23)*PS13 - P(3,23)*PS12;
|
||||
nextP(2,23) = P(0,23)*PS12 - P(1,23)*PS13 - P(10,23)*PS9 - P(11,23)*PS34 + P(12,23)*PS6 + P(2,23) + P(3,23)*PS11;
|
||||
nextP(3,23) = P(0,23)*PS13 + P(1,23)*PS12 + P(10,23)*PS7 - P(11,23)*PS6 - P(12,23)*PS34 - P(2,23)*PS11 + P(3,23);
|
||||
nextP(4,23) = P(0,23)*PS174 + P(1,23)*PS173 + P(13,23)*PS43 + P(14,23)*PS172 - P(15,23)*PS171 + P(2,23)*PS175 - P(3,23)*PS176 + P(4,23);
|
||||
nextP(5,23) = -P(0,23)*PS202 - P(1,23)*PS204 - P(13,23)*PS193 + P(14,23)*PS75 + P(15,23)*PS190 + P(2,23)*PS201 + P(3,23)*PS203 + P(5,23);
|
||||
nextP(6,23) = P(0,23)*PS216 + P(1,23)*PS217 + P(13,23)*PS199 - P(14,23)*PS197 + P(15,23)*PS87 - P(2,23)*PS214 + P(3,23)*PS215 + P(6,23);
|
||||
nextP(7,23) = P(4,23)*dt + P(7,23);
|
||||
nextP(8,23) = P(5,23)*dt + P(8,23);
|
||||
nextP(9,23) = P(6,23)*dt + P(9,23);
|
||||
nextP(10,23) = P(10,23);
|
||||
nextP(11,23) = P(11,23);
|
||||
nextP(12,23) = P(12,23);
|
||||
nextP(13,23) = P(13,23);
|
||||
nextP(14,23) = P(14,23);
|
||||
nextP(15,23) = P(15,23);
|
||||
nextP(16,23) = P(16,23);
|
||||
nextP(17,23) = P(17,23);
|
||||
nextP(18,23) = P(18,23);
|
||||
nextP(19,23) = P(19,23);
|
||||
nextP(20,23) = P(20,23);
|
||||
nextP(21,23) = P(21,23);
|
||||
nextP(22,23) = P(22,23);
|
||||
nextP(23,23) = P(23,23);
|
||||
|
||||
|
||||
+955
@@ -0,0 +1,955 @@
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <cstdlib>
|
||||
#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;
|
||||
}
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
// create input data
|
||||
const float dt = 0.01f;
|
||||
|
||||
for (uint8_t iteration=0; iteration<100; iteration++) {
|
||||
// quaternion states 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;
|
||||
|
||||
// up to 1 rad/sec of rate
|
||||
const float dax = 2.0f * dt * ((float)rand() - 0.5f);
|
||||
const float day = 2.0f * dt * ((float)rand() - 0.5f);
|
||||
const float daz = 2.0f * dt * ((float)rand() - 0.5f);
|
||||
|
||||
// up to 2g of accel
|
||||
const float dvx = 2.0f * 20.0f * dt * ((float)rand() - 0.5f);
|
||||
const float dvy = 2.0f * 20.0f * dt * ((float)rand() - 0.5f);
|
||||
const float dvz = 2.0f * 20.0f * dt * ((float)rand() - 0.5f);
|
||||
|
||||
// up to 0.1 rad/sec of gyro bias
|
||||
const float dax_b = 2.0f * 0.1f * dt * ((float)rand() - 0.5f);
|
||||
const float day_b = 2.0f * 0.1f * dt * ((float)rand() - 0.5f);
|
||||
const float daz_b = 2.0f * 0.1f * dt * ((float)rand() - 0.5f);
|
||||
|
||||
// up to 0.5 m/s/s of accel bias
|
||||
const float dvx_b = 2.0f * 0.5f * dt * ((float)rand() - 0.5f);
|
||||
const float dvy_b = 2.0f * 0.5f * dt * ((float)rand() - 0.5f);
|
||||
const float dvz_b = 2.0f * 0.5f * dt * ((float)rand() - 0.5f);
|
||||
|
||||
|
||||
const float daxVar = sq(dt * 0.015f);
|
||||
const float dayVar = daxVar;
|
||||
const float dazVar = daxVar;
|
||||
|
||||
const float dvxVar = sq(dt * 0.3f);
|
||||
const float dvyVar = dvxVar;
|
||||
const float dvzVar = dvxVar;
|
||||
|
||||
// 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) = 0.01f * 2.0f * ((float)rand() - 0.5f);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Equations for covariance matrix prediction, without process noise!
|
||||
const float PS0 = ecl::powf(q1, 2);
|
||||
const float PS1 = 0.25F*daxVar;
|
||||
const float PS2 = ecl::powf(q2, 2);
|
||||
const float PS3 = 0.25F*dayVar;
|
||||
const float PS4 = ecl::powf(q3, 2);
|
||||
const float PS5 = 0.25F*dazVar;
|
||||
const float PS6 = 0.5F*q1;
|
||||
const float PS7 = 0.5F*q2;
|
||||
const float PS8 = P(10,11)*PS7;
|
||||
const float PS9 = 0.5F*q3;
|
||||
const float PS10 = P(10,12)*PS9;
|
||||
const float PS11 = 0.5F*dax - 0.5F*dax_b;
|
||||
const float PS12 = 0.5F*day - 0.5F*day_b;
|
||||
const float PS13 = 0.5F*daz - 0.5F*daz_b;
|
||||
const float PS14 = P(0,10) - P(1,10)*PS11 + P(10,10)*PS6 - P(2,10)*PS12 - P(3,10)*PS13 + PS10 + PS8;
|
||||
const float PS15 = P(10,11)*PS6;
|
||||
const float PS16 = P(11,12)*PS9;
|
||||
const float PS17 = P(0,11) - P(1,11)*PS11 + P(11,11)*PS7 - P(2,11)*PS12 - P(3,11)*PS13 + PS15 + PS16;
|
||||
const float PS18 = P(10,12)*PS6;
|
||||
const float PS19 = P(11,12)*PS7;
|
||||
const float PS20 = P(0,12) - P(1,12)*PS11 + P(12,12)*PS9 - P(2,12)*PS12 - P(3,12)*PS13 + PS18 + PS19;
|
||||
const float PS21 = P(1,2)*PS12;
|
||||
const float PS22 = -P(1,3)*PS13;
|
||||
const float PS23 = P(0,1) - P(1,1)*PS11 + P(1,10)*PS6 + P(1,11)*PS7 + P(1,12)*PS9 - PS21 + PS22;
|
||||
const float PS24 = -P(1,2)*PS11;
|
||||
const float PS25 = P(2,3)*PS13;
|
||||
const float PS26 = P(0,2) + P(2,10)*PS6 + P(2,11)*PS7 + P(2,12)*PS9 - P(2,2)*PS12 + PS24 - PS25;
|
||||
const float PS27 = P(1,3)*PS11;
|
||||
const float PS28 = -P(2,3)*PS12;
|
||||
const float PS29 = P(0,3) + P(3,10)*PS6 + P(3,11)*PS7 + P(3,12)*PS9 - P(3,3)*PS13 - PS27 + PS28;
|
||||
const float PS30 = P(0,1)*PS11;
|
||||
const float PS31 = P(0,2)*PS12;
|
||||
const float PS32 = P(0,3)*PS13;
|
||||
const float PS33 = P(0,0) + P(0,10)*PS6 + P(0,11)*PS7 + P(0,12)*PS9 - PS30 - PS31 - PS32;
|
||||
const float PS34 = 0.5F*q0;
|
||||
const float PS35 = q2*q3;
|
||||
const float PS36 = q0*q1;
|
||||
const float PS37 = q1*q3;
|
||||
const float PS38 = q0*q2;
|
||||
const float PS39 = q1*q2;
|
||||
const float PS40 = q0*q3;
|
||||
const float PS41 = -PS2;
|
||||
const float PS42 = ecl::powf(q0, 2);
|
||||
const float PS43 = -PS4 + PS42;
|
||||
const float PS44 = PS0 + PS41 + PS43;
|
||||
const float PS45 = P(0,13) - P(1,13)*PS11 + P(10,13)*PS6 + P(11,13)*PS7 + P(12,13)*PS9 - P(2,13)*PS12 - P(3,13)*PS13;
|
||||
const float PS46 = PS37 + PS38;
|
||||
const float PS47 = P(0,15) - P(1,15)*PS11 + P(10,15)*PS6 + P(11,15)*PS7 + P(12,15)*PS9 - P(2,15)*PS12 - P(3,15)*PS13;
|
||||
const float PS48 = 2*PS47;
|
||||
const float PS49 = dvy - dvy_b;
|
||||
const float PS50 = dvx - dvx_b;
|
||||
const float PS51 = dvz - dvz_b;
|
||||
const float PS52 = PS49*q0 + PS50*q3 - PS51*q1;
|
||||
const float PS53 = 2*PS29;
|
||||
const float PS54 = -PS39 + PS40;
|
||||
const float PS55 = P(0,14) - P(1,14)*PS11 + P(10,14)*PS6 + P(11,14)*PS7 + P(12,14)*PS9 - P(2,14)*PS12 - P(3,14)*PS13;
|
||||
const float PS56 = 2*PS55;
|
||||
const float PS57 = -PS49*q3 + PS50*q0 + PS51*q2;
|
||||
const float PS58 = 2*PS33;
|
||||
const float PS59 = PS49*q1 - PS50*q2 + PS51*q0;
|
||||
const float PS60 = 2*PS59;
|
||||
const float PS61 = PS49*q2 + PS50*q1 + PS51*q3;
|
||||
const float PS62 = 2*PS61;
|
||||
const float PS63 = P(0,4) - P(1,4)*PS11 - P(2,4)*PS12 - P(3,4)*PS13 + P(4,10)*PS6 + P(4,11)*PS7 + P(4,12)*PS9;
|
||||
const float PS64 = -PS0;
|
||||
const float PS65 = PS2 + PS43 + PS64;
|
||||
const float PS66 = PS39 + PS40;
|
||||
const float PS67 = 2*PS45;
|
||||
const float PS68 = -PS35 + PS36;
|
||||
const float PS69 = P(0,5) - P(1,5)*PS11 - P(2,5)*PS12 - P(3,5)*PS13 + P(5,10)*PS6 + P(5,11)*PS7 + P(5,12)*PS9;
|
||||
const float PS70 = PS4 + PS41 + PS42 + PS64;
|
||||
const float PS71 = PS35 + PS36;
|
||||
const float PS72 = 2*PS57;
|
||||
const float PS73 = -PS37 + PS38;
|
||||
const float PS74 = 2*PS52;
|
||||
const float PS75 = P(0,6) - P(1,6)*PS11 - P(2,6)*PS12 - P(3,6)*PS13 + P(6,10)*PS6 + P(6,11)*PS7 + P(6,12)*PS9;
|
||||
const float PS76 = -P(10,11)*PS34;
|
||||
const float PS77 = P(0,11)*PS11 + P(1,11) + P(11,11)*PS9 + P(2,11)*PS13 - P(3,11)*PS12 - PS19 + PS76;
|
||||
const float PS78 = P(0,2)*PS13;
|
||||
const float PS79 = P(0,3)*PS12;
|
||||
const float PS80 = P(0,0)*PS11 + P(0,1) - P(0,10)*PS34 + P(0,11)*PS9 - P(0,12)*PS7 + PS78 - PS79;
|
||||
const float PS81 = P(0,2)*PS11;
|
||||
const float PS82 = P(1,2) - P(2,10)*PS34 + P(2,11)*PS9 - P(2,12)*PS7 + P(2,2)*PS13 + PS28 + PS81;
|
||||
const float PS83 = P(10,11)*PS9;
|
||||
const float PS84 = P(10,12)*PS7;
|
||||
const float PS85 = P(0,10)*PS11 + P(1,10) - P(10,10)*PS34 + P(2,10)*PS13 - P(3,10)*PS12 + PS83 - PS84;
|
||||
const float PS86 = -P(10,12)*PS34;
|
||||
const float PS87 = P(0,12)*PS11 + P(1,12) - P(12,12)*PS7 + P(2,12)*PS13 - P(3,12)*PS12 + PS16 + PS86;
|
||||
const float PS88 = P(0,3)*PS11;
|
||||
const float PS89 = P(1,3) - P(3,10)*PS34 + P(3,11)*PS9 - P(3,12)*PS7 - P(3,3)*PS12 + PS25 + PS88;
|
||||
const float PS90 = P(1,2)*PS13;
|
||||
const float PS91 = P(1,3)*PS12;
|
||||
const float PS92 = P(1,1) - P(1,10)*PS34 + P(1,11)*PS9 - P(1,12)*PS7 + PS30 + PS90 - PS91;
|
||||
const float PS93 = P(0,13)*PS11 + P(1,13) - P(10,13)*PS34 + P(11,13)*PS9 - P(12,13)*PS7 + P(2,13)*PS13 - P(3,13)*PS12;
|
||||
const float PS94 = P(0,15)*PS11 + P(1,15) - P(10,15)*PS34 + P(11,15)*PS9 - P(12,15)*PS7 + P(2,15)*PS13 - P(3,15)*PS12;
|
||||
const float PS95 = 2*PS94;
|
||||
const float PS96 = P(0,14)*PS11 + P(1,14) - P(10,14)*PS34 + P(11,14)*PS9 - P(12,14)*PS7 + P(2,14)*PS13 - P(3,14)*PS12;
|
||||
const float PS97 = 2*PS96;
|
||||
const float PS98 = P(0,4)*PS11 + P(1,4) + P(2,4)*PS13 - P(3,4)*PS12 - P(4,10)*PS34 + P(4,11)*PS9 - P(4,12)*PS7;
|
||||
const float PS99 = 2*PS93;
|
||||
const float PS100 = P(0,5)*PS11 + P(1,5) + P(2,5)*PS13 - P(3,5)*PS12 - P(5,10)*PS34 + P(5,11)*PS9 - P(5,12)*PS7;
|
||||
const float PS101 = P(0,6)*PS11 + P(1,6) + P(2,6)*PS13 - P(3,6)*PS12 - P(6,10)*PS34 + P(6,11)*PS9 - P(6,12)*PS7;
|
||||
const float PS102 = -P(11,12)*PS34;
|
||||
const float PS103 = P(0,12)*PS12 - P(1,12)*PS13 + P(12,12)*PS6 + P(2,12) + P(3,12)*PS11 - PS10 + PS102;
|
||||
const float PS104 = P(2,3) - P(3,10)*PS9 - P(3,11)*PS34 + P(3,12)*PS6 + P(3,3)*PS11 + PS22 + PS79;
|
||||
const float PS105 = P(0,1)*PS13;
|
||||
const float PS106 = P(0,0)*PS12 - P(0,10)*PS9 - P(0,11)*PS34 + P(0,12)*PS6 + P(0,2) - PS105 + PS88;
|
||||
const float PS107 = P(11,12)*PS6;
|
||||
const float PS108 = P(0,11)*PS12 - P(1,11)*PS13 - P(11,11)*PS34 + P(2,11) + P(3,11)*PS11 + PS107 - PS83;
|
||||
const float PS109 = P(0,10)*PS12 - P(1,10)*PS13 - P(10,10)*PS9 + P(2,10) + P(3,10)*PS11 + PS18 + PS76;
|
||||
const float PS110 = P(0,1)*PS12;
|
||||
const float PS111 = -P(1,1)*PS13 - P(1,10)*PS9 - P(1,11)*PS34 + P(1,12)*PS6 + P(1,2) + PS110 + PS27;
|
||||
const float PS112 = P(2,3)*PS11;
|
||||
const float PS113 = -P(2,10)*PS9 - P(2,11)*PS34 + P(2,12)*PS6 + P(2,2) + PS112 + PS31 - PS90;
|
||||
const float PS114 = P(0,13)*PS12 - P(1,13)*PS13 - P(10,13)*PS9 - P(11,13)*PS34 + P(12,13)*PS6 + P(2,13) + P(3,13)*PS11;
|
||||
const float PS115 = P(0,15)*PS12 - P(1,15)*PS13 - P(10,15)*PS9 - P(11,15)*PS34 + P(12,15)*PS6 + P(2,15) + P(3,15)*PS11;
|
||||
const float PS116 = 2*PS115;
|
||||
const float PS117 = P(0,14)*PS12 - P(1,14)*PS13 - P(10,14)*PS9 - P(11,14)*PS34 + P(12,14)*PS6 + P(2,14) + P(3,14)*PS11;
|
||||
const float PS118 = 2*PS117;
|
||||
const float PS119 = P(0,4)*PS12 - P(1,4)*PS13 + P(2,4) + P(3,4)*PS11 - P(4,10)*PS9 - P(4,11)*PS34 + P(4,12)*PS6;
|
||||
const float PS120 = 2*PS114;
|
||||
const float PS121 = P(0,5)*PS12 - P(1,5)*PS13 + P(2,5) + P(3,5)*PS11 - P(5,10)*PS9 - P(5,11)*PS34 + P(5,12)*PS6;
|
||||
const float PS122 = P(0,6)*PS12 - P(1,6)*PS13 + P(2,6) + P(3,6)*PS11 - P(6,10)*PS9 - P(6,11)*PS34 + P(6,12)*PS6;
|
||||
const float PS123 = P(0,10)*PS13 + P(1,10)*PS12 + P(10,10)*PS7 - P(2,10)*PS11 + P(3,10) - PS15 + PS86;
|
||||
const float PS124 = P(1,1)*PS12 + P(1,10)*PS7 - P(1,11)*PS6 - P(1,12)*PS34 + P(1,3) + PS105 + PS24;
|
||||
const float PS125 = P(0,0)*PS13 + P(0,10)*PS7 - P(0,11)*PS6 - P(0,12)*PS34 + P(0,3) + PS110 - PS81;
|
||||
const float PS126 = P(0,12)*PS13 + P(1,12)*PS12 - P(12,12)*PS34 - P(2,12)*PS11 + P(3,12) - PS107 + PS84;
|
||||
const float PS127 = P(0,11)*PS13 + P(1,11)*PS12 - P(11,11)*PS6 - P(2,11)*PS11 + P(3,11) + PS102 + PS8;
|
||||
const float PS128 = P(2,10)*PS7 - P(2,11)*PS6 - P(2,12)*PS34 - P(2,2)*PS11 + P(2,3) + PS21 + PS78;
|
||||
const float PS129 = P(3,10)*PS7 - P(3,11)*PS6 - P(3,12)*PS34 + P(3,3) - PS112 + PS32 + PS91;
|
||||
const float PS130 = P(0,13)*PS13 + P(1,13)*PS12 + P(10,13)*PS7 - P(11,13)*PS6 - P(12,13)*PS34 - P(2,13)*PS11 + P(3,13);
|
||||
const float PS131 = P(0,15)*PS13 + P(1,15)*PS12 + P(10,15)*PS7 - P(11,15)*PS6 - P(12,15)*PS34 - P(2,15)*PS11 + P(3,15);
|
||||
const float PS132 = 2*PS131;
|
||||
const float PS133 = P(0,14)*PS13 + P(1,14)*PS12 + P(10,14)*PS7 - P(11,14)*PS6 - P(12,14)*PS34 - P(2,14)*PS11 + P(3,14);
|
||||
const float PS134 = 2*PS133;
|
||||
const float PS135 = P(0,4)*PS13 + P(1,4)*PS12 - P(2,4)*PS11 + P(3,4) + P(4,10)*PS7 - P(4,11)*PS6 - P(4,12)*PS34;
|
||||
const float PS136 = 2*PS130;
|
||||
const float PS137 = P(0,5)*PS13 + P(1,5)*PS12 - P(2,5)*PS11 + P(3,5) + P(5,10)*PS7 - P(5,11)*PS6 - P(5,12)*PS34;
|
||||
const float PS138 = P(0,6)*PS13 + P(1,6)*PS12 - P(2,6)*PS11 + P(3,6) + P(6,10)*PS7 - P(6,11)*PS6 - P(6,12)*PS34;
|
||||
const float PS139 = 2*PS46;
|
||||
const float PS140 = 2*PS54;
|
||||
const float PS141 = P(0,13)*PS72 + P(1,13)*PS62 - P(13,13)*PS44 + P(13,14)*PS140 - P(13,15)*PS139 + P(2,13)*PS60 - P(3,13)*PS74 + P(4,13);
|
||||
const float PS142 = P(0,15)*PS72 + P(1,15)*PS62 - P(13,15)*PS44 + P(14,15)*PS140 - P(15,15)*PS139 + P(2,15)*PS60 - P(3,15)*PS74 + P(4,15);
|
||||
const float PS143 = P(1,3)*PS62;
|
||||
const float PS144 = P(0,3)*PS72;
|
||||
const float PS145 = P(2,3)*PS60 - P(3,13)*PS44 + P(3,14)*PS140 - P(3,15)*PS139 - P(3,3)*PS74 + P(3,4) + PS143 + PS144;
|
||||
const float PS146 = P(0,14)*PS72 + P(1,14)*PS62 - P(13,14)*PS44 + P(14,14)*PS140 - P(14,15)*PS139 + P(2,14)*PS60 - P(3,14)*PS74 + P(4,14);
|
||||
const float PS147 = P(0,2)*PS60;
|
||||
const float PS148 = P(0,3)*PS74;
|
||||
const float PS149 = P(0,0)*PS72 + P(0,1)*PS62 - P(0,13)*PS44 + P(0,14)*PS140 - P(0,15)*PS139 + P(0,4) + PS147 - PS148;
|
||||
const float PS150 = P(1,2)*PS62;
|
||||
const float PS151 = P(0,2)*PS72;
|
||||
const float PS152 = -P(2,13)*PS44 + P(2,14)*PS140 - P(2,15)*PS139 + P(2,2)*PS60 - P(2,3)*PS74 + P(2,4) + PS150 + PS151;
|
||||
const float PS153 = P(1,2)*PS60;
|
||||
const float PS154 = P(1,3)*PS74;
|
||||
const float PS155 = P(0,1)*PS72 + P(1,1)*PS62 - P(1,13)*PS44 + P(1,14)*PS140 - P(1,15)*PS139 + P(1,4) + PS153 - PS154;
|
||||
const float PS156 = 4*dvyVar;
|
||||
const float PS157 = 4*dvzVar;
|
||||
const float PS158 = P(0,4)*PS72 + P(1,4)*PS62 + P(2,4)*PS60 - P(3,4)*PS74 - P(4,13)*PS44 + P(4,14)*PS140 - P(4,15)*PS139 + P(4,4);
|
||||
const float PS159 = 2*PS141;
|
||||
const float PS160 = 2*PS68;
|
||||
const float PS161 = PS65*dvyVar;
|
||||
const float PS162 = 2*PS66;
|
||||
const float PS163 = PS44*dvxVar;
|
||||
const float PS164 = P(0,5)*PS72 + P(1,5)*PS62 + P(2,5)*PS60 - P(3,5)*PS74 + P(4,5) - P(5,13)*PS44 + P(5,14)*PS140 - P(5,15)*PS139;
|
||||
const float PS165 = 2*PS71;
|
||||
const float PS166 = 2*PS73;
|
||||
const float PS167 = PS70*dvzVar;
|
||||
const float PS168 = P(0,6)*PS72 + P(1,6)*PS62 + P(2,6)*PS60 - P(3,6)*PS74 + P(4,6) - P(6,13)*PS44 + P(6,14)*PS140 - P(6,15)*PS139;
|
||||
const float PS169 = P(0,14)*PS74 - P(1,14)*PS60 - P(13,14)*PS162 - P(14,14)*PS65 + P(14,15)*PS160 + P(2,14)*PS62 + P(3,14)*PS72 + P(5,14);
|
||||
const float PS170 = P(0,13)*PS74 - P(1,13)*PS60 - P(13,13)*PS162 - P(13,14)*PS65 + P(13,15)*PS160 + P(2,13)*PS62 + P(3,13)*PS72 + P(5,13);
|
||||
const float PS171 = P(0,1)*PS74;
|
||||
const float PS172 = -P(1,1)*PS60 - P(1,13)*PS162 - P(1,14)*PS65 + P(1,15)*PS160 + P(1,3)*PS72 + P(1,5) + PS150 + PS171;
|
||||
const float PS173 = P(0,15)*PS74 - P(1,15)*PS60 - P(13,15)*PS162 - P(14,15)*PS65 + P(15,15)*PS160 + P(2,15)*PS62 + P(3,15)*PS72 + P(5,15);
|
||||
const float PS174 = P(2,3)*PS62;
|
||||
const float PS175 = -P(1,3)*PS60 - P(3,13)*PS162 - P(3,14)*PS65 + P(3,15)*PS160 + P(3,3)*PS72 + P(3,5) + PS148 + PS174;
|
||||
const float PS176 = P(0,1)*PS60;
|
||||
const float PS177 = P(0,0)*PS74 - P(0,13)*PS162 - P(0,14)*PS65 + P(0,15)*PS160 + P(0,2)*PS62 + P(0,5) + PS144 - PS176;
|
||||
const float PS178 = P(2,3)*PS72;
|
||||
const float PS179 = P(0,2)*PS74 - P(2,13)*PS162 - P(2,14)*PS65 + P(2,15)*PS160 + P(2,2)*PS62 + P(2,5) - PS153 + PS178;
|
||||
const float PS180 = 4*dvxVar;
|
||||
const float PS181 = P(0,5)*PS74 - P(1,5)*PS60 + P(2,5)*PS62 + P(3,5)*PS72 - P(5,13)*PS162 - P(5,14)*PS65 + P(5,15)*PS160 + P(5,5);
|
||||
const float PS182 = P(0,6)*PS74 - P(1,6)*PS60 + P(2,6)*PS62 + P(3,6)*PS72 + P(5,6) - P(6,13)*PS162 - P(6,14)*PS65 + P(6,15)*PS160;
|
||||
const float PS183 = P(0,15)*PS60 + P(1,15)*PS74 + P(13,15)*PS166 - P(14,15)*PS165 - P(15,15)*PS70 - P(2,15)*PS72 + P(3,15)*PS62 + P(6,15);
|
||||
const float PS184 = P(0,14)*PS60 + P(1,14)*PS74 + P(13,14)*PS166 - P(14,14)*PS165 - P(14,15)*PS70 - P(2,14)*PS72 + P(3,14)*PS62 + P(6,14);
|
||||
const float PS185 = P(0,13)*PS60 + P(1,13)*PS74 + P(13,13)*PS166 - P(13,14)*PS165 - P(13,15)*PS70 - P(2,13)*PS72 + P(3,13)*PS62 + P(6,13);
|
||||
const float PS186 = P(0,6)*PS60 + P(1,6)*PS74 - P(2,6)*PS72 + P(3,6)*PS62 + P(6,13)*PS166 - P(6,14)*PS165 - P(6,15)*PS70 + P(6,6);
|
||||
|
||||
SquareMatrix24f nextP;
|
||||
|
||||
nextP(0,0) = PS0*PS1 - PS11*PS23 - PS12*PS26 - PS13*PS29 + PS14*PS6 + PS17*PS7 + PS2*PS3 + PS20*PS9 + PS33 + PS4*PS5;
|
||||
nextP(0,1) = -PS1*PS36 + PS11*PS33 - PS12*PS29 + PS13*PS26 - PS14*PS34 + PS17*PS9 - PS20*PS7 + PS23 + PS3*PS35 - PS35*PS5;
|
||||
nextP(1,1) = PS1*PS42 + PS11*PS80 - PS12*PS89 + PS13*PS82 + PS2*PS5 + PS3*PS4 - PS34*PS85 - PS7*PS87 + PS77*PS9 + PS92;
|
||||
nextP(0,2) = -PS1*PS37 + PS11*PS29 + PS12*PS33 - PS13*PS23 - PS14*PS9 - PS17*PS34 + PS20*PS6 + PS26 - PS3*PS38 + PS37*PS5;
|
||||
nextP(1,2) = PS1*PS40 + PS11*PS89 + PS12*PS80 - PS13*PS92 - PS3*PS40 - PS34*PS77 - PS39*PS5 + PS6*PS87 + PS82 - PS85*PS9;
|
||||
nextP(2,2) = PS0*PS5 + PS1*PS4 + PS103*PS6 + PS104*PS11 + PS106*PS12 - PS108*PS34 - PS109*PS9 - PS111*PS13 + PS113 + PS3*PS42;
|
||||
nextP(0,3) = PS1*PS39 - PS11*PS26 + PS12*PS23 + PS13*PS33 + PS14*PS7 - PS17*PS6 - PS20*PS34 + PS29 - PS3*PS39 - PS40*PS5;
|
||||
nextP(1,3) = -PS1*PS38 - PS11*PS82 + PS12*PS92 + PS13*PS80 - PS3*PS37 - PS34*PS87 + PS38*PS5 - PS6*PS77 + PS7*PS85 + PS89;
|
||||
nextP(2,3) = -PS1*PS35 - PS103*PS34 + PS104 + PS106*PS13 - PS108*PS6 + PS109*PS7 - PS11*PS113 + PS111*PS12 + PS3*PS36 - PS36*PS5;
|
||||
nextP(3,3) = PS0*PS3 + PS1*PS2 - PS11*PS128 + PS12*PS124 + PS123*PS7 + PS125*PS13 - PS126*PS34 - PS127*PS6 + PS129 + PS42*PS5;
|
||||
nextP(0,4) = PS23*PS62 + PS26*PS60 - PS44*PS45 - PS46*PS48 - PS52*PS53 + PS54*PS56 + PS57*PS58 + PS63;
|
||||
nextP(1,4) = -PS44*PS93 - PS46*PS95 + PS54*PS97 + PS60*PS82 + PS62*PS92 + PS72*PS80 - PS74*PS89 + PS98;
|
||||
nextP(2,4) = -PS104*PS74 + PS106*PS72 + PS111*PS62 + PS113*PS60 - PS114*PS44 - PS116*PS46 + PS118*PS54 + PS119;
|
||||
nextP(3,4) = PS124*PS62 + PS125*PS72 + PS128*PS60 - PS129*PS74 - PS130*PS44 - PS132*PS46 + PS134*PS54 + PS135;
|
||||
nextP(4,4) = -PS139*PS142 + PS140*PS146 - PS141*PS44 - PS145*PS74 + PS149*PS72 + PS152*PS60 + PS155*PS62 + PS156*ecl::powf(PS54, 2) + PS157*ecl::powf(PS46, 2) + PS158 + ecl::powf(PS44, 2)*dvxVar;
|
||||
nextP(0,5) = -PS23*PS60 + PS26*PS62 + PS48*PS68 + PS52*PS58 + PS53*PS57 - PS55*PS65 - PS66*PS67 + PS69;
|
||||
nextP(1,5) = PS100 - PS60*PS92 + PS62*PS82 - PS65*PS96 - PS66*PS99 + PS68*PS95 + PS72*PS89 + PS74*PS80;
|
||||
nextP(2,5) = PS104*PS72 + PS106*PS74 - PS111*PS60 + PS113*PS62 + PS116*PS68 - PS117*PS65 - PS120*PS66 + PS121;
|
||||
nextP(3,5) = -PS124*PS60 + PS125*PS74 + PS128*PS62 + PS129*PS72 + PS132*PS68 - PS133*PS65 - PS136*PS66 + PS137;
|
||||
nextP(4,5) = -PS140*PS161 + PS142*PS160 + PS145*PS72 - PS146*PS65 + PS149*PS74 + PS152*PS62 - PS155*PS60 - PS157*PS46*PS68 - PS159*PS66 + PS162*PS163 + PS164;
|
||||
nextP(5,5) = PS157*ecl::powf(PS68, 2) + PS160*PS173 - PS162*PS170 - PS169*PS65 - PS172*PS60 + PS175*PS72 + PS177*PS74 + PS179*PS62 + PS180*ecl::powf(PS66, 2) + PS181 + ecl::powf(PS65, 2)*dvyVar;
|
||||
nextP(0,6) = PS23*PS74 - PS26*PS72 - PS47*PS70 + PS53*PS61 - PS56*PS71 + PS58*PS59 + PS67*PS73 + PS75;
|
||||
nextP(1,6) = PS101 + PS60*PS80 + PS62*PS89 - PS70*PS94 - PS71*PS97 - PS72*PS82 + PS73*PS99 + PS74*PS92;
|
||||
nextP(2,6) = PS104*PS62 + PS106*PS60 + PS111*PS74 - PS113*PS72 - PS115*PS70 - PS118*PS71 + PS120*PS73 + PS122;
|
||||
nextP(3,6) = PS124*PS74 + PS125*PS60 - PS128*PS72 + PS129*PS62 - PS131*PS70 - PS134*PS71 + PS136*PS73 + PS138;
|
||||
nextP(4,6) = PS139*PS167 - PS142*PS70 + PS145*PS62 - PS146*PS165 + PS149*PS60 - PS152*PS72 + PS155*PS74 - PS156*PS54*PS71 + PS159*PS73 - PS163*PS166 + PS168;
|
||||
nextP(5,6) = -PS160*PS167 + PS161*PS165 - PS165*PS169 + PS166*PS170 + PS172*PS74 - PS173*PS70 + PS175*PS62 + PS177*PS60 - PS179*PS72 - PS180*PS66*PS73 + PS182;
|
||||
nextP(6,6) = PS156*ecl::powf(PS71, 2) - PS165*PS184 + PS166*PS185 + PS180*ecl::powf(PS73, 2) - PS183*PS70 + PS186 + PS60*(P(0,0)*PS60 + P(0,13)*PS166 - P(0,14)*PS165 - P(0,15)*PS70 + P(0,3)*PS62 + P(0,6) - PS151 + PS171) + PS62*(P(0,3)*PS60 + P(3,13)*PS166 - P(3,14)*PS165 - P(3,15)*PS70 + P(3,3)*PS62 + P(3,6) + PS154 - PS178) + ecl::powf(PS70, 2)*dvzVar - PS72*(P(1,2)*PS74 + P(2,13)*PS166 - P(2,14)*PS165 - P(2,15)*PS70 - P(2,2)*PS72 + P(2,6) + PS147 + PS174) + PS74*(P(1,1)*PS74 + P(1,13)*PS166 - P(1,14)*PS165 - P(1,15)*PS70 - P(1,2)*PS72 + P(1,6) + PS143 + PS176);
|
||||
nextP(0,7) = P(0,7) - P(1,7)*PS11 - P(2,7)*PS12 - P(3,7)*PS13 + P(7,10)*PS6 + P(7,11)*PS7 + P(7,12)*PS9 + PS63*dt;
|
||||
nextP(1,7) = P(0,7)*PS11 + P(1,7) + P(2,7)*PS13 - P(3,7)*PS12 - P(7,10)*PS34 + P(7,11)*PS9 - P(7,12)*PS7 + PS98*dt;
|
||||
nextP(2,7) = P(0,7)*PS12 - P(1,7)*PS13 + P(2,7) + P(3,7)*PS11 - P(7,10)*PS9 - P(7,11)*PS34 + P(7,12)*PS6 + PS119*dt;
|
||||
nextP(3,7) = P(0,7)*PS13 + P(1,7)*PS12 - P(2,7)*PS11 + P(3,7) + P(7,10)*PS7 - P(7,11)*PS6 - P(7,12)*PS34 + PS135*dt;
|
||||
nextP(4,7) = P(0,7)*PS72 + P(1,7)*PS62 + P(2,7)*PS60 - P(3,7)*PS74 + P(4,7) - P(7,13)*PS44 + P(7,14)*PS140 - P(7,15)*PS139 + PS158*dt;
|
||||
nextP(5,7) = P(0,7)*PS74 - P(1,7)*PS60 + P(2,7)*PS62 + P(3,7)*PS72 + P(5,7) - P(7,13)*PS162 - P(7,14)*PS65 + P(7,15)*PS160 + dt*(P(0,4)*PS74 - P(1,4)*PS60 + P(2,4)*PS62 + P(3,4)*PS72 - P(4,13)*PS162 - P(4,14)*PS65 + P(4,15)*PS160 + P(4,5));
|
||||
nextP(6,7) = P(0,7)*PS60 + P(1,7)*PS74 - P(2,7)*PS72 + P(3,7)*PS62 + P(6,7) + P(7,13)*PS166 - P(7,14)*PS165 - P(7,15)*PS70 + dt*(P(0,4)*PS60 + P(1,4)*PS74 - P(2,4)*PS72 + P(3,4)*PS62 + P(4,13)*PS166 - P(4,14)*PS165 - P(4,15)*PS70 + P(4,6));
|
||||
nextP(7,7) = P(4,7)*dt + P(7,7) + dt*(P(4,4)*dt + P(4,7));
|
||||
nextP(0,8) = P(0,8) - P(1,8)*PS11 - P(2,8)*PS12 - P(3,8)*PS13 + P(8,10)*PS6 + P(8,11)*PS7 + P(8,12)*PS9 + PS69*dt;
|
||||
nextP(1,8) = P(0,8)*PS11 + P(1,8) + P(2,8)*PS13 - P(3,8)*PS12 - P(8,10)*PS34 + P(8,11)*PS9 - P(8,12)*PS7 + PS100*dt;
|
||||
nextP(2,8) = P(0,8)*PS12 - P(1,8)*PS13 + P(2,8) + P(3,8)*PS11 - P(8,10)*PS9 - P(8,11)*PS34 + P(8,12)*PS6 + PS121*dt;
|
||||
nextP(3,8) = P(0,8)*PS13 + P(1,8)*PS12 - P(2,8)*PS11 + P(3,8) + P(8,10)*PS7 - P(8,11)*PS6 - P(8,12)*PS34 + PS137*dt;
|
||||
nextP(4,8) = P(0,8)*PS72 + P(1,8)*PS62 + P(2,8)*PS60 - P(3,8)*PS74 + P(4,8) - P(8,13)*PS44 + P(8,14)*PS140 - P(8,15)*PS139 + PS164*dt;
|
||||
nextP(5,8) = P(0,8)*PS74 - P(1,8)*PS60 + P(2,8)*PS62 + P(3,8)*PS72 + P(5,8) - P(8,13)*PS162 - P(8,14)*PS65 + P(8,15)*PS160 + PS181*dt;
|
||||
nextP(6,8) = P(0,8)*PS60 + P(1,8)*PS74 - P(2,8)*PS72 + P(3,8)*PS62 + P(6,8) + P(8,13)*PS166 - P(8,14)*PS165 - P(8,15)*PS70 + dt*(P(0,5)*PS60 + P(1,5)*PS74 - P(2,5)*PS72 + P(3,5)*PS62 + P(5,13)*PS166 - P(5,14)*PS165 - P(5,15)*PS70 + P(5,6));
|
||||
nextP(7,8) = P(4,8)*dt + P(7,8) + dt*(P(4,5)*dt + P(5,7));
|
||||
nextP(8,8) = P(5,8)*dt + P(8,8) + dt*(P(5,5)*dt + P(5,8));
|
||||
nextP(0,9) = P(0,9) - P(1,9)*PS11 - P(2,9)*PS12 - P(3,9)*PS13 + P(9,10)*PS6 + P(9,11)*PS7 + P(9,12)*PS9 + PS75*dt;
|
||||
nextP(1,9) = P(0,9)*PS11 + P(1,9) + P(2,9)*PS13 - P(3,9)*PS12 - P(9,10)*PS34 + P(9,11)*PS9 - P(9,12)*PS7 + PS101*dt;
|
||||
nextP(2,9) = P(0,9)*PS12 - P(1,9)*PS13 + P(2,9) + P(3,9)*PS11 - P(9,10)*PS9 - P(9,11)*PS34 + P(9,12)*PS6 + PS122*dt;
|
||||
nextP(3,9) = P(0,9)*PS13 + P(1,9)*PS12 - P(2,9)*PS11 + P(3,9) + P(9,10)*PS7 - P(9,11)*PS6 - P(9,12)*PS34 + PS138*dt;
|
||||
nextP(4,9) = P(0,9)*PS72 + P(1,9)*PS62 + P(2,9)*PS60 - P(3,9)*PS74 + P(4,9) - P(9,13)*PS44 + P(9,14)*PS140 - P(9,15)*PS139 + PS168*dt;
|
||||
nextP(5,9) = P(0,9)*PS74 - P(1,9)*PS60 + P(2,9)*PS62 + P(3,9)*PS72 + P(5,9) - P(9,13)*PS162 - P(9,14)*PS65 + P(9,15)*PS160 + PS182*dt;
|
||||
nextP(6,9) = P(0,9)*PS60 + P(1,9)*PS74 - P(2,9)*PS72 + P(3,9)*PS62 + P(6,9) + P(9,13)*PS166 - P(9,14)*PS165 - P(9,15)*PS70 + PS186*dt;
|
||||
nextP(7,9) = P(4,9)*dt + P(7,9) + dt*(P(4,6)*dt + P(6,7));
|
||||
nextP(8,9) = P(5,9)*dt + P(8,9) + dt*(P(5,6)*dt + P(6,8));
|
||||
nextP(9,9) = P(6,9)*dt + P(9,9) + dt*(P(6,6)*dt + P(6,9));
|
||||
nextP(0,10) = PS14;
|
||||
nextP(1,10) = PS85;
|
||||
nextP(2,10) = PS109;
|
||||
nextP(3,10) = PS123;
|
||||
nextP(4,10) = P(0,10)*PS72 + P(1,10)*PS62 - P(10,13)*PS44 + P(10,14)*PS140 - P(10,15)*PS139 + P(2,10)*PS60 - P(3,10)*PS74 + P(4,10);
|
||||
nextP(5,10) = P(0,10)*PS74 - P(1,10)*PS60 - P(10,13)*PS162 - P(10,14)*PS65 + P(10,15)*PS160 + P(2,10)*PS62 + P(3,10)*PS72 + P(5,10);
|
||||
nextP(6,10) = P(0,10)*PS60 + P(1,10)*PS74 + P(10,13)*PS166 - P(10,14)*PS165 - P(10,15)*PS70 - P(2,10)*PS72 + P(3,10)*PS62 + P(6,10);
|
||||
nextP(7,10) = P(4,10)*dt + P(7,10);
|
||||
nextP(8,10) = P(5,10)*dt + P(8,10);
|
||||
nextP(9,10) = P(6,10)*dt + P(9,10);
|
||||
nextP(10,10) = P(10,10);
|
||||
nextP(0,11) = PS17;
|
||||
nextP(1,11) = PS77;
|
||||
nextP(2,11) = PS108;
|
||||
nextP(3,11) = PS127;
|
||||
nextP(4,11) = P(0,11)*PS72 + P(1,11)*PS62 - P(11,13)*PS44 + P(11,14)*PS140 - P(11,15)*PS139 + P(2,11)*PS60 - P(3,11)*PS74 + P(4,11);
|
||||
nextP(5,11) = P(0,11)*PS74 - P(1,11)*PS60 - P(11,13)*PS162 - P(11,14)*PS65 + P(11,15)*PS160 + P(2,11)*PS62 + P(3,11)*PS72 + P(5,11);
|
||||
nextP(6,11) = P(0,11)*PS60 + P(1,11)*PS74 + P(11,13)*PS166 - P(11,14)*PS165 - P(11,15)*PS70 - P(2,11)*PS72 + P(3,11)*PS62 + P(6,11);
|
||||
nextP(7,11) = P(4,11)*dt + P(7,11);
|
||||
nextP(8,11) = P(5,11)*dt + P(8,11);
|
||||
nextP(9,11) = P(6,11)*dt + P(9,11);
|
||||
nextP(10,11) = P(10,11);
|
||||
nextP(11,11) = P(11,11);
|
||||
nextP(0,12) = PS20;
|
||||
nextP(1,12) = PS87;
|
||||
nextP(2,12) = PS103;
|
||||
nextP(3,12) = PS126;
|
||||
nextP(4,12) = P(0,12)*PS72 + P(1,12)*PS62 - P(12,13)*PS44 + P(12,14)*PS140 - P(12,15)*PS139 + P(2,12)*PS60 - P(3,12)*PS74 + P(4,12);
|
||||
nextP(5,12) = P(0,12)*PS74 - P(1,12)*PS60 - P(12,13)*PS162 - P(12,14)*PS65 + P(12,15)*PS160 + P(2,12)*PS62 + P(3,12)*PS72 + P(5,12);
|
||||
nextP(6,12) = P(0,12)*PS60 + P(1,12)*PS74 + P(12,13)*PS166 - P(12,14)*PS165 - P(12,15)*PS70 - P(2,12)*PS72 + P(3,12)*PS62 + P(6,12);
|
||||
nextP(7,12) = P(4,12)*dt + P(7,12);
|
||||
nextP(8,12) = P(5,12)*dt + P(8,12);
|
||||
nextP(9,12) = P(6,12)*dt + P(9,12);
|
||||
nextP(10,12) = P(10,12);
|
||||
nextP(11,12) = P(11,12);
|
||||
nextP(12,12) = P(12,12);
|
||||
nextP(0,13) = PS45;
|
||||
nextP(1,13) = PS93;
|
||||
nextP(2,13) = PS114;
|
||||
nextP(3,13) = PS130;
|
||||
nextP(4,13) = PS141;
|
||||
nextP(5,13) = PS170;
|
||||
nextP(6,13) = PS185;
|
||||
nextP(7,13) = P(4,13)*dt + P(7,13);
|
||||
nextP(8,13) = P(5,13)*dt + P(8,13);
|
||||
nextP(9,13) = P(6,13)*dt + P(9,13);
|
||||
nextP(10,13) = P(10,13);
|
||||
nextP(11,13) = P(11,13);
|
||||
nextP(12,13) = P(12,13);
|
||||
nextP(13,13) = P(13,13);
|
||||
nextP(0,14) = PS55;
|
||||
nextP(1,14) = PS96;
|
||||
nextP(2,14) = PS117;
|
||||
nextP(3,14) = PS133;
|
||||
nextP(4,14) = PS146;
|
||||
nextP(5,14) = PS169;
|
||||
nextP(6,14) = PS184;
|
||||
nextP(7,14) = P(4,14)*dt + P(7,14);
|
||||
nextP(8,14) = P(5,14)*dt + P(8,14);
|
||||
nextP(9,14) = P(6,14)*dt + P(9,14);
|
||||
nextP(10,14) = P(10,14);
|
||||
nextP(11,14) = P(11,14);
|
||||
nextP(12,14) = P(12,14);
|
||||
nextP(13,14) = P(13,14);
|
||||
nextP(14,14) = P(14,14);
|
||||
nextP(0,15) = PS47;
|
||||
nextP(1,15) = PS94;
|
||||
nextP(2,15) = PS115;
|
||||
nextP(3,15) = PS131;
|
||||
nextP(4,15) = PS142;
|
||||
nextP(5,15) = PS173;
|
||||
nextP(6,15) = PS183;
|
||||
nextP(7,15) = P(4,15)*dt + P(7,15);
|
||||
nextP(8,15) = P(5,15)*dt + P(8,15);
|
||||
nextP(9,15) = P(6,15)*dt + P(9,15);
|
||||
nextP(10,15) = P(10,15);
|
||||
nextP(11,15) = P(11,15);
|
||||
nextP(12,15) = P(12,15);
|
||||
nextP(13,15) = P(13,15);
|
||||
nextP(14,15) = P(14,15);
|
||||
nextP(15,15) = P(15,15);
|
||||
nextP(0,16) = P(0,16) - P(1,16)*PS11 + P(10,16)*PS6 + P(11,16)*PS7 + P(12,16)*PS9 - P(2,16)*PS12 - P(3,16)*PS13;
|
||||
nextP(1,16) = P(0,16)*PS11 + P(1,16) - P(10,16)*PS34 + P(11,16)*PS9 - P(12,16)*PS7 + P(2,16)*PS13 - P(3,16)*PS12;
|
||||
nextP(2,16) = P(0,16)*PS12 - P(1,16)*PS13 - P(10,16)*PS9 - P(11,16)*PS34 + P(12,16)*PS6 + P(2,16) + P(3,16)*PS11;
|
||||
nextP(3,16) = P(0,16)*PS13 + P(1,16)*PS12 + P(10,16)*PS7 - P(11,16)*PS6 - P(12,16)*PS34 - P(2,16)*PS11 + P(3,16);
|
||||
nextP(4,16) = P(0,16)*PS72 + P(1,16)*PS62 - P(13,16)*PS44 + P(14,16)*PS140 - P(15,16)*PS139 + P(2,16)*PS60 - P(3,16)*PS74 + P(4,16);
|
||||
nextP(5,16) = P(0,16)*PS74 - P(1,16)*PS60 - P(13,16)*PS162 - P(14,16)*PS65 + P(15,16)*PS160 + P(2,16)*PS62 + P(3,16)*PS72 + P(5,16);
|
||||
nextP(6,16) = P(0,16)*PS60 + P(1,16)*PS74 + P(13,16)*PS166 - P(14,16)*PS165 - P(15,16)*PS70 - P(2,16)*PS72 + P(3,16)*PS62 + P(6,16);
|
||||
nextP(7,16) = P(4,16)*dt + P(7,16);
|
||||
nextP(8,16) = P(5,16)*dt + P(8,16);
|
||||
nextP(9,16) = P(6,16)*dt + P(9,16);
|
||||
nextP(10,16) = P(10,16);
|
||||
nextP(11,16) = P(11,16);
|
||||
nextP(12,16) = P(12,16);
|
||||
nextP(13,16) = P(13,16);
|
||||
nextP(14,16) = P(14,16);
|
||||
nextP(15,16) = P(15,16);
|
||||
nextP(16,16) = P(16,16);
|
||||
nextP(0,17) = P(0,17) - P(1,17)*PS11 + P(10,17)*PS6 + P(11,17)*PS7 + P(12,17)*PS9 - P(2,17)*PS12 - P(3,17)*PS13;
|
||||
nextP(1,17) = P(0,17)*PS11 + P(1,17) - P(10,17)*PS34 + P(11,17)*PS9 - P(12,17)*PS7 + P(2,17)*PS13 - P(3,17)*PS12;
|
||||
nextP(2,17) = P(0,17)*PS12 - P(1,17)*PS13 - P(10,17)*PS9 - P(11,17)*PS34 + P(12,17)*PS6 + P(2,17) + P(3,17)*PS11;
|
||||
nextP(3,17) = P(0,17)*PS13 + P(1,17)*PS12 + P(10,17)*PS7 - P(11,17)*PS6 - P(12,17)*PS34 - P(2,17)*PS11 + P(3,17);
|
||||
nextP(4,17) = P(0,17)*PS72 + P(1,17)*PS62 - P(13,17)*PS44 + P(14,17)*PS140 - P(15,17)*PS139 + P(2,17)*PS60 - P(3,17)*PS74 + P(4,17);
|
||||
nextP(5,17) = P(0,17)*PS74 - P(1,17)*PS60 - P(13,17)*PS162 - P(14,17)*PS65 + P(15,17)*PS160 + P(2,17)*PS62 + P(3,17)*PS72 + P(5,17);
|
||||
nextP(6,17) = P(0,17)*PS60 + P(1,17)*PS74 + P(13,17)*PS166 - P(14,17)*PS165 - P(15,17)*PS70 - P(2,17)*PS72 + P(3,17)*PS62 + P(6,17);
|
||||
nextP(7,17) = P(4,17)*dt + P(7,17);
|
||||
nextP(8,17) = P(5,17)*dt + P(8,17);
|
||||
nextP(9,17) = P(6,17)*dt + P(9,17);
|
||||
nextP(10,17) = P(10,17);
|
||||
nextP(11,17) = P(11,17);
|
||||
nextP(12,17) = P(12,17);
|
||||
nextP(13,17) = P(13,17);
|
||||
nextP(14,17) = P(14,17);
|
||||
nextP(15,17) = P(15,17);
|
||||
nextP(16,17) = P(16,17);
|
||||
nextP(17,17) = P(17,17);
|
||||
nextP(0,18) = P(0,18) - P(1,18)*PS11 + P(10,18)*PS6 + P(11,18)*PS7 + P(12,18)*PS9 - P(2,18)*PS12 - P(3,18)*PS13;
|
||||
nextP(1,18) = P(0,18)*PS11 + P(1,18) - P(10,18)*PS34 + P(11,18)*PS9 - P(12,18)*PS7 + P(2,18)*PS13 - P(3,18)*PS12;
|
||||
nextP(2,18) = P(0,18)*PS12 - P(1,18)*PS13 - P(10,18)*PS9 - P(11,18)*PS34 + P(12,18)*PS6 + P(2,18) + P(3,18)*PS11;
|
||||
nextP(3,18) = P(0,18)*PS13 + P(1,18)*PS12 + P(10,18)*PS7 - P(11,18)*PS6 - P(12,18)*PS34 - P(2,18)*PS11 + P(3,18);
|
||||
nextP(4,18) = P(0,18)*PS72 + P(1,18)*PS62 - P(13,18)*PS44 + P(14,18)*PS140 - P(15,18)*PS139 + P(2,18)*PS60 - P(3,18)*PS74 + P(4,18);
|
||||
nextP(5,18) = P(0,18)*PS74 - P(1,18)*PS60 - P(13,18)*PS162 - P(14,18)*PS65 + P(15,18)*PS160 + P(2,18)*PS62 + P(3,18)*PS72 + P(5,18);
|
||||
nextP(6,18) = P(0,18)*PS60 + P(1,18)*PS74 + P(13,18)*PS166 - P(14,18)*PS165 - P(15,18)*PS70 - P(2,18)*PS72 + P(3,18)*PS62 + P(6,18);
|
||||
nextP(7,18) = P(4,18)*dt + P(7,18);
|
||||
nextP(8,18) = P(5,18)*dt + P(8,18);
|
||||
nextP(9,18) = P(6,18)*dt + P(9,18);
|
||||
nextP(10,18) = P(10,18);
|
||||
nextP(11,18) = P(11,18);
|
||||
nextP(12,18) = P(12,18);
|
||||
nextP(13,18) = P(13,18);
|
||||
nextP(14,18) = P(14,18);
|
||||
nextP(15,18) = P(15,18);
|
||||
nextP(16,18) = P(16,18);
|
||||
nextP(17,18) = P(17,18);
|
||||
nextP(18,18) = P(18,18);
|
||||
nextP(0,19) = P(0,19) - P(1,19)*PS11 + P(10,19)*PS6 + P(11,19)*PS7 + P(12,19)*PS9 - P(2,19)*PS12 - P(3,19)*PS13;
|
||||
nextP(1,19) = P(0,19)*PS11 + P(1,19) - P(10,19)*PS34 + P(11,19)*PS9 - P(12,19)*PS7 + P(2,19)*PS13 - P(3,19)*PS12;
|
||||
nextP(2,19) = P(0,19)*PS12 - P(1,19)*PS13 - P(10,19)*PS9 - P(11,19)*PS34 + P(12,19)*PS6 + P(2,19) + P(3,19)*PS11;
|
||||
nextP(3,19) = P(0,19)*PS13 + P(1,19)*PS12 + P(10,19)*PS7 - P(11,19)*PS6 - P(12,19)*PS34 - P(2,19)*PS11 + P(3,19);
|
||||
nextP(4,19) = P(0,19)*PS72 + P(1,19)*PS62 - P(13,19)*PS44 + P(14,19)*PS140 - P(15,19)*PS139 + P(2,19)*PS60 - P(3,19)*PS74 + P(4,19);
|
||||
nextP(5,19) = P(0,19)*PS74 - P(1,19)*PS60 - P(13,19)*PS162 - P(14,19)*PS65 + P(15,19)*PS160 + P(2,19)*PS62 + P(3,19)*PS72 + P(5,19);
|
||||
nextP(6,19) = P(0,19)*PS60 + P(1,19)*PS74 + P(13,19)*PS166 - P(14,19)*PS165 - P(15,19)*PS70 - P(2,19)*PS72 + P(3,19)*PS62 + P(6,19);
|
||||
nextP(7,19) = P(4,19)*dt + P(7,19);
|
||||
nextP(8,19) = P(5,19)*dt + P(8,19);
|
||||
nextP(9,19) = P(6,19)*dt + P(9,19);
|
||||
nextP(10,19) = P(10,19);
|
||||
nextP(11,19) = P(11,19);
|
||||
nextP(12,19) = P(12,19);
|
||||
nextP(13,19) = P(13,19);
|
||||
nextP(14,19) = P(14,19);
|
||||
nextP(15,19) = P(15,19);
|
||||
nextP(16,19) = P(16,19);
|
||||
nextP(17,19) = P(17,19);
|
||||
nextP(18,19) = P(18,19);
|
||||
nextP(19,19) = P(19,19);
|
||||
nextP(0,20) = P(0,20) - P(1,20)*PS11 + P(10,20)*PS6 + P(11,20)*PS7 + P(12,20)*PS9 - P(2,20)*PS12 - P(3,20)*PS13;
|
||||
nextP(1,20) = P(0,20)*PS11 + P(1,20) - P(10,20)*PS34 + P(11,20)*PS9 - P(12,20)*PS7 + P(2,20)*PS13 - P(3,20)*PS12;
|
||||
nextP(2,20) = P(0,20)*PS12 - P(1,20)*PS13 - P(10,20)*PS9 - P(11,20)*PS34 + P(12,20)*PS6 + P(2,20) + P(3,20)*PS11;
|
||||
nextP(3,20) = P(0,20)*PS13 + P(1,20)*PS12 + P(10,20)*PS7 - P(11,20)*PS6 - P(12,20)*PS34 - P(2,20)*PS11 + P(3,20);
|
||||
nextP(4,20) = P(0,20)*PS72 + P(1,20)*PS62 - P(13,20)*PS44 + P(14,20)*PS140 - P(15,20)*PS139 + P(2,20)*PS60 - P(3,20)*PS74 + P(4,20);
|
||||
nextP(5,20) = P(0,20)*PS74 - P(1,20)*PS60 - P(13,20)*PS162 - P(14,20)*PS65 + P(15,20)*PS160 + P(2,20)*PS62 + P(3,20)*PS72 + P(5,20);
|
||||
nextP(6,20) = P(0,20)*PS60 + P(1,20)*PS74 + P(13,20)*PS166 - P(14,20)*PS165 - P(15,20)*PS70 - P(2,20)*PS72 + P(3,20)*PS62 + P(6,20);
|
||||
nextP(7,20) = P(4,20)*dt + P(7,20);
|
||||
nextP(8,20) = P(5,20)*dt + P(8,20);
|
||||
nextP(9,20) = P(6,20)*dt + P(9,20);
|
||||
nextP(10,20) = P(10,20);
|
||||
nextP(11,20) = P(11,20);
|
||||
nextP(12,20) = P(12,20);
|
||||
nextP(13,20) = P(13,20);
|
||||
nextP(14,20) = P(14,20);
|
||||
nextP(15,20) = P(15,20);
|
||||
nextP(16,20) = P(16,20);
|
||||
nextP(17,20) = P(17,20);
|
||||
nextP(18,20) = P(18,20);
|
||||
nextP(19,20) = P(19,20);
|
||||
nextP(20,20) = P(20,20);
|
||||
nextP(0,21) = P(0,21) - P(1,21)*PS11 + P(10,21)*PS6 + P(11,21)*PS7 + P(12,21)*PS9 - P(2,21)*PS12 - P(3,21)*PS13;
|
||||
nextP(1,21) = P(0,21)*PS11 + P(1,21) - P(10,21)*PS34 + P(11,21)*PS9 - P(12,21)*PS7 + P(2,21)*PS13 - P(3,21)*PS12;
|
||||
nextP(2,21) = P(0,21)*PS12 - P(1,21)*PS13 - P(10,21)*PS9 - P(11,21)*PS34 + P(12,21)*PS6 + P(2,21) + P(3,21)*PS11;
|
||||
nextP(3,21) = P(0,21)*PS13 + P(1,21)*PS12 + P(10,21)*PS7 - P(11,21)*PS6 - P(12,21)*PS34 - P(2,21)*PS11 + P(3,21);
|
||||
nextP(4,21) = P(0,21)*PS72 + P(1,21)*PS62 - P(13,21)*PS44 + P(14,21)*PS140 - P(15,21)*PS139 + P(2,21)*PS60 - P(3,21)*PS74 + P(4,21);
|
||||
nextP(5,21) = P(0,21)*PS74 - P(1,21)*PS60 - P(13,21)*PS162 - P(14,21)*PS65 + P(15,21)*PS160 + P(2,21)*PS62 + P(3,21)*PS72 + P(5,21);
|
||||
nextP(6,21) = P(0,21)*PS60 + P(1,21)*PS74 + P(13,21)*PS166 - P(14,21)*PS165 - P(15,21)*PS70 - P(2,21)*PS72 + P(3,21)*PS62 + P(6,21);
|
||||
nextP(7,21) = P(4,21)*dt + P(7,21);
|
||||
nextP(8,21) = P(5,21)*dt + P(8,21);
|
||||
nextP(9,21) = P(6,21)*dt + P(9,21);
|
||||
nextP(10,21) = P(10,21);
|
||||
nextP(11,21) = P(11,21);
|
||||
nextP(12,21) = P(12,21);
|
||||
nextP(13,21) = P(13,21);
|
||||
nextP(14,21) = P(14,21);
|
||||
nextP(15,21) = P(15,21);
|
||||
nextP(16,21) = P(16,21);
|
||||
nextP(17,21) = P(17,21);
|
||||
nextP(18,21) = P(18,21);
|
||||
nextP(19,21) = P(19,21);
|
||||
nextP(20,21) = P(20,21);
|
||||
nextP(21,21) = P(21,21);
|
||||
nextP(0,22) = P(0,22) - P(1,22)*PS11 + P(10,22)*PS6 + P(11,22)*PS7 + P(12,22)*PS9 - P(2,22)*PS12 - P(3,22)*PS13;
|
||||
nextP(1,22) = P(0,22)*PS11 + P(1,22) - P(10,22)*PS34 + P(11,22)*PS9 - P(12,22)*PS7 + P(2,22)*PS13 - P(3,22)*PS12;
|
||||
nextP(2,22) = P(0,22)*PS12 - P(1,22)*PS13 - P(10,22)*PS9 - P(11,22)*PS34 + P(12,22)*PS6 + P(2,22) + P(3,22)*PS11;
|
||||
nextP(3,22) = P(0,22)*PS13 + P(1,22)*PS12 + P(10,22)*PS7 - P(11,22)*PS6 - P(12,22)*PS34 - P(2,22)*PS11 + P(3,22);
|
||||
nextP(4,22) = P(0,22)*PS72 + P(1,22)*PS62 - P(13,22)*PS44 + P(14,22)*PS140 - P(15,22)*PS139 + P(2,22)*PS60 - P(3,22)*PS74 + P(4,22);
|
||||
nextP(5,22) = P(0,22)*PS74 - P(1,22)*PS60 - P(13,22)*PS162 - P(14,22)*PS65 + P(15,22)*PS160 + P(2,22)*PS62 + P(3,22)*PS72 + P(5,22);
|
||||
nextP(6,22) = P(0,22)*PS60 + P(1,22)*PS74 + P(13,22)*PS166 - P(14,22)*PS165 - P(15,22)*PS70 - P(2,22)*PS72 + P(3,22)*PS62 + P(6,22);
|
||||
nextP(7,22) = P(4,22)*dt + P(7,22);
|
||||
nextP(8,22) = P(5,22)*dt + P(8,22);
|
||||
nextP(9,22) = P(6,22)*dt + P(9,22);
|
||||
nextP(10,22) = P(10,22);
|
||||
nextP(11,22) = P(11,22);
|
||||
nextP(12,22) = P(12,22);
|
||||
nextP(13,22) = P(13,22);
|
||||
nextP(14,22) = P(14,22);
|
||||
nextP(15,22) = P(15,22);
|
||||
nextP(16,22) = P(16,22);
|
||||
nextP(17,22) = P(17,22);
|
||||
nextP(18,22) = P(18,22);
|
||||
nextP(19,22) = P(19,22);
|
||||
nextP(20,22) = P(20,22);
|
||||
nextP(21,22) = P(21,22);
|
||||
nextP(22,22) = P(22,22);
|
||||
nextP(0,23) = P(0,23) - P(1,23)*PS11 + P(10,23)*PS6 + P(11,23)*PS7 + P(12,23)*PS9 - P(2,23)*PS12 - P(3,23)*PS13;
|
||||
nextP(1,23) = P(0,23)*PS11 + P(1,23) - P(10,23)*PS34 + P(11,23)*PS9 - P(12,23)*PS7 + P(2,23)*PS13 - P(3,23)*PS12;
|
||||
nextP(2,23) = P(0,23)*PS12 - P(1,23)*PS13 - P(10,23)*PS9 - P(11,23)*PS34 + P(12,23)*PS6 + P(2,23) + P(3,23)*PS11;
|
||||
nextP(3,23) = P(0,23)*PS13 + P(1,23)*PS12 + P(10,23)*PS7 - P(11,23)*PS6 - P(12,23)*PS34 - P(2,23)*PS11 + P(3,23);
|
||||
nextP(4,23) = P(0,23)*PS72 + P(1,23)*PS62 - P(13,23)*PS44 + P(14,23)*PS140 - P(15,23)*PS139 + P(2,23)*PS60 - P(3,23)*PS74 + P(4,23);
|
||||
nextP(5,23) = P(0,23)*PS74 - P(1,23)*PS60 - P(13,23)*PS162 - P(14,23)*PS65 + P(15,23)*PS160 + P(2,23)*PS62 + P(3,23)*PS72 + P(5,23);
|
||||
nextP(6,23) = P(0,23)*PS60 + P(1,23)*PS74 + P(13,23)*PS166 - P(14,23)*PS165 - P(15,23)*PS70 - P(2,23)*PS72 + P(3,23)*PS62 + P(6,23);
|
||||
nextP(7,23) = P(4,23)*dt + P(7,23);
|
||||
nextP(8,23) = P(5,23)*dt + P(8,23);
|
||||
nextP(9,23) = P(6,23)*dt + P(9,23);
|
||||
nextP(10,23) = P(10,23);
|
||||
nextP(11,23) = P(11,23);
|
||||
nextP(12,23) = P(12,23);
|
||||
nextP(13,23) = P(13,23);
|
||||
nextP(14,23) = P(14,23);
|
||||
nextP(15,23) = P(15,23);
|
||||
nextP(16,23) = P(16,23);
|
||||
nextP(17,23) = P(17,23);
|
||||
nextP(18,23) = P(18,23);
|
||||
nextP(19,23) = P(19,23);
|
||||
nextP(20,23) = P(20,23);
|
||||
nextP(21,23) = P(21,23);
|
||||
nextP(22,23) = P(22,23);
|
||||
nextP(23,23) = P(23,23);
|
||||
|
||||
// save output and repeat calculation using legacy matlab generated code
|
||||
SquareMatrix24f nextP_sympy;
|
||||
for (int col=0; col<=23; col++) {
|
||||
for (int row=0; row<=col; row++) {
|
||||
nextP_sympy(row,col) = nextP(row,col);
|
||||
}
|
||||
}
|
||||
|
||||
// intermediate calculations
|
||||
float SF[21];
|
||||
SF[0] = dvz - dvz_b;
|
||||
SF[1] = dvy - dvy_b;
|
||||
SF[2] = dvx - dvx_b;
|
||||
SF[3] = 2*q1*SF[2] + 2*q2*SF[1] + 2*q3*SF[0];
|
||||
SF[4] = 2*q0*SF[1] - 2*q1*SF[0] + 2*q3*SF[2];
|
||||
SF[5] = 2*q0*SF[2] + 2*q2*SF[0] - 2*q3*SF[1];
|
||||
SF[6] = day*0.5f - day_b*0.5f;
|
||||
SF[7] = daz*0.5f - daz_b*0.5f;
|
||||
SF[8] = dax*0.5f - dax_b*0.5f;
|
||||
SF[9] = dax_b*0.5f - dax*0.5f;
|
||||
SF[10] = daz_b*0.5f - daz*0.5f;
|
||||
SF[11] = day_b*0.5f - day*0.5f;
|
||||
SF[12] = 2*q1*SF[1];
|
||||
SF[13] = 2*q0*SF[0];
|
||||
SF[14] = q1*0.5f;
|
||||
SF[15] = q2*0.5f;
|
||||
SF[16] = q3*0.5f;
|
||||
SF[17] = sq(q3);
|
||||
SF[18] = sq(q2);
|
||||
SF[19] = sq(q1);
|
||||
SF[20] = sq(q0);
|
||||
|
||||
float SG[8];
|
||||
SG[0] = q0*0.5f;
|
||||
SG[1] = sq(q3);
|
||||
SG[2] = sq(q2);
|
||||
SG[3] = sq(q1);
|
||||
SG[4] = sq(q0);
|
||||
SG[5] = 2*q2*q3;
|
||||
SG[6] = 2*q1*q3;
|
||||
SG[7] = 2*q1*q2;
|
||||
|
||||
float SQ[11];
|
||||
SQ[0] = dvzVar*(SG[5] - 2*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyVar*(SG[5] + 2*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxVar*(SG[6] - 2*q0*q2)*(SG[7] + 2*q0*q3);
|
||||
SQ[1] = dvzVar*(SG[6] + 2*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxVar*(SG[6] - 2*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyVar*(SG[5] + 2*q0*q1)*(SG[7] - 2*q0*q3);
|
||||
SQ[2] = dvzVar*(SG[5] - 2*q0*q1)*(SG[6] + 2*q0*q2) - dvyVar*(SG[7] - 2*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxVar*(SG[7] + 2*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]);
|
||||
SQ[3] = (dayVar*q1*SG[0])*0.5f - (dazVar*q1*SG[0])*0.5f - (daxVar*q2*q3)*0.25f;
|
||||
SQ[4] = (dazVar*q2*SG[0])*0.5f - (daxVar*q2*SG[0])*0.5f - (dayVar*q1*q3)*0.25f;
|
||||
SQ[5] = (daxVar*q3*SG[0])*0.5f - (dayVar*q3*SG[0])*0.5f - (dazVar*q1*q2)*0.25f;
|
||||
SQ[6] = (daxVar*q1*q2)*0.25f - (dazVar*q3*SG[0])*0.5f - (dayVar*q1*q2)*0.25f;
|
||||
SQ[7] = (dazVar*q1*q3)*0.25f - (daxVar*q1*q3)*0.25f - (dayVar*q2*SG[0])*0.5f;
|
||||
SQ[8] = (dayVar*q2*q3)*0.25f - (daxVar*q1*SG[0])*0.5f - (dazVar*q2*q3)*0.25f;
|
||||
SQ[9] = sq(SG[0]);
|
||||
SQ[10] = sq(q1);
|
||||
|
||||
float SPP[11];
|
||||
SPP[0] = SF[12] + SF[13] - 2*q2*SF[2];
|
||||
SPP[1] = SF[17] - SF[18] - SF[19] + SF[20];
|
||||
SPP[2] = SF[17] - SF[18] + SF[19] - SF[20];
|
||||
SPP[3] = SF[17] + SF[18] - SF[19] - SF[20];
|
||||
SPP[4] = 2*q0*q2 - 2*q1*q3;
|
||||
SPP[5] = 2*q0*q1 - 2*q2*q3;
|
||||
SPP[6] = 2*q0*q3 - 2*q1*q2;
|
||||
SPP[7] = 2*q0*q1 + 2*q2*q3;
|
||||
SPP[8] = 2*q0*q3 + 2*q1*q2;
|
||||
SPP[9] = 2*q0*q2 + 2*q1*q3;
|
||||
SPP[10] = SF[16];
|
||||
|
||||
// calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states
|
||||
nextP(0,0) = P(0,0) + P(1,0)*SF[9] + P(2,0)*SF[11] + P(3,0)*SF[10] + P(10,0)*SF[14] + P(11,0)*SF[15] + P(12,0)*SPP[10] + (daxVar*SQ[10])*0.25f + SF[9]*(P(0,1) + P(1,1)*SF[9] + P(2,1)*SF[11] + P(3,1)*SF[10] + P(10,1)*SF[14] + P(11,1)*SF[15] + P(12,1)*SPP[10]) + SF[11]*(P(0,2) + P(1,2)*SF[9] + P(2,2)*SF[11] + P(3,2)*SF[10] + P(10,2)*SF[14] + P(11,2)*SF[15] + P(12,2)*SPP[10]) + SF[10]*(P(0,3) + P(1,3)*SF[9] + P(2,3)*SF[11] + P(3,3)*SF[10] + P(10,3)*SF[14] + P(11,3)*SF[15] + P(12,3)*SPP[10]) + SF[14]*(P(0,10) + P(1,10)*SF[9] + P(2,10)*SF[11] + P(3,10)*SF[10] + P(10,10)*SF[14] + P(11,10)*SF[15] + P(12,10)*SPP[10]) + SF[15]*(P(0,11) + P(1,11)*SF[9] + P(2,11)*SF[11] + P(3,11)*SF[10] + P(10,11)*SF[14] + P(11,11)*SF[15] + P(12,11)*SPP[10]) + SPP[10]*(P(0,12) + P(1,12)*SF[9] + P(2,12)*SF[11] + P(3,12)*SF[10] + P(10,12)*SF[14] + P(11,12)*SF[15] + P(12,12)*SPP[10]) + (dayVar*sq(q2))*0.25f + (dazVar*sq(q3))*0.25f;
|
||||
nextP(0,1) = P(0,1) + SQ[8] + P(1,1)*SF[9] + P(2,1)*SF[11] + P(3,1)*SF[10] + P(10,1)*SF[14] + P(11,1)*SF[15] + P(12,1)*SPP[10] + SF[8]*(P(0,0) + P(1,0)*SF[9] + P(2,0)*SF[11] + P(3,0)*SF[10] + P(10,0)*SF[14] + P(11,0)*SF[15] + P(12,0)*SPP[10]) + SF[7]*(P(0,2) + P(1,2)*SF[9] + P(2,2)*SF[11] + P(3,2)*SF[10] + P(10,2)*SF[14] + P(11,2)*SF[15] + P(12,2)*SPP[10]) + SF[11]*(P(0,3) + P(1,3)*SF[9] + P(2,3)*SF[11] + P(3,3)*SF[10] + P(10,3)*SF[14] + P(11,3)*SF[15] + P(12,3)*SPP[10]) - SF[15]*(P(0,12) + P(1,12)*SF[9] + P(2,12)*SF[11] + P(3,12)*SF[10] + P(10,12)*SF[14] + P(11,12)*SF[15] + P(12,12)*SPP[10]) + SPP[10]*(P(0,11) + P(1,11)*SF[9] + P(2,11)*SF[11] + P(3,11)*SF[10] + P(10,11)*SF[14] + P(11,11)*SF[15] + P(12,11)*SPP[10]) - (q0*(P(0,10) + P(1,10)*SF[9] + P(2,10)*SF[11] + P(3,10)*SF[10] + P(10,10)*SF[14] + P(11,10)*SF[15] + P(12,10)*SPP[10]))*0.5f;
|
||||
nextP(1,1) = P(1,1) + P(0,1)*SF[8] + P(2,1)*SF[7] + P(3,1)*SF[11] - P(12,1)*SF[15] + P(11,1)*SPP[10] + daxVar*SQ[9] - (P(10,1)*q0)*0.5f + SF[8]*(P(1,0) + P(0,0)*SF[8] + P(2,0)*SF[7] + P(3,0)*SF[11] - P(12,0)*SF[15] + P(11,0)*SPP[10] - (P(10,0)*q0)*0.5f) + SF[7]*(P(1,2) + P(0,2)*SF[8] + P(2,2)*SF[7] + P(3,2)*SF[11] - P(12,2)*SF[15] + P(11,2)*SPP[10] - (P(10,2)*q0)*0.5f) + SF[11]*(P(1,3) + P(0,3)*SF[8] + P(2,3)*SF[7] + P(3,3)*SF[11] - P(12,3)*SF[15] + P(11,3)*SPP[10] - (P(10,3)*q0)*0.5f) - SF[15]*(P(1,12) + P(0,12)*SF[8] + P(2,12)*SF[7] + P(3,12)*SF[11] - P(12,12)*SF[15] + P(11,12)*SPP[10] - (P(10,12)*q0)*0.5f) + SPP[10]*(P(1,11) + P(0,11)*SF[8] + P(2,11)*SF[7] + P(3,11)*SF[11] - P(12,11)*SF[15] + P(11,11)*SPP[10] - (P(10,11)*q0)*0.5f) + (dayVar*sq(q3))*0.25f + (dazVar*sq(q2))*0.25f - (q0*(P(1,10) + P(0,10)*SF[8] + P(2,10)*SF[7] + P(3,10)*SF[11] - P(12,10)*SF[15] + P(11,10)*SPP[10] - (P(10,10)*q0)*0.5f))*0.5f;
|
||||
nextP(0,2) = P(0,2) + SQ[7] + P(1,2)*SF[9] + P(2,2)*SF[11] + P(3,2)*SF[10] + P(10,2)*SF[14] + P(11,2)*SF[15] + P(12,2)*SPP[10] + SF[6]*(P(0,0) + P(1,0)*SF[9] + P(2,0)*SF[11] + P(3,0)*SF[10] + P(10,0)*SF[14] + P(11,0)*SF[15] + P(12,0)*SPP[10]) + SF[10]*(P(0,1) + P(1,1)*SF[9] + P(2,1)*SF[11] + P(3,1)*SF[10] + P(10,1)*SF[14] + P(11,1)*SF[15] + P(12,1)*SPP[10]) + SF[8]*(P(0,3) + P(1,3)*SF[9] + P(2,3)*SF[11] + P(3,3)*SF[10] + P(10,3)*SF[14] + P(11,3)*SF[15] + P(12,3)*SPP[10]) + SF[14]*(P(0,12) + P(1,12)*SF[9] + P(2,12)*SF[11] + P(3,12)*SF[10] + P(10,12)*SF[14] + P(11,12)*SF[15] + P(12,12)*SPP[10]) - SPP[10]*(P(0,10) + P(1,10)*SF[9] + P(2,10)*SF[11] + P(3,10)*SF[10] + P(10,10)*SF[14] + P(11,10)*SF[15] + P(12,10)*SPP[10]) - (q0*(P(0,11) + P(1,11)*SF[9] + P(2,11)*SF[11] + P(3,11)*SF[10] + P(10,11)*SF[14] + P(11,11)*SF[15] + P(12,11)*SPP[10]))*0.5f;
|
||||
nextP(1,2) = P(1,2) + SQ[5] + P(0,2)*SF[8] + P(2,2)*SF[7] + P(3,2)*SF[11] - P(12,2)*SF[15] + P(11,2)*SPP[10] - (P(10,2)*q0)*0.5f + SF[6]*(P(1,0) + P(0,0)*SF[8] + P(2,0)*SF[7] + P(3,0)*SF[11] - P(12,0)*SF[15] + P(11,0)*SPP[10] - (P(10,0)*q0)*0.5f) + SF[10]*(P(1,1) + P(0,1)*SF[8] + P(2,1)*SF[7] + P(3,1)*SF[11] - P(12,1)*SF[15] + P(11,1)*SPP[10] - (P(10,1)*q0)*0.5f) + SF[8]*(P(1,3) + P(0,3)*SF[8] + P(2,3)*SF[7] + P(3,3)*SF[11] - P(12,3)*SF[15] + P(11,3)*SPP[10] - (P(10,3)*q0)*0.5f) + SF[14]*(P(1,12) + P(0,12)*SF[8] + P(2,12)*SF[7] + P(3,12)*SF[11] - P(12,12)*SF[15] + P(11,12)*SPP[10] - (P(10,12)*q0)*0.5f) - SPP[10]*(P(1,10) + P(0,10)*SF[8] + P(2,10)*SF[7] + P(3,10)*SF[11] - P(12,10)*SF[15] + P(11,10)*SPP[10] - (P(10,10)*q0)*0.5f) - (q0*(P(1,11) + P(0,11)*SF[8] + P(2,11)*SF[7] + P(3,11)*SF[11] - P(12,11)*SF[15] + P(11,11)*SPP[10] - (P(10,11)*q0)*0.5f))*0.5f;
|
||||
nextP(2,2) = P(2,2) + P(0,2)*SF[6] + P(1,2)*SF[10] + P(3,2)*SF[8] + P(12,2)*SF[14] - P(10,2)*SPP[10] + dayVar*SQ[9] + (dazVar*SQ[10])*0.25f - (P(11,2)*q0)*0.5f + SF[6]*(P(2,0) + P(0,0)*SF[6] + P(1,0)*SF[10] + P(3,0)*SF[8] + P(12,0)*SF[14] - P(10,0)*SPP[10] - (P(11,0)*q0)*0.5f) + SF[10]*(P(2,1) + P(0,1)*SF[6] + P(1,1)*SF[10] + P(3,1)*SF[8] + P(12,1)*SF[14] - P(10,1)*SPP[10] - (P(11,1)*q0)*0.5f) + SF[8]*(P(2,3) + P(0,3)*SF[6] + P(1,3)*SF[10] + P(3,3)*SF[8] + P(12,3)*SF[14] - P(10,3)*SPP[10] - (P(11,3)*q0)*0.5f) + SF[14]*(P(2,12) + P(0,12)*SF[6] + P(1,12)*SF[10] + P(3,12)*SF[8] + P(12,12)*SF[14] - P(10,12)*SPP[10] - (P(11,12)*q0)*0.5f) - SPP[10]*(P(2,10) + P(0,10)*SF[6] + P(1,10)*SF[10] + P(3,10)*SF[8] + P(12,10)*SF[14] - P(10,10)*SPP[10] - (P(11,10)*q0)*0.5f) + (daxVar*sq(q3))*0.25f - (q0*(P(2,11) + P(0,11)*SF[6] + P(1,11)*SF[10] + P(3,11)*SF[8] + P(12,11)*SF[14] - P(10,11)*SPP[10] - (P(11,11)*q0)*0.5f))*0.5f;
|
||||
nextP(0,3) = P(0,3) + SQ[6] + P(1,3)*SF[9] + P(2,3)*SF[11] + P(3,3)*SF[10] + P(10,3)*SF[14] + P(11,3)*SF[15] + P(12,3)*SPP[10] + SF[7]*(P(0,0) + P(1,0)*SF[9] + P(2,0)*SF[11] + P(3,0)*SF[10] + P(10,0)*SF[14] + P(11,0)*SF[15] + P(12,0)*SPP[10]) + SF[6]*(P(0,1) + P(1,1)*SF[9] + P(2,1)*SF[11] + P(3,1)*SF[10] + P(10,1)*SF[14] + P(11,1)*SF[15] + P(12,1)*SPP[10]) + SF[9]*(P(0,2) + P(1,2)*SF[9] + P(2,2)*SF[11] + P(3,2)*SF[10] + P(10,2)*SF[14] + P(11,2)*SF[15] + P(12,2)*SPP[10]) + SF[15]*(P(0,10) + P(1,10)*SF[9] + P(2,10)*SF[11] + P(3,10)*SF[10] + P(10,10)*SF[14] + P(11,10)*SF[15] + P(12,10)*SPP[10]) - SF[14]*(P(0,11) + P(1,11)*SF[9] + P(2,11)*SF[11] + P(3,11)*SF[10] + P(10,11)*SF[14] + P(11,11)*SF[15] + P(12,11)*SPP[10]) - (q0*(P(0,12) + P(1,12)*SF[9] + P(2,12)*SF[11] + P(3,12)*SF[10] + P(10,12)*SF[14] + P(11,12)*SF[15] + P(12,12)*SPP[10]))*0.5f;
|
||||
nextP(1,3) = P(1,3) + SQ[4] + P(0,3)*SF[8] + P(2,3)*SF[7] + P(3,3)*SF[11] - P(12,3)*SF[15] + P(11,3)*SPP[10] - (P(10,3)*q0)*0.5f + SF[7]*(P(1,0) + P(0,0)*SF[8] + P(2,0)*SF[7] + P(3,0)*SF[11] - P(12,0)*SF[15] + P(11,0)*SPP[10] - (P(10,0)*q0)*0.5f) + SF[6]*(P(1,1) + P(0,1)*SF[8] + P(2,1)*SF[7] + P(3,1)*SF[11] - P(12,1)*SF[15] + P(11,1)*SPP[10] - (P(10,1)*q0)*0.5f) + SF[9]*(P(1,2) + P(0,2)*SF[8] + P(2,2)*SF[7] + P(3,2)*SF[11] - P(12,2)*SF[15] + P(11,2)*SPP[10] - (P(10,2)*q0)*0.5f) + SF[15]*(P(1,10) + P(0,10)*SF[8] + P(2,10)*SF[7] + P(3,10)*SF[11] - P(12,10)*SF[15] + P(11,10)*SPP[10] - (P(10,10)*q0)*0.5f) - SF[14]*(P(1,11) + P(0,11)*SF[8] + P(2,11)*SF[7] + P(3,11)*SF[11] - P(12,11)*SF[15] + P(11,11)*SPP[10] - (P(10,11)*q0)*0.5f) - (q0*(P(1,12) + P(0,12)*SF[8] + P(2,12)*SF[7] + P(3,12)*SF[11] - P(12,12)*SF[15] + P(11,12)*SPP[10] - (P(10,12)*q0)*0.5f))*0.5f;
|
||||
nextP(2,3) = P(2,3) + SQ[3] + P(0,3)*SF[6] + P(1,3)*SF[10] + P(3,3)*SF[8] + P(12,3)*SF[14] - P(10,3)*SPP[10] - (P(11,3)*q0)*0.5f + SF[7]*(P(2,0) + P(0,0)*SF[6] + P(1,0)*SF[10] + P(3,0)*SF[8] + P(12,0)*SF[14] - P(10,0)*SPP[10] - (P(11,0)*q0)*0.5f) + SF[6]*(P(2,1) + P(0,1)*SF[6] + P(1,1)*SF[10] + P(3,1)*SF[8] + P(12,1)*SF[14] - P(10,1)*SPP[10] - (P(11,1)*q0)*0.5f) + SF[9]*(P(2,2) + P(0,2)*SF[6] + P(1,2)*SF[10] + P(3,2)*SF[8] + P(12,2)*SF[14] - P(10,2)*SPP[10] - (P(11,2)*q0)*0.5f) + SF[15]*(P(2,10) + P(0,10)*SF[6] + P(1,10)*SF[10] + P(3,10)*SF[8] + P(12,10)*SF[14] - P(10,10)*SPP[10] - (P(11,10)*q0)*0.5f) - SF[14]*(P(2,11) + P(0,11)*SF[6] + P(1,11)*SF[10] + P(3,11)*SF[8] + P(12,11)*SF[14] - P(10,11)*SPP[10] - (P(11,11)*q0)*0.5f) - (q0*(P(2,12) + P(0,12)*SF[6] + P(1,12)*SF[10] + P(3,12)*SF[8] + P(12,12)*SF[14] - P(10,12)*SPP[10] - (P(11,12)*q0)*0.5f))*0.5f;
|
||||
nextP(3,3) = P(3,3) + P(0,3)*SF[7] + P(1,3)*SF[6] + P(2,3)*SF[9] + P(10,3)*SF[15] - P(11,3)*SF[14] + (dayVar*SQ[10])*0.25f + dazVar*SQ[9] - (P(12,3)*q0)*0.5f + SF[7]*(P(3,0) + P(0,0)*SF[7] + P(1,0)*SF[6] + P(2,0)*SF[9] + P(10,0)*SF[15] - P(11,0)*SF[14] - (P(12,0)*q0)*0.5f) + SF[6]*(P(3,1) + P(0,1)*SF[7] + P(1,1)*SF[6] + P(2,1)*SF[9] + P(10,1)*SF[15] - P(11,1)*SF[14] - (P(12,1)*q0)*0.5f) + SF[9]*(P(3,2) + P(0,2)*SF[7] + P(1,2)*SF[6] + P(2,2)*SF[9] + P(10,2)*SF[15] - P(11,2)*SF[14] - (P(12,2)*q0)*0.5f) + SF[15]*(P(3,10) + P(0,10)*SF[7] + P(1,10)*SF[6] + P(2,10)*SF[9] + P(10,10)*SF[15] - P(11,10)*SF[14] - (P(12,10)*q0)*0.5f) - SF[14]*(P(3,11) + P(0,11)*SF[7] + P(1,11)*SF[6] + P(2,11)*SF[9] + P(10,11)*SF[15] - P(11,11)*SF[14] - (P(12,11)*q0)*0.5f) + (daxVar*sq(q2))*0.25f - (q0*(P(3,12) + P(0,12)*SF[7] + P(1,12)*SF[6] + P(2,12)*SF[9] + P(10,12)*SF[15] - P(11,12)*SF[14] - (P(12,12)*q0)*0.5f))*0.5f;
|
||||
nextP(0,4) = P(0,4) + P(1,4)*SF[9] + P(2,4)*SF[11] + P(3,4)*SF[10] + P(10,4)*SF[14] + P(11,4)*SF[15] + P(12,4)*SPP[10] + SF[5]*(P(0,0) + P(1,0)*SF[9] + P(2,0)*SF[11] + P(3,0)*SF[10] + P(10,0)*SF[14] + P(11,0)*SF[15] + P(12,0)*SPP[10]) + SF[3]*(P(0,1) + P(1,1)*SF[9] + P(2,1)*SF[11] + P(3,1)*SF[10] + P(10,1)*SF[14] + P(11,1)*SF[15] + P(12,1)*SPP[10]) - SF[4]*(P(0,3) + P(1,3)*SF[9] + P(2,3)*SF[11] + P(3,3)*SF[10] + P(10,3)*SF[14] + P(11,3)*SF[15] + P(12,3)*SPP[10]) + SPP[0]*(P(0,2) + P(1,2)*SF[9] + P(2,2)*SF[11] + P(3,2)*SF[10] + P(10,2)*SF[14] + P(11,2)*SF[15] + P(12,2)*SPP[10]) + SPP[3]*(P(0,13) + P(1,13)*SF[9] + P(2,13)*SF[11] + P(3,13)*SF[10] + P(10,13)*SF[14] + P(11,13)*SF[15] + P(12,13)*SPP[10]) + SPP[6]*(P(0,14) + P(1,14)*SF[9] + P(2,14)*SF[11] + P(3,14)*SF[10] + P(10,14)*SF[14] + P(11,14)*SF[15] + P(12,14)*SPP[10]) - SPP[9]*(P(0,15) + P(1,15)*SF[9] + P(2,15)*SF[11] + P(3,15)*SF[10] + P(10,15)*SF[14] + P(11,15)*SF[15] + P(12,15)*SPP[10]);
|
||||
nextP(1,4) = P(1,4) + P(0,4)*SF[8] + P(2,4)*SF[7] + P(3,4)*SF[11] - P(12,4)*SF[15] + P(11,4)*SPP[10] - (P(10,4)*q0)*0.5f + SF[5]*(P(1,0) + P(0,0)*SF[8] + P(2,0)*SF[7] + P(3,0)*SF[11] - P(12,0)*SF[15] + P(11,0)*SPP[10] - (P(10,0)*q0)*0.5f) + SF[3]*(P(1,1) + P(0,1)*SF[8] + P(2,1)*SF[7] + P(3,1)*SF[11] - P(12,1)*SF[15] + P(11,1)*SPP[10] - (P(10,1)*q0)*0.5f) - SF[4]*(P(1,3) + P(0,3)*SF[8] + P(2,3)*SF[7] + P(3,3)*SF[11] - P(12,3)*SF[15] + P(11,3)*SPP[10] - (P(10,3)*q0)*0.5f) + SPP[0]*(P(1,2) + P(0,2)*SF[8] + P(2,2)*SF[7] + P(3,2)*SF[11] - P(12,2)*SF[15] + P(11,2)*SPP[10] - (P(10,2)*q0)*0.5f) + SPP[3]*(P(1,13) + P(0,13)*SF[8] + P(2,13)*SF[7] + P(3,13)*SF[11] - P(12,13)*SF[15] + P(11,13)*SPP[10] - (P(10,13)*q0)*0.5f) + SPP[6]*(P(1,14) + P(0,14)*SF[8] + P(2,14)*SF[7] + P(3,14)*SF[11] - P(12,14)*SF[15] + P(11,14)*SPP[10] - (P(10,14)*q0)*0.5f) - SPP[9]*(P(1,15) + P(0,15)*SF[8] + P(2,15)*SF[7] + P(3,15)*SF[11] - P(12,15)*SF[15] + P(11,15)*SPP[10] - (P(10,15)*q0)*0.5f);
|
||||
nextP(2,4) = P(2,4) + P(0,4)*SF[6] + P(1,4)*SF[10] + P(3,4)*SF[8] + P(12,4)*SF[14] - P(10,4)*SPP[10] - (P(11,4)*q0)*0.5f + SF[5]*(P(2,0) + P(0,0)*SF[6] + P(1,0)*SF[10] + P(3,0)*SF[8] + P(12,0)*SF[14] - P(10,0)*SPP[10] - (P(11,0)*q0)*0.5f) + SF[3]*(P(2,1) + P(0,1)*SF[6] + P(1,1)*SF[10] + P(3,1)*SF[8] + P(12,1)*SF[14] - P(10,1)*SPP[10] - (P(11,1)*q0)*0.5f) - SF[4]*(P(2,3) + P(0,3)*SF[6] + P(1,3)*SF[10] + P(3,3)*SF[8] + P(12,3)*SF[14] - P(10,3)*SPP[10] - (P(11,3)*q0)*0.5f) + SPP[0]*(P(2,2) + P(0,2)*SF[6] + P(1,2)*SF[10] + P(3,2)*SF[8] + P(12,2)*SF[14] - P(10,2)*SPP[10] - (P(11,2)*q0)*0.5f) + SPP[3]*(P(2,13) + P(0,13)*SF[6] + P(1,13)*SF[10] + P(3,13)*SF[8] + P(12,13)*SF[14] - P(10,13)*SPP[10] - (P(11,13)*q0)*0.5f) + SPP[6]*(P(2,14) + P(0,14)*SF[6] + P(1,14)*SF[10] + P(3,14)*SF[8] + P(12,14)*SF[14] - P(10,14)*SPP[10] - (P(11,14)*q0)*0.5f) - SPP[9]*(P(2,15) + P(0,15)*SF[6] + P(1,15)*SF[10] + P(3,15)*SF[8] + P(12,15)*SF[14] - P(10,15)*SPP[10] - (P(11,15)*q0)*0.5f);
|
||||
nextP(3,4) = P(3,4) + P(0,4)*SF[7] + P(1,4)*SF[6] + P(2,4)*SF[9] + P(10,4)*SF[15] - P(11,4)*SF[14] - (P(12,4)*q0)*0.5f + SF[5]*(P(3,0) + P(0,0)*SF[7] + P(1,0)*SF[6] + P(2,0)*SF[9] + P(10,0)*SF[15] - P(11,0)*SF[14] - (P(12,0)*q0)*0.5f) + SF[3]*(P(3,1) + P(0,1)*SF[7] + P(1,1)*SF[6] + P(2,1)*SF[9] + P(10,1)*SF[15] - P(11,1)*SF[14] - (P(12,1)*q0)*0.5f) - SF[4]*(P(3,3) + P(0,3)*SF[7] + P(1,3)*SF[6] + P(2,3)*SF[9] + P(10,3)*SF[15] - P(11,3)*SF[14] - (P(12,3)*q0)*0.5f) + SPP[0]*(P(3,2) + P(0,2)*SF[7] + P(1,2)*SF[6] + P(2,2)*SF[9] + P(10,2)*SF[15] - P(11,2)*SF[14] - (P(12,2)*q0)*0.5f) + SPP[3]*(P(3,13) + P(0,13)*SF[7] + P(1,13)*SF[6] + P(2,13)*SF[9] + P(10,13)*SF[15] - P(11,13)*SF[14] - (P(12,13)*q0)*0.5f) + SPP[6]*(P(3,14) + P(0,14)*SF[7] + P(1,14)*SF[6] + P(2,14)*SF[9] + P(10,14)*SF[15] - P(11,14)*SF[14] - (P(12,14)*q0)*0.5f) - SPP[9]*(P(3,15) + P(0,15)*SF[7] + P(1,15)*SF[6] + P(2,15)*SF[9] + P(10,15)*SF[15] - P(11,15)*SF[14] - (P(12,15)*q0)*0.5f);
|
||||
nextP(4,4) = P(4,4) + P(0,4)*SF[5] + P(1,4)*SF[3] - P(3,4)*SF[4] + P(2,4)*SPP[0] + P(13,4)*SPP[3] + P(14,4)*SPP[6] - P(15,4)*SPP[9] + dvyVar*sq(SG[7] - 2*q0*q3) + dvzVar*sq(SG[6] + 2*q0*q2) + SF[5]*(P(4,0) + P(0,0)*SF[5] + P(1,0)*SF[3] - P(3,0)*SF[4] + P(2,0)*SPP[0] + P(13,0)*SPP[3] + P(14,0)*SPP[6] - P(15,0)*SPP[9]) + SF[3]*(P(4,1) + P(0,1)*SF[5] + P(1,1)*SF[3] - P(3,1)*SF[4] + P(2,1)*SPP[0] + P(13,1)*SPP[3] + P(14,1)*SPP[6] - P(15,1)*SPP[9]) - SF[4]*(P(4,3) + P(0,3)*SF[5] + P(1,3)*SF[3] - P(3,3)*SF[4] + P(2,3)*SPP[0] + P(13,3)*SPP[3] + P(14,3)*SPP[6] - P(15,3)*SPP[9]) + SPP[0]*(P(4,2) + P(0,2)*SF[5] + P(1,2)*SF[3] - P(3,2)*SF[4] + P(2,2)*SPP[0] + P(13,2)*SPP[3] + P(14,2)*SPP[6] - P(15,2)*SPP[9]) + SPP[3]*(P(4,13) + P(0,13)*SF[5] + P(1,13)*SF[3] - P(3,13)*SF[4] + P(2,13)*SPP[0] + P(13,13)*SPP[3] + P(14,13)*SPP[6] - P(15,13)*SPP[9]) + SPP[6]*(P(4,14) + P(0,14)*SF[5] + P(1,14)*SF[3] - P(3,14)*SF[4] + P(2,14)*SPP[0] + P(13,14)*SPP[3] + P(14,14)*SPP[6] - P(15,14)*SPP[9]) - SPP[9]*(P(4,15) + P(0,15)*SF[5] + P(1,15)*SF[3] - P(3,15)*SF[4] + P(2,15)*SPP[0] + P(13,15)*SPP[3] + P(14,15)*SPP[6] - P(15,15)*SPP[9]) + dvxVar*sq(SG[1] + SG[2] - SG[3] - SG[4]);
|
||||
nextP(0,5) = P(0,5) + P(1,5)*SF[9] + P(2,5)*SF[11] + P(3,5)*SF[10] + P(10,5)*SF[14] + P(11,5)*SF[15] + P(12,5)*SPP[10] + SF[4]*(P(0,0) + P(1,0)*SF[9] + P(2,0)*SF[11] + P(3,0)*SF[10] + P(10,0)*SF[14] + P(11,0)*SF[15] + P(12,0)*SPP[10]) + SF[3]*(P(0,2) + P(1,2)*SF[9] + P(2,2)*SF[11] + P(3,2)*SF[10] + P(10,2)*SF[14] + P(11,2)*SF[15] + P(12,2)*SPP[10]) + SF[5]*(P(0,3) + P(1,3)*SF[9] + P(2,3)*SF[11] + P(3,3)*SF[10] + P(10,3)*SF[14] + P(11,3)*SF[15] + P(12,3)*SPP[10]) - SPP[0]*(P(0,1) + P(1,1)*SF[9] + P(2,1)*SF[11] + P(3,1)*SF[10] + P(10,1)*SF[14] + P(11,1)*SF[15] + P(12,1)*SPP[10]) - SPP[8]*(P(0,13) + P(1,13)*SF[9] + P(2,13)*SF[11] + P(3,13)*SF[10] + P(10,13)*SF[14] + P(11,13)*SF[15] + P(12,13)*SPP[10]) + SPP[2]*(P(0,14) + P(1,14)*SF[9] + P(2,14)*SF[11] + P(3,14)*SF[10] + P(10,14)*SF[14] + P(11,14)*SF[15] + P(12,14)*SPP[10]) + SPP[5]*(P(0,15) + P(1,15)*SF[9] + P(2,15)*SF[11] + P(3,15)*SF[10] + P(10,15)*SF[14] + P(11,15)*SF[15] + P(12,15)*SPP[10]);
|
||||
nextP(1,5) = P(1,5) + P(0,5)*SF[8] + P(2,5)*SF[7] + P(3,5)*SF[11] - P(12,5)*SF[15] + P(11,5)*SPP[10] - (P(10,5)*q0)*0.5f + SF[4]*(P(1,0) + P(0,0)*SF[8] + P(2,0)*SF[7] + P(3,0)*SF[11] - P(12,0)*SF[15] + P(11,0)*SPP[10] - (P(10,0)*q0)*0.5f) + SF[3]*(P(1,2) + P(0,2)*SF[8] + P(2,2)*SF[7] + P(3,2)*SF[11] - P(12,2)*SF[15] + P(11,2)*SPP[10] - (P(10,2)*q0)*0.5f) + SF[5]*(P(1,3) + P(0,3)*SF[8] + P(2,3)*SF[7] + P(3,3)*SF[11] - P(12,3)*SF[15] + P(11,3)*SPP[10] - (P(10,3)*q0)*0.5f) - SPP[0]*(P(1,1) + P(0,1)*SF[8] + P(2,1)*SF[7] + P(3,1)*SF[11] - P(12,1)*SF[15] + P(11,1)*SPP[10] - (P(10,1)*q0)*0.5f) - SPP[8]*(P(1,13) + P(0,13)*SF[8] + P(2,13)*SF[7] + P(3,13)*SF[11] - P(12,13)*SF[15] + P(11,13)*SPP[10] - (P(10,13)*q0)*0.5f) + SPP[2]*(P(1,14) + P(0,14)*SF[8] + P(2,14)*SF[7] + P(3,14)*SF[11] - P(12,14)*SF[15] + P(11,14)*SPP[10] - (P(10,14)*q0)*0.5f) + SPP[5]*(P(1,15) + P(0,15)*SF[8] + P(2,15)*SF[7] + P(3,15)*SF[11] - P(12,15)*SF[15] + P(11,15)*SPP[10] - (P(10,15)*q0)*0.5f);
|
||||
nextP(2,5) = P(2,5) + P(0,5)*SF[6] + P(1,5)*SF[10] + P(3,5)*SF[8] + P(12,5)*SF[14] - P(10,5)*SPP[10] - (P(11,5)*q0)*0.5f + SF[4]*(P(2,0) + P(0,0)*SF[6] + P(1,0)*SF[10] + P(3,0)*SF[8] + P(12,0)*SF[14] - P(10,0)*SPP[10] - (P(11,0)*q0)*0.5f) + SF[3]*(P(2,2) + P(0,2)*SF[6] + P(1,2)*SF[10] + P(3,2)*SF[8] + P(12,2)*SF[14] - P(10,2)*SPP[10] - (P(11,2)*q0)*0.5f) + SF[5]*(P(2,3) + P(0,3)*SF[6] + P(1,3)*SF[10] + P(3,3)*SF[8] + P(12,3)*SF[14] - P(10,3)*SPP[10] - (P(11,3)*q0)*0.5f) - SPP[0]*(P(2,1) + P(0,1)*SF[6] + P(1,1)*SF[10] + P(3,1)*SF[8] + P(12,1)*SF[14] - P(10,1)*SPP[10] - (P(11,1)*q0)*0.5f) - SPP[8]*(P(2,13) + P(0,13)*SF[6] + P(1,13)*SF[10] + P(3,13)*SF[8] + P(12,13)*SF[14] - P(10,13)*SPP[10] - (P(11,13)*q0)*0.5f) + SPP[2]*(P(2,14) + P(0,14)*SF[6] + P(1,14)*SF[10] + P(3,14)*SF[8] + P(12,14)*SF[14] - P(10,14)*SPP[10] - (P(11,14)*q0)*0.5f) + SPP[5]*(P(2,15) + P(0,15)*SF[6] + P(1,15)*SF[10] + P(3,15)*SF[8] + P(12,15)*SF[14] - P(10,15)*SPP[10] - (P(11,15)*q0)*0.5f);
|
||||
nextP(3,5) = P(3,5) + P(0,5)*SF[7] + P(1,5)*SF[6] + P(2,5)*SF[9] + P(10,5)*SF[15] - P(11,5)*SF[14] - (P(12,5)*q0)*0.5f + SF[4]*(P(3,0) + P(0,0)*SF[7] + P(1,0)*SF[6] + P(2,0)*SF[9] + P(10,0)*SF[15] - P(11,0)*SF[14] - (P(12,0)*q0)*0.5f) + SF[3]*(P(3,2) + P(0,2)*SF[7] + P(1,2)*SF[6] + P(2,2)*SF[9] + P(10,2)*SF[15] - P(11,2)*SF[14] - (P(12,2)*q0)*0.5f) + SF[5]*(P(3,3) + P(0,3)*SF[7] + P(1,3)*SF[6] + P(2,3)*SF[9] + P(10,3)*SF[15] - P(11,3)*SF[14] - (P(12,3)*q0)*0.5f) - SPP[0]*(P(3,1) + P(0,1)*SF[7] + P(1,1)*SF[6] + P(2,1)*SF[9] + P(10,1)*SF[15] - P(11,1)*SF[14] - (P(12,1)*q0)*0.5f) - SPP[8]*(P(3,13) + P(0,13)*SF[7] + P(1,13)*SF[6] + P(2,13)*SF[9] + P(10,13)*SF[15] - P(11,13)*SF[14] - (P(12,13)*q0)*0.5f) + SPP[2]*(P(3,14) + P(0,14)*SF[7] + P(1,14)*SF[6] + P(2,14)*SF[9] + P(10,14)*SF[15] - P(11,14)*SF[14] - (P(12,14)*q0)*0.5f) + SPP[5]*(P(3,15) + P(0,15)*SF[7] + P(1,15)*SF[6] + P(2,15)*SF[9] + P(10,15)*SF[15] - P(11,15)*SF[14] - (P(12,15)*q0)*0.5f);
|
||||
nextP(4,5) = P(4,5) + SQ[2] + P(0,5)*SF[5] + P(1,5)*SF[3] - P(3,5)*SF[4] + P(2,5)*SPP[0] + P(13,5)*SPP[3] + P(14,5)*SPP[6] - P(15,5)*SPP[9] + SF[4]*(P(4,0) + P(0,0)*SF[5] + P(1,0)*SF[3] - P(3,0)*SF[4] + P(2,0)*SPP[0] + P(13,0)*SPP[3] + P(14,0)*SPP[6] - P(15,0)*SPP[9]) + SF[3]*(P(4,2) + P(0,2)*SF[5] + P(1,2)*SF[3] - P(3,2)*SF[4] + P(2,2)*SPP[0] + P(13,2)*SPP[3] + P(14,2)*SPP[6] - P(15,2)*SPP[9]) + SF[5]*(P(4,3) + P(0,3)*SF[5] + P(1,3)*SF[3] - P(3,3)*SF[4] + P(2,3)*SPP[0] + P(13,3)*SPP[3] + P(14,3)*SPP[6] - P(15,3)*SPP[9]) - SPP[0]*(P(4,1) + P(0,1)*SF[5] + P(1,1)*SF[3] - P(3,1)*SF[4] + P(2,1)*SPP[0] + P(13,1)*SPP[3] + P(14,1)*SPP[6] - P(15,1)*SPP[9]) - SPP[8]*(P(4,13) + P(0,13)*SF[5] + P(1,13)*SF[3] - P(3,13)*SF[4] + P(2,13)*SPP[0] + P(13,13)*SPP[3] + P(14,13)*SPP[6] - P(15,13)*SPP[9]) + SPP[2]*(P(4,14) + P(0,14)*SF[5] + P(1,14)*SF[3] - P(3,14)*SF[4] + P(2,14)*SPP[0] + P(13,14)*SPP[3] + P(14,14)*SPP[6] - P(15,14)*SPP[9]) + SPP[5]*(P(4,15) + P(0,15)*SF[5] + P(1,15)*SF[3] - P(3,15)*SF[4] + P(2,15)*SPP[0] + P(13,15)*SPP[3] + P(14,15)*SPP[6] - P(15,15)*SPP[9]);
|
||||
nextP(5,5) = P(5,5) + P(0,5)*SF[4] + P(2,5)*SF[3] + P(3,5)*SF[5] - P(1,5)*SPP[0] - P(13,5)*SPP[8] + P(14,5)*SPP[2] + P(15,5)*SPP[5] + dvxVar*sq(SG[7] + 2*q0*q3) + dvzVar*sq(SG[5] - 2*q0*q1) + SF[4]*(P(5,0) + P(0,0)*SF[4] + P(2,0)*SF[3] + P(3,0)*SF[5] - P(1,0)*SPP[0] - P(13,0)*SPP[8] + P(14,0)*SPP[2] + P(15,0)*SPP[5]) + SF[3]*(P(5,2) + P(0,2)*SF[4] + P(2,2)*SF[3] + P(3,2)*SF[5] - P(1,2)*SPP[0] - P(13,2)*SPP[8] + P(14,2)*SPP[2] + P(15,2)*SPP[5]) + SF[5]*(P(5,3) + P(0,3)*SF[4] + P(2,3)*SF[3] + P(3,3)*SF[5] - P(1,3)*SPP[0] - P(13,3)*SPP[8] + P(14,3)*SPP[2] + P(15,3)*SPP[5]) - SPP[0]*(P(5,1) + P(0,1)*SF[4] + P(2,1)*SF[3] + P(3,1)*SF[5] - P(1,1)*SPP[0] - P(13,1)*SPP[8] + P(14,1)*SPP[2] + P(15,1)*SPP[5]) - SPP[8]*(P(5,13) + P(0,13)*SF[4] + P(2,13)*SF[3] + P(3,13)*SF[5] - P(1,13)*SPP[0] - P(13,13)*SPP[8] + P(14,13)*SPP[2] + P(15,13)*SPP[5]) + SPP[2]*(P(5,14) + P(0,14)*SF[4] + P(2,14)*SF[3] + P(3,14)*SF[5] - P(1,14)*SPP[0] - P(13,14)*SPP[8] + P(14,14)*SPP[2] + P(15,14)*SPP[5]) + SPP[5]*(P(5,15) + P(0,15)*SF[4] + P(2,15)*SF[3] + P(3,15)*SF[5] - P(1,15)*SPP[0] - P(13,15)*SPP[8] + P(14,15)*SPP[2] + P(15,15)*SPP[5]) + dvyVar*sq(SG[1] - SG[2] + SG[3] - SG[4]);
|
||||
nextP(0,6) = P(0,6) + P(1,6)*SF[9] + P(2,6)*SF[11] + P(3,6)*SF[10] + P(10,6)*SF[14] + P(11,6)*SF[15] + P(12,6)*SPP[10] + SF[4]*(P(0,1) + P(1,1)*SF[9] + P(2,1)*SF[11] + P(3,1)*SF[10] + P(10,1)*SF[14] + P(11,1)*SF[15] + P(12,1)*SPP[10]) - SF[5]*(P(0,2) + P(1,2)*SF[9] + P(2,2)*SF[11] + P(3,2)*SF[10] + P(10,2)*SF[14] + P(11,2)*SF[15] + P(12,2)*SPP[10]) + SF[3]*(P(0,3) + P(1,3)*SF[9] + P(2,3)*SF[11] + P(3,3)*SF[10] + P(10,3)*SF[14] + P(11,3)*SF[15] + P(12,3)*SPP[10]) + SPP[0]*(P(0,0) + P(1,0)*SF[9] + P(2,0)*SF[11] + P(3,0)*SF[10] + P(10,0)*SF[14] + P(11,0)*SF[15] + P(12,0)*SPP[10]) + SPP[4]*(P(0,13) + P(1,13)*SF[9] + P(2,13)*SF[11] + P(3,13)*SF[10] + P(10,13)*SF[14] + P(11,13)*SF[15] + P(12,13)*SPP[10]) - SPP[7]*(P(0,14) + P(1,14)*SF[9] + P(2,14)*SF[11] + P(3,14)*SF[10] + P(10,14)*SF[14] + P(11,14)*SF[15] + P(12,14)*SPP[10]) - SPP[1]*(P(0,15) + P(1,15)*SF[9] + P(2,15)*SF[11] + P(3,15)*SF[10] + P(10,15)*SF[14] + P(11,15)*SF[15] + P(12,15)*SPP[10]);
|
||||
nextP(1,6) = P(1,6) + P(0,6)*SF[8] + P(2,6)*SF[7] + P(3,6)*SF[11] - P(12,6)*SF[15] + P(11,6)*SPP[10] - (P(10,6)*q0)*0.5f + SF[4]*(P(1,1) + P(0,1)*SF[8] + P(2,1)*SF[7] + P(3,1)*SF[11] - P(12,1)*SF[15] + P(11,1)*SPP[10] - (P(10,1)*q0)*0.5f) - SF[5]*(P(1,2) + P(0,2)*SF[8] + P(2,2)*SF[7] + P(3,2)*SF[11] - P(12,2)*SF[15] + P(11,2)*SPP[10] - (P(10,2)*q0)*0.5f) + SF[3]*(P(1,3) + P(0,3)*SF[8] + P(2,3)*SF[7] + P(3,3)*SF[11] - P(12,3)*SF[15] + P(11,3)*SPP[10] - (P(10,3)*q0)*0.5f) + SPP[0]*(P(1,0) + P(0,0)*SF[8] + P(2,0)*SF[7] + P(3,0)*SF[11] - P(12,0)*SF[15] + P(11,0)*SPP[10] - (P(10,0)*q0)*0.5f) + SPP[4]*(P(1,13) + P(0,13)*SF[8] + P(2,13)*SF[7] + P(3,13)*SF[11] - P(12,13)*SF[15] + P(11,13)*SPP[10] - (P(10,13)*q0)*0.5f) - SPP[7]*(P(1,14) + P(0,14)*SF[8] + P(2,14)*SF[7] + P(3,14)*SF[11] - P(12,14)*SF[15] + P(11,14)*SPP[10] - (P(10,14)*q0)*0.5f) - SPP[1]*(P(1,15) + P(0,15)*SF[8] + P(2,15)*SF[7] + P(3,15)*SF[11] - P(12,15)*SF[15] + P(11,15)*SPP[10] - (P(10,15)*q0)*0.5f);
|
||||
nextP(2,6) = P(2,6) + P(0,6)*SF[6] + P(1,6)*SF[10] + P(3,6)*SF[8] + P(12,6)*SF[14] - P(10,6)*SPP[10] - (P(11,6)*q0)*0.5f + SF[4]*(P(2,1) + P(0,1)*SF[6] + P(1,1)*SF[10] + P(3,1)*SF[8] + P(12,1)*SF[14] - P(10,1)*SPP[10] - (P(11,1)*q0)*0.5f) - SF[5]*(P(2,2) + P(0,2)*SF[6] + P(1,2)*SF[10] + P(3,2)*SF[8] + P(12,2)*SF[14] - P(10,2)*SPP[10] - (P(11,2)*q0)*0.5f) + SF[3]*(P(2,3) + P(0,3)*SF[6] + P(1,3)*SF[10] + P(3,3)*SF[8] + P(12,3)*SF[14] - P(10,3)*SPP[10] - (P(11,3)*q0)*0.5f) + SPP[0]*(P(2,0) + P(0,0)*SF[6] + P(1,0)*SF[10] + P(3,0)*SF[8] + P(12,0)*SF[14] - P(10,0)*SPP[10] - (P(11,0)*q0)*0.5f) + SPP[4]*(P(2,13) + P(0,13)*SF[6] + P(1,13)*SF[10] + P(3,13)*SF[8] + P(12,13)*SF[14] - P(10,13)*SPP[10] - (P(11,13)*q0)*0.5f) - SPP[7]*(P(2,14) + P(0,14)*SF[6] + P(1,14)*SF[10] + P(3,14)*SF[8] + P(12,14)*SF[14] - P(10,14)*SPP[10] - (P(11,14)*q0)*0.5f) - SPP[1]*(P(2,15) + P(0,15)*SF[6] + P(1,15)*SF[10] + P(3,15)*SF[8] + P(12,15)*SF[14] - P(10,15)*SPP[10] - (P(11,15)*q0)*0.5f);
|
||||
nextP(3,6) = P(3,6) + P(0,6)*SF[7] + P(1,6)*SF[6] + P(2,6)*SF[9] + P(10,6)*SF[15] - P(11,6)*SF[14] - (P(12,6)*q0)*0.5f + SF[4]*(P(3,1) + P(0,1)*SF[7] + P(1,1)*SF[6] + P(2,1)*SF[9] + P(10,1)*SF[15] - P(11,1)*SF[14] - (P(12,1)*q0)*0.5f) - SF[5]*(P(3,2) + P(0,2)*SF[7] + P(1,2)*SF[6] + P(2,2)*SF[9] + P(10,2)*SF[15] - P(11,2)*SF[14] - (P(12,2)*q0)*0.5f) + SF[3]*(P(3,3) + P(0,3)*SF[7] + P(1,3)*SF[6] + P(2,3)*SF[9] + P(10,3)*SF[15] - P(11,3)*SF[14] - (P(12,3)*q0)*0.5f) + SPP[0]*(P(3,0) + P(0,0)*SF[7] + P(1,0)*SF[6] + P(2,0)*SF[9] + P(10,0)*SF[15] - P(11,0)*SF[14] - (P(12,0)*q0)*0.5f) + SPP[4]*(P(3,13) + P(0,13)*SF[7] + P(1,13)*SF[6] + P(2,13)*SF[9] + P(10,13)*SF[15] - P(11,13)*SF[14] - (P(12,13)*q0)*0.5f) - SPP[7]*(P(3,14) + P(0,14)*SF[7] + P(1,14)*SF[6] + P(2,14)*SF[9] + P(10,14)*SF[15] - P(11,14)*SF[14] - (P(12,14)*q0)*0.5f) - SPP[1]*(P(3,15) + P(0,15)*SF[7] + P(1,15)*SF[6] + P(2,15)*SF[9] + P(10,15)*SF[15] - P(11,15)*SF[14] - (P(12,15)*q0)*0.5f);
|
||||
nextP(4,6) = P(4,6) + SQ[1] + P(0,6)*SF[5] + P(1,6)*SF[3] - P(3,6)*SF[4] + P(2,6)*SPP[0] + P(13,6)*SPP[3] + P(14,6)*SPP[6] - P(15,6)*SPP[9] + SF[4]*(P(4,1) + P(0,1)*SF[5] + P(1,1)*SF[3] - P(3,1)*SF[4] + P(2,1)*SPP[0] + P(13,1)*SPP[3] + P(14,1)*SPP[6] - P(15,1)*SPP[9]) - SF[5]*(P(4,2) + P(0,2)*SF[5] + P(1,2)*SF[3] - P(3,2)*SF[4] + P(2,2)*SPP[0] + P(13,2)*SPP[3] + P(14,2)*SPP[6] - P(15,2)*SPP[9]) + SF[3]*(P(4,3) + P(0,3)*SF[5] + P(1,3)*SF[3] - P(3,3)*SF[4] + P(2,3)*SPP[0] + P(13,3)*SPP[3] + P(14,3)*SPP[6] - P(15,3)*SPP[9]) + SPP[0]*(P(4,0) + P(0,0)*SF[5] + P(1,0)*SF[3] - P(3,0)*SF[4] + P(2,0)*SPP[0] + P(13,0)*SPP[3] + P(14,0)*SPP[6] - P(15,0)*SPP[9]) + SPP[4]*(P(4,13) + P(0,13)*SF[5] + P(1,13)*SF[3] - P(3,13)*SF[4] + P(2,13)*SPP[0] + P(13,13)*SPP[3] + P(14,13)*SPP[6] - P(15,13)*SPP[9]) - SPP[7]*(P(4,14) + P(0,14)*SF[5] + P(1,14)*SF[3] - P(3,14)*SF[4] + P(2,14)*SPP[0] + P(13,14)*SPP[3] + P(14,14)*SPP[6] - P(15,14)*SPP[9]) - SPP[1]*(P(4,15) + P(0,15)*SF[5] + P(1,15)*SF[3] - P(3,15)*SF[4] + P(2,15)*SPP[0] + P(13,15)*SPP[3] + P(14,15)*SPP[6] - P(15,15)*SPP[9]);
|
||||
nextP(5,6) = P(5,6) + SQ[0] + P(0,6)*SF[4] + P(2,6)*SF[3] + P(3,6)*SF[5] - P(1,6)*SPP[0] - P(13,6)*SPP[8] + P(14,6)*SPP[2] + P(15,6)*SPP[5] + SF[4]*(P(5,1) + P(0,1)*SF[4] + P(2,1)*SF[3] + P(3,1)*SF[5] - P(1,1)*SPP[0] - P(13,1)*SPP[8] + P(14,1)*SPP[2] + P(15,1)*SPP[5]) - SF[5]*(P(5,2) + P(0,2)*SF[4] + P(2,2)*SF[3] + P(3,2)*SF[5] - P(1,2)*SPP[0] - P(13,2)*SPP[8] + P(14,2)*SPP[2] + P(15,2)*SPP[5]) + SF[3]*(P(5,3) + P(0,3)*SF[4] + P(2,3)*SF[3] + P(3,3)*SF[5] - P(1,3)*SPP[0] - P(13,3)*SPP[8] + P(14,3)*SPP[2] + P(15,3)*SPP[5]) + SPP[0]*(P(5,0) + P(0,0)*SF[4] + P(2,0)*SF[3] + P(3,0)*SF[5] - P(1,0)*SPP[0] - P(13,0)*SPP[8] + P(14,0)*SPP[2] + P(15,0)*SPP[5]) + SPP[4]*(P(5,13) + P(0,13)*SF[4] + P(2,13)*SF[3] + P(3,13)*SF[5] - P(1,13)*SPP[0] - P(13,13)*SPP[8] + P(14,13)*SPP[2] + P(15,13)*SPP[5]) - SPP[7]*(P(5,14) + P(0,14)*SF[4] + P(2,14)*SF[3] + P(3,14)*SF[5] - P(1,14)*SPP[0] - P(13,14)*SPP[8] + P(14,14)*SPP[2] + P(15,14)*SPP[5]) - SPP[1]*(P(5,15) + P(0,15)*SF[4] + P(2,15)*SF[3] + P(3,15)*SF[5] - P(1,15)*SPP[0] - P(13,15)*SPP[8] + P(14,15)*SPP[2] + P(15,15)*SPP[5]);
|
||||
nextP(6,6) = P(6,6) + P(1,6)*SF[4] - P(2,6)*SF[5] + P(3,6)*SF[3] + P(0,6)*SPP[0] + P(13,6)*SPP[4] - P(14,6)*SPP[7] - P(15,6)*SPP[1] + dvxVar*sq(SG[6] - 2*q0*q2) + dvyVar*sq(SG[5] + 2*q0*q1) + SF[4]*(P(6,1) + P(1,1)*SF[4] - P(2,1)*SF[5] + P(3,1)*SF[3] + P(0,1)*SPP[0] + P(13,1)*SPP[4] - P(14,1)*SPP[7] - P(15,1)*SPP[1]) - SF[5]*(P(6,2) + P(1,2)*SF[4] - P(2,2)*SF[5] + P(3,2)*SF[3] + P(0,2)*SPP[0] + P(13,2)*SPP[4] - P(14,2)*SPP[7] - P(15,2)*SPP[1]) + SF[3]*(P(6,3) + P(1,3)*SF[4] - P(2,3)*SF[5] + P(3,3)*SF[3] + P(0,3)*SPP[0] + P(13,3)*SPP[4] - P(14,3)*SPP[7] - P(15,3)*SPP[1]) + SPP[0]*(P(6,0) + P(1,0)*SF[4] - P(2,0)*SF[5] + P(3,0)*SF[3] + P(0,0)*SPP[0] + P(13,0)*SPP[4] - P(14,0)*SPP[7] - P(15,0)*SPP[1]) + SPP[4]*(P(6,13) + P(1,13)*SF[4] - P(2,13)*SF[5] + P(3,13)*SF[3] + P(0,13)*SPP[0] + P(13,13)*SPP[4] - P(14,13)*SPP[7] - P(15,13)*SPP[1]) - SPP[7]*(P(6,14) + P(1,14)*SF[4] - P(2,14)*SF[5] + P(3,14)*SF[3] + P(0,14)*SPP[0] + P(13,14)*SPP[4] - P(14,14)*SPP[7] - P(15,14)*SPP[1]) - SPP[1]*(P(6,15) + P(1,15)*SF[4] - P(2,15)*SF[5] + P(3,15)*SF[3] + P(0,15)*SPP[0] + P(13,15)*SPP[4] - P(14,15)*SPP[7] - P(15,15)*SPP[1]) + dvzVar*sq(SG[1] - SG[2] - SG[3] + SG[4]);
|
||||
nextP(0,7) = P(0,7) + P(1,7)*SF[9] + P(2,7)*SF[11] + P(3,7)*SF[10] + P(10,7)*SF[14] + P(11,7)*SF[15] + P(12,7)*SPP[10] + dt*(P(0,4) + P(1,4)*SF[9] + P(2,4)*SF[11] + P(3,4)*SF[10] + P(10,4)*SF[14] + P(11,4)*SF[15] + P(12,4)*SPP[10]);
|
||||
nextP(1,7) = P(1,7) + P(0,7)*SF[8] + P(2,7)*SF[7] + P(3,7)*SF[11] - P(12,7)*SF[15] + P(11,7)*SPP[10] - (P(10,7)*q0)*0.5f + dt*(P(1,4) + P(0,4)*SF[8] + P(2,4)*SF[7] + P(3,4)*SF[11] - P(12,4)*SF[15] + P(11,4)*SPP[10] - (P(10,4)*q0)*0.5f);
|
||||
nextP(2,7) = P(2,7) + P(0,7)*SF[6] + P(1,7)*SF[10] + P(3,7)*SF[8] + P(12,7)*SF[14] - P(10,7)*SPP[10] - (P(11,7)*q0)*0.5f + dt*(P(2,4) + P(0,4)*SF[6] + P(1,4)*SF[10] + P(3,4)*SF[8] + P(12,4)*SF[14] - P(10,4)*SPP[10] - (P(11,4)*q0)*0.5f);
|
||||
nextP(3,7) = P(3,7) + P(0,7)*SF[7] + P(1,7)*SF[6] + P(2,7)*SF[9] + P(10,7)*SF[15] - P(11,7)*SF[14] - (P(12,7)*q0)*0.5f + dt*(P(3,4) + P(0,4)*SF[7] + P(1,4)*SF[6] + P(2,4)*SF[9] + P(10,4)*SF[15] - P(11,4)*SF[14] - (P(12,4)*q0)*0.5f);
|
||||
nextP(4,7) = P(4,7) + P(0,7)*SF[5] + P(1,7)*SF[3] - P(3,7)*SF[4] + P(2,7)*SPP[0] + P(13,7)*SPP[3] + P(14,7)*SPP[6] - P(15,7)*SPP[9] + dt*(P(4,4) + P(0,4)*SF[5] + P(1,4)*SF[3] - P(3,4)*SF[4] + P(2,4)*SPP[0] + P(13,4)*SPP[3] + P(14,4)*SPP[6] - P(15,4)*SPP[9]);
|
||||
nextP(5,7) = P(5,7) + P(0,7)*SF[4] + P(2,7)*SF[3] + P(3,7)*SF[5] - P(1,7)*SPP[0] - P(13,7)*SPP[8] + P(14,7)*SPP[2] + P(15,7)*SPP[5] + dt*(P(5,4) + P(0,4)*SF[4] + P(2,4)*SF[3] + P(3,4)*SF[5] - P(1,4)*SPP[0] - P(13,4)*SPP[8] + P(14,4)*SPP[2] + P(15,4)*SPP[5]);
|
||||
nextP(6,7) = P(6,7) + P(1,7)*SF[4] - P(2,7)*SF[5] + P(3,7)*SF[3] + P(0,7)*SPP[0] + P(13,7)*SPP[4] - P(14,7)*SPP[7] - P(15,7)*SPP[1] + dt*(P(6,4) + P(1,4)*SF[4] - P(2,4)*SF[5] + P(3,4)*SF[3] + P(0,4)*SPP[0] + P(13,4)*SPP[4] - P(14,4)*SPP[7] - P(15,4)*SPP[1]);
|
||||
nextP(7,7) = P(7,7) + P(4,7)*dt + dt*(P(7,4) + P(4,4)*dt);
|
||||
nextP(0,8) = P(0,8) + P(1,8)*SF[9] + P(2,8)*SF[11] + P(3,8)*SF[10] + P(10,8)*SF[14] + P(11,8)*SF[15] + P(12,8)*SPP[10] + dt*(P(0,5) + P(1,5)*SF[9] + P(2,5)*SF[11] + P(3,5)*SF[10] + P(10,5)*SF[14] + P(11,5)*SF[15] + P(12,5)*SPP[10]);
|
||||
nextP(1,8) = P(1,8) + P(0,8)*SF[8] + P(2,8)*SF[7] + P(3,8)*SF[11] - P(12,8)*SF[15] + P(11,8)*SPP[10] - (P(10,8)*q0)*0.5f + dt*(P(1,5) + P(0,5)*SF[8] + P(2,5)*SF[7] + P(3,5)*SF[11] - P(12,5)*SF[15] + P(11,5)*SPP[10] - (P(10,5)*q0)*0.5f);
|
||||
nextP(2,8) = P(2,8) + P(0,8)*SF[6] + P(1,8)*SF[10] + P(3,8)*SF[8] + P(12,8)*SF[14] - P(10,8)*SPP[10] - (P(11,8)*q0)*0.5f + dt*(P(2,5) + P(0,5)*SF[6] + P(1,5)*SF[10] + P(3,5)*SF[8] + P(12,5)*SF[14] - P(10,5)*SPP[10] - (P(11,5)*q0)*0.5f);
|
||||
nextP(3,8) = P(3,8) + P(0,8)*SF[7] + P(1,8)*SF[6] + P(2,8)*SF[9] + P(10,8)*SF[15] - P(11,8)*SF[14] - (P(12,8)*q0)*0.5f + dt*(P(3,5) + P(0,5)*SF[7] + P(1,5)*SF[6] + P(2,5)*SF[9] + P(10,5)*SF[15] - P(11,5)*SF[14] - (P(12,5)*q0)*0.5f);
|
||||
nextP(4,8) = P(4,8) + P(0,8)*SF[5] + P(1,8)*SF[3] - P(3,8)*SF[4] + P(2,8)*SPP[0] + P(13,8)*SPP[3] + P(14,8)*SPP[6] - P(15,8)*SPP[9] + dt*(P(4,5) + P(0,5)*SF[5] + P(1,5)*SF[3] - P(3,5)*SF[4] + P(2,5)*SPP[0] + P(13,5)*SPP[3] + P(14,5)*SPP[6] - P(15,5)*SPP[9]);
|
||||
nextP(5,8) = P(5,8) + P(0,8)*SF[4] + P(2,8)*SF[3] + P(3,8)*SF[5] - P(1,8)*SPP[0] - P(13,8)*SPP[8] + P(14,8)*SPP[2] + P(15,8)*SPP[5] + dt*(P(5,5) + P(0,5)*SF[4] + P(2,5)*SF[3] + P(3,5)*SF[5] - P(1,5)*SPP[0] - P(13,5)*SPP[8] + P(14,5)*SPP[2] + P(15,5)*SPP[5]);
|
||||
nextP(6,8) = P(6,8) + P(1,8)*SF[4] - P(2,8)*SF[5] + P(3,8)*SF[3] + P(0,8)*SPP[0] + P(13,8)*SPP[4] - P(14,8)*SPP[7] - P(15,8)*SPP[1] + dt*(P(6,5) + P(1,5)*SF[4] - P(2,5)*SF[5] + P(3,5)*SF[3] + P(0,5)*SPP[0] + P(13,5)*SPP[4] - P(14,5)*SPP[7] - P(15,5)*SPP[1]);
|
||||
nextP(7,8) = P(7,8) + P(4,8)*dt + dt*(P(7,5) + P(4,5)*dt);
|
||||
nextP(8,8) = P(8,8) + P(5,8)*dt + dt*(P(8,5) + P(5,5)*dt);
|
||||
nextP(0,9) = P(0,9) + P(1,9)*SF[9] + P(2,9)*SF[11] + P(3,9)*SF[10] + P(10,9)*SF[14] + P(11,9)*SF[15] + P(12,9)*SPP[10] + dt*(P(0,6) + P(1,6)*SF[9] + P(2,6)*SF[11] + P(3,6)*SF[10] + P(10,6)*SF[14] + P(11,6)*SF[15] + P(12,6)*SPP[10]);
|
||||
nextP(1,9) = P(1,9) + P(0,9)*SF[8] + P(2,9)*SF[7] + P(3,9)*SF[11] - P(12,9)*SF[15] + P(11,9)*SPP[10] - (P(10,9)*q0)*0.5f + dt*(P(1,6) + P(0,6)*SF[8] + P(2,6)*SF[7] + P(3,6)*SF[11] - P(12,6)*SF[15] + P(11,6)*SPP[10] - (P(10,6)*q0)*0.5f);
|
||||
nextP(2,9) = P(2,9) + P(0,9)*SF[6] + P(1,9)*SF[10] + P(3,9)*SF[8] + P(12,9)*SF[14] - P(10,9)*SPP[10] - (P(11,9)*q0)*0.5f + dt*(P(2,6) + P(0,6)*SF[6] + P(1,6)*SF[10] + P(3,6)*SF[8] + P(12,6)*SF[14] - P(10,6)*SPP[10] - (P(11,6)*q0)*0.5f);
|
||||
nextP(3,9) = P(3,9) + P(0,9)*SF[7] + P(1,9)*SF[6] + P(2,9)*SF[9] + P(10,9)*SF[15] - P(11,9)*SF[14] - (P(12,9)*q0)*0.5f + dt*(P(3,6) + P(0,6)*SF[7] + P(1,6)*SF[6] + P(2,6)*SF[9] + P(10,6)*SF[15] - P(11,6)*SF[14] - (P(12,6)*q0)*0.5f);
|
||||
nextP(4,9) = P(4,9) + P(0,9)*SF[5] + P(1,9)*SF[3] - P(3,9)*SF[4] + P(2,9)*SPP[0] + P(13,9)*SPP[3] + P(14,9)*SPP[6] - P(15,9)*SPP[9] + dt*(P(4,6) + P(0,6)*SF[5] + P(1,6)*SF[3] - P(3,6)*SF[4] + P(2,6)*SPP[0] + P(13,6)*SPP[3] + P(14,6)*SPP[6] - P(15,6)*SPP[9]);
|
||||
nextP(5,9) = P(5,9) + P(0,9)*SF[4] + P(2,9)*SF[3] + P(3,9)*SF[5] - P(1,9)*SPP[0] - P(13,9)*SPP[8] + P(14,9)*SPP[2] + P(15,9)*SPP[5] + dt*(P(5,6) + P(0,6)*SF[4] + P(2,6)*SF[3] + P(3,6)*SF[5] - P(1,6)*SPP[0] - P(13,6)*SPP[8] + P(14,6)*SPP[2] + P(15,6)*SPP[5]);
|
||||
nextP(6,9) = P(6,9) + P(1,9)*SF[4] - P(2,9)*SF[5] + P(3,9)*SF[3] + P(0,9)*SPP[0] + P(13,9)*SPP[4] - P(14,9)*SPP[7] - P(15,9)*SPP[1] + dt*(P(6,6) + P(1,6)*SF[4] - P(2,6)*SF[5] + P(3,6)*SF[3] + P(0,6)*SPP[0] + P(13,6)*SPP[4] - P(14,6)*SPP[7] - P(15,6)*SPP[1]);
|
||||
nextP(7,9) = P(7,9) + P(4,9)*dt + dt*(P(7,6) + P(4,6)*dt);
|
||||
nextP(8,9) = P(8,9) + P(5,9)*dt + dt*(P(8,6) + P(5,6)*dt);
|
||||
nextP(9,9) = P(9,9) + P(6,9)*dt + dt*(P(9,6) + P(6,6)*dt);
|
||||
nextP(0,10) = P(0,10) + P(1,10)*SF[9] + P(2,10)*SF[11] + P(3,10)*SF[10] + P(10,10)*SF[14] + P(11,10)*SF[15] + P(12,10)*SPP[10];
|
||||
nextP(1,10) = P(1,10) + P(0,10)*SF[8] + P(2,10)*SF[7] + P(3,10)*SF[11] - P(12,10)*SF[15] + P(11,10)*SPP[10] - (P(10,10)*q0)*0.5f;
|
||||
nextP(2,10) = P(2,10) + P(0,10)*SF[6] + P(1,10)*SF[10] + P(3,10)*SF[8] + P(12,10)*SF[14] - P(10,10)*SPP[10] - (P(11,10)*q0)*0.5f;
|
||||
nextP(3,10) = P(3,10) + P(0,10)*SF[7] + P(1,10)*SF[6] + P(2,10)*SF[9] + P(10,10)*SF[15] - P(11,10)*SF[14] - (P(12,10)*q0)*0.5f;
|
||||
nextP(4,10) = P(4,10) + P(0,10)*SF[5] + P(1,10)*SF[3] - P(3,10)*SF[4] + P(2,10)*SPP[0] + P(13,10)*SPP[3] + P(14,10)*SPP[6] - P(15,10)*SPP[9];
|
||||
nextP(5,10) = P(5,10) + P(0,10)*SF[4] + P(2,10)*SF[3] + P(3,10)*SF[5] - P(1,10)*SPP[0] - P(13,10)*SPP[8] + P(14,10)*SPP[2] + P(15,10)*SPP[5];
|
||||
nextP(6,10) = P(6,10) + P(1,10)*SF[4] - P(2,10)*SF[5] + P(3,10)*SF[3] + P(0,10)*SPP[0] + P(13,10)*SPP[4] - P(14,10)*SPP[7] - P(15,10)*SPP[1];
|
||||
nextP(7,10) = P(7,10) + P(4,10)*dt;
|
||||
nextP(8,10) = P(8,10) + P(5,10)*dt;
|
||||
nextP(9,10) = P(9,10) + P(6,10)*dt;
|
||||
nextP(10,10) = P(10,10);
|
||||
nextP(0,11) = P(0,11) + P(1,11)*SF[9] + P(2,11)*SF[11] + P(3,11)*SF[10] + P(10,11)*SF[14] + P(11,11)*SF[15] + P(12,11)*SPP[10];
|
||||
nextP(1,11) = P(1,11) + P(0,11)*SF[8] + P(2,11)*SF[7] + P(3,11)*SF[11] - P(12,11)*SF[15] + P(11,11)*SPP[10] - (P(10,11)*q0)*0.5f;
|
||||
nextP(2,11) = P(2,11) + P(0,11)*SF[6] + P(1,11)*SF[10] + P(3,11)*SF[8] + P(12,11)*SF[14] - P(10,11)*SPP[10] - (P(11,11)*q0)*0.5f;
|
||||
nextP(3,11) = P(3,11) + P(0,11)*SF[7] + P(1,11)*SF[6] + P(2,11)*SF[9] + P(10,11)*SF[15] - P(11,11)*SF[14] - (P(12,11)*q0)*0.5f;
|
||||
nextP(4,11) = P(4,11) + P(0,11)*SF[5] + P(1,11)*SF[3] - P(3,11)*SF[4] + P(2,11)*SPP[0] + P(13,11)*SPP[3] + P(14,11)*SPP[6] - P(15,11)*SPP[9];
|
||||
nextP(5,11) = P(5,11) + P(0,11)*SF[4] + P(2,11)*SF[3] + P(3,11)*SF[5] - P(1,11)*SPP[0] - P(13,11)*SPP[8] + P(14,11)*SPP[2] + P(15,11)*SPP[5];
|
||||
nextP(6,11) = P(6,11) + P(1,11)*SF[4] - P(2,11)*SF[5] + P(3,11)*SF[3] + P(0,11)*SPP[0] + P(13,11)*SPP[4] - P(14,11)*SPP[7] - P(15,11)*SPP[1];
|
||||
nextP(7,11) = P(7,11) + P(4,11)*dt;
|
||||
nextP(8,11) = P(8,11) + P(5,11)*dt;
|
||||
nextP(9,11) = P(9,11) + P(6,11)*dt;
|
||||
nextP(10,11) = P(10,11);
|
||||
nextP(11,11) = P(11,11);
|
||||
nextP(0,12) = P(0,12) + P(1,12)*SF[9] + P(2,12)*SF[11] + P(3,12)*SF[10] + P(10,12)*SF[14] + P(11,12)*SF[15] + P(12,12)*SPP[10];
|
||||
nextP(1,12) = P(1,12) + P(0,12)*SF[8] + P(2,12)*SF[7] + P(3,12)*SF[11] - P(12,12)*SF[15] + P(11,12)*SPP[10] - (P(10,12)*q0)*0.5f;
|
||||
nextP(2,12) = P(2,12) + P(0,12)*SF[6] + P(1,12)*SF[10] + P(3,12)*SF[8] + P(12,12)*SF[14] - P(10,12)*SPP[10] - (P(11,12)*q0)*0.5f;
|
||||
nextP(3,12) = P(3,12) + P(0,12)*SF[7] + P(1,12)*SF[6] + P(2,12)*SF[9] + P(10,12)*SF[15] - P(11,12)*SF[14] - (P(12,12)*q0)*0.5f;
|
||||
nextP(4,12) = P(4,12) + P(0,12)*SF[5] + P(1,12)*SF[3] - P(3,12)*SF[4] + P(2,12)*SPP[0] + P(13,12)*SPP[3] + P(14,12)*SPP[6] - P(15,12)*SPP[9];
|
||||
nextP(5,12) = P(5,12) + P(0,12)*SF[4] + P(2,12)*SF[3] + P(3,12)*SF[5] - P(1,12)*SPP[0] - P(13,12)*SPP[8] + P(14,12)*SPP[2] + P(15,12)*SPP[5];
|
||||
nextP(6,12) = P(6,12) + P(1,12)*SF[4] - P(2,12)*SF[5] + P(3,12)*SF[3] + P(0,12)*SPP[0] + P(13,12)*SPP[4] - P(14,12)*SPP[7] - P(15,12)*SPP[1];
|
||||
nextP(7,12) = P(7,12) + P(4,12)*dt;
|
||||
nextP(8,12) = P(8,12) + P(5,12)*dt;
|
||||
nextP(9,12) = P(9,12) + P(6,12)*dt;
|
||||
nextP(10,12) = P(10,12);
|
||||
nextP(11,12) = P(11,12);
|
||||
nextP(12,12) = P(12,12);
|
||||
|
||||
for (unsigned i = 13; i <= 15; i++) {
|
||||
nextP(0,i) = P(0,i) + P(1,i)*SF[9] + P(2,i)*SF[11] + P(3,i)*SF[10] + P(10,i)*SF[14] + P(11,i)*SF[15] + P(12,i)*SPP[10];
|
||||
nextP(1,i) = P(1,i) + P(0,i)*SF[8] + P(2,i)*SF[7] + P(3,i)*SF[11] - P(12,i)*SF[15] + P(11,i)*SPP[10] - (P(10,i)*q0)*0.5f;
|
||||
nextP(2,i) = P(2,i) + P(0,i)*SF[6] + P(1,i)*SF[10] + P(3,i)*SF[8] + P(12,i)*SF[14] - P(10,i)*SPP[10] - (P(11,i)*q0)*0.5f;
|
||||
nextP(3,i) = P(3,i) + P(0,i)*SF[7] + P(1,i)*SF[6] + P(2,i)*SF[9] + P(10,i)*SF[15] - P(11,i)*SF[14] - (P(12,i)*q0)*0.5f;
|
||||
nextP(4,i) = P(4,i) + P(0,i)*SF[5] + P(1,i)*SF[3] - P(3,i)*SF[4] + P(2,i)*SPP[0] + P(13,i)*SPP[3] + P(14,i)*SPP[6] - P(15,i)*SPP[9];
|
||||
nextP(5,i) = P(5,i) + P(0,i)*SF[4] + P(2,i)*SF[3] + P(3,i)*SF[5] - P(1,i)*SPP[0] - P(13,i)*SPP[8] + P(14,i)*SPP[2] + P(15,i)*SPP[5];
|
||||
nextP(6,i) = P(6,i) + P(1,i)*SF[4] - P(2,i)*SF[5] + P(3,i)*SF[3] + P(0,i)*SPP[0] + P(13,i)*SPP[4] - P(14,i)*SPP[7] - P(15,i)*SPP[1];
|
||||
nextP(7,i) = P(7,i) + P(4,i)*dt;
|
||||
nextP(8,i) = P(8,i) + P(5,i)*dt;
|
||||
nextP(9,i) = P(9,i) + P(6,i)*dt;
|
||||
nextP(10,i) = P(10,i);
|
||||
nextP(11,i) = P(11,i);
|
||||
nextP(12,i) = P(12,i);
|
||||
nextP(13,i) = P(13,i);
|
||||
|
||||
if (i > 13) {
|
||||
nextP(14,i) = P(14,i);
|
||||
}
|
||||
|
||||
if (i > 14) {
|
||||
nextP(15,i) = P(15,i);
|
||||
}
|
||||
}
|
||||
|
||||
nextP(0,16) = P(0,16) + P(1,16)*SF[9] + P(2,16)*SF[11] + P(3,16)*SF[10] + P(10,16)*SF[14] + P(11,16)*SF[15] + P(12,16)*SPP[10];
|
||||
nextP(1,16) = P(1,16) + P(0,16)*SF[8] + P(2,16)*SF[7] + P(3,16)*SF[11] - P(12,16)*SF[15] + P(11,16)*SPP[10] - (P(10,16)*q0)*0.5f;
|
||||
nextP(2,16) = P(2,16) + P(0,16)*SF[6] + P(1,16)*SF[10] + P(3,16)*SF[8] + P(12,16)*SF[14] - P(10,16)*SPP[10] - (P(11,16)*q0)*0.5f;
|
||||
nextP(3,16) = P(3,16) + P(0,16)*SF[7] + P(1,16)*SF[6] + P(2,16)*SF[9] + P(10,16)*SF[15] - P(11,16)*SF[14] - (P(12,16)*q0)*0.5f;
|
||||
nextP(4,16) = P(4,16) + P(0,16)*SF[5] + P(1,16)*SF[3] - P(3,16)*SF[4] + P(2,16)*SPP[0] + P(13,16)*SPP[3] + P(14,16)*SPP[6] - P(15,16)*SPP[9];
|
||||
nextP(5,16) = P(5,16) + P(0,16)*SF[4] + P(2,16)*SF[3] + P(3,16)*SF[5] - P(1,16)*SPP[0] - P(13,16)*SPP[8] + P(14,16)*SPP[2] + P(15,16)*SPP[5];
|
||||
nextP(6,16) = P(6,16) + P(1,16)*SF[4] - P(2,16)*SF[5] + P(3,16)*SF[3] + P(0,16)*SPP[0] + P(13,16)*SPP[4] - P(14,16)*SPP[7] - P(15,16)*SPP[1];
|
||||
nextP(7,16) = P(7,16) + P(4,16)*dt;
|
||||
nextP(8,16) = P(8,16) + P(5,16)*dt;
|
||||
nextP(9,16) = P(9,16) + P(6,16)*dt;
|
||||
nextP(10,16) = P(10,16);
|
||||
nextP(11,16) = P(11,16);
|
||||
nextP(12,16) = P(12,16);
|
||||
nextP(13,16) = P(13,16);
|
||||
nextP(14,16) = P(14,16);
|
||||
nextP(15,16) = P(15,16);
|
||||
nextP(16,16) = P(16,16);
|
||||
nextP(0,17) = P(0,17) + P(1,17)*SF[9] + P(2,17)*SF[11] + P(3,17)*SF[10] + P(10,17)*SF[14] + P(11,17)*SF[15] + P(12,17)*SPP[10];
|
||||
nextP(1,17) = P(1,17) + P(0,17)*SF[8] + P(2,17)*SF[7] + P(3,17)*SF[11] - P(12,17)*SF[15] + P(11,17)*SPP[10] - (P(10,17)*q0)*0.5f;
|
||||
nextP(2,17) = P(2,17) + P(0,17)*SF[6] + P(1,17)*SF[10] + P(3,17)*SF[8] + P(12,17)*SF[14] - P(10,17)*SPP[10] - (P(11,17)*q0)*0.5f;
|
||||
nextP(3,17) = P(3,17) + P(0,17)*SF[7] + P(1,17)*SF[6] + P(2,17)*SF[9] + P(10,17)*SF[15] - P(11,17)*SF[14] - (P(12,17)*q0)*0.5f;
|
||||
nextP(4,17) = P(4,17) + P(0,17)*SF[5] + P(1,17)*SF[3] - P(3,17)*SF[4] + P(2,17)*SPP[0] + P(13,17)*SPP[3] + P(14,17)*SPP[6] - P(15,17)*SPP[9];
|
||||
nextP(5,17) = P(5,17) + P(0,17)*SF[4] + P(2,17)*SF[3] + P(3,17)*SF[5] - P(1,17)*SPP[0] - P(13,17)*SPP[8] + P(14,17)*SPP[2] + P(15,17)*SPP[5];
|
||||
nextP(6,17) = P(6,17) + P(1,17)*SF[4] - P(2,17)*SF[5] + P(3,17)*SF[3] + P(0,17)*SPP[0] + P(13,17)*SPP[4] - P(14,17)*SPP[7] - P(15,17)*SPP[1];
|
||||
nextP(7,17) = P(7,17) + P(4,17)*dt;
|
||||
nextP(8,17) = P(8,17) + P(5,17)*dt;
|
||||
nextP(9,17) = P(9,17) + P(6,17)*dt;
|
||||
nextP(10,17) = P(10,17);
|
||||
nextP(11,17) = P(11,17);
|
||||
nextP(12,17) = P(12,17);
|
||||
nextP(13,17) = P(13,17);
|
||||
nextP(14,17) = P(14,17);
|
||||
nextP(15,17) = P(15,17);
|
||||
nextP(16,17) = P(16,17);
|
||||
nextP(17,17) = P(17,17);
|
||||
nextP(0,18) = P(0,18) + P(1,18)*SF[9] + P(2,18)*SF[11] + P(3,18)*SF[10] + P(10,18)*SF[14] + P(11,18)*SF[15] + P(12,18)*SPP[10];
|
||||
nextP(1,18) = P(1,18) + P(0,18)*SF[8] + P(2,18)*SF[7] + P(3,18)*SF[11] - P(12,18)*SF[15] + P(11,18)*SPP[10] - (P(10,18)*q0)*0.5f;
|
||||
nextP(2,18) = P(2,18) + P(0,18)*SF[6] + P(1,18)*SF[10] + P(3,18)*SF[8] + P(12,18)*SF[14] - P(10,18)*SPP[10] - (P(11,18)*q0)*0.5f;
|
||||
nextP(3,18) = P(3,18) + P(0,18)*SF[7] + P(1,18)*SF[6] + P(2,18)*SF[9] + P(10,18)*SF[15] - P(11,18)*SF[14] - (P(12,18)*q0)*0.5f;
|
||||
nextP(4,18) = P(4,18) + P(0,18)*SF[5] + P(1,18)*SF[3] - P(3,18)*SF[4] + P(2,18)*SPP[0] + P(13,18)*SPP[3] + P(14,18)*SPP[6] - P(15,18)*SPP[9];
|
||||
nextP(5,18) = P(5,18) + P(0,18)*SF[4] + P(2,18)*SF[3] + P(3,18)*SF[5] - P(1,18)*SPP[0] - P(13,18)*SPP[8] + P(14,18)*SPP[2] + P(15,18)*SPP[5];
|
||||
nextP(6,18) = P(6,18) + P(1,18)*SF[4] - P(2,18)*SF[5] + P(3,18)*SF[3] + P(0,18)*SPP[0] + P(13,18)*SPP[4] - P(14,18)*SPP[7] - P(15,18)*SPP[1];
|
||||
nextP(7,18) = P(7,18) + P(4,18)*dt;
|
||||
nextP(8,18) = P(8,18) + P(5,18)*dt;
|
||||
nextP(9,18) = P(9,18) + P(6,18)*dt;
|
||||
nextP(10,18) = P(10,18);
|
||||
nextP(11,18) = P(11,18);
|
||||
nextP(12,18) = P(12,18);
|
||||
nextP(13,18) = P(13,18);
|
||||
nextP(14,18) = P(14,18);
|
||||
nextP(15,18) = P(15,18);
|
||||
nextP(16,18) = P(16,18);
|
||||
nextP(17,18) = P(17,18);
|
||||
nextP(18,18) = P(18,18);
|
||||
nextP(0,19) = P(0,19) + P(1,19)*SF[9] + P(2,19)*SF[11] + P(3,19)*SF[10] + P(10,19)*SF[14] + P(11,19)*SF[15] + P(12,19)*SPP[10];
|
||||
nextP(1,19) = P(1,19) + P(0,19)*SF[8] + P(2,19)*SF[7] + P(3,19)*SF[11] - P(12,19)*SF[15] + P(11,19)*SPP[10] - (P(10,19)*q0)*0.5f;
|
||||
nextP(2,19) = P(2,19) + P(0,19)*SF[6] + P(1,19)*SF[10] + P(3,19)*SF[8] + P(12,19)*SF[14] - P(10,19)*SPP[10] - (P(11,19)*q0)*0.5f;
|
||||
nextP(3,19) = P(3,19) + P(0,19)*SF[7] + P(1,19)*SF[6] + P(2,19)*SF[9] + P(10,19)*SF[15] - P(11,19)*SF[14] - (P(12,19)*q0)*0.5f;
|
||||
nextP(4,19) = P(4,19) + P(0,19)*SF[5] + P(1,19)*SF[3] - P(3,19)*SF[4] + P(2,19)*SPP[0] + P(13,19)*SPP[3] + P(14,19)*SPP[6] - P(15,19)*SPP[9];
|
||||
nextP(5,19) = P(5,19) + P(0,19)*SF[4] + P(2,19)*SF[3] + P(3,19)*SF[5] - P(1,19)*SPP[0] - P(13,19)*SPP[8] + P(14,19)*SPP[2] + P(15,19)*SPP[5];
|
||||
nextP(6,19) = P(6,19) + P(1,19)*SF[4] - P(2,19)*SF[5] + P(3,19)*SF[3] + P(0,19)*SPP[0] + P(13,19)*SPP[4] - P(14,19)*SPP[7] - P(15,19)*SPP[1];
|
||||
nextP(7,19) = P(7,19) + P(4,19)*dt;
|
||||
nextP(8,19) = P(8,19) + P(5,19)*dt;
|
||||
nextP(9,19) = P(9,19) + P(6,19)*dt;
|
||||
nextP(10,19) = P(10,19);
|
||||
nextP(11,19) = P(11,19);
|
||||
nextP(12,19) = P(12,19);
|
||||
nextP(13,19) = P(13,19);
|
||||
nextP(14,19) = P(14,19);
|
||||
nextP(15,19) = P(15,19);
|
||||
nextP(16,19) = P(16,19);
|
||||
nextP(17,19) = P(17,19);
|
||||
nextP(18,19) = P(18,19);
|
||||
nextP(19,19) = P(19,19);
|
||||
nextP(0,20) = P(0,20) + P(1,20)*SF[9] + P(2,20)*SF[11] + P(3,20)*SF[10] + P(10,20)*SF[14] + P(11,20)*SF[15] + P(12,20)*SPP[10];
|
||||
nextP(1,20) = P(1,20) + P(0,20)*SF[8] + P(2,20)*SF[7] + P(3,20)*SF[11] - P(12,20)*SF[15] + P(11,20)*SPP[10] - (P(10,20)*q0)*0.5f;
|
||||
nextP(2,20) = P(2,20) + P(0,20)*SF[6] + P(1,20)*SF[10] + P(3,20)*SF[8] + P(12,20)*SF[14] - P(10,20)*SPP[10] - (P(11,20)*q0)*0.5f;
|
||||
nextP(3,20) = P(3,20) + P(0,20)*SF[7] + P(1,20)*SF[6] + P(2,20)*SF[9] + P(10,20)*SF[15] - P(11,20)*SF[14] - (P(12,20)*q0)*0.5f;
|
||||
nextP(4,20) = P(4,20) + P(0,20)*SF[5] + P(1,20)*SF[3] - P(3,20)*SF[4] + P(2,20)*SPP[0] + P(13,20)*SPP[3] + P(14,20)*SPP[6] - P(15,20)*SPP[9];
|
||||
nextP(5,20) = P(5,20) + P(0,20)*SF[4] + P(2,20)*SF[3] + P(3,20)*SF[5] - P(1,20)*SPP[0] - P(13,20)*SPP[8] + P(14,20)*SPP[2] + P(15,20)*SPP[5];
|
||||
nextP(6,20) = P(6,20) + P(1,20)*SF[4] - P(2,20)*SF[5] + P(3,20)*SF[3] + P(0,20)*SPP[0] + P(13,20)*SPP[4] - P(14,20)*SPP[7] - P(15,20)*SPP[1];
|
||||
nextP(7,20) = P(7,20) + P(4,20)*dt;
|
||||
nextP(8,20) = P(8,20) + P(5,20)*dt;
|
||||
nextP(9,20) = P(9,20) + P(6,20)*dt;
|
||||
nextP(10,20) = P(10,20);
|
||||
nextP(11,20) = P(11,20);
|
||||
nextP(12,20) = P(12,20);
|
||||
nextP(13,20) = P(13,20);
|
||||
nextP(14,20) = P(14,20);
|
||||
nextP(15,20) = P(15,20);
|
||||
nextP(16,20) = P(16,20);
|
||||
nextP(17,20) = P(17,20);
|
||||
nextP(18,20) = P(18,20);
|
||||
nextP(19,20) = P(19,20);
|
||||
nextP(20,20) = P(20,20);
|
||||
nextP(0,21) = P(0,21) + P(1,21)*SF[9] + P(2,21)*SF[11] + P(3,21)*SF[10] + P(10,21)*SF[14] + P(11,21)*SF[15] + P(12,21)*SPP[10];
|
||||
nextP(1,21) = P(1,21) + P(0,21)*SF[8] + P(2,21)*SF[7] + P(3,21)*SF[11] - P(12,21)*SF[15] + P(11,21)*SPP[10] - (P(10,21)*q0)*0.5f;
|
||||
nextP(2,21) = P(2,21) + P(0,21)*SF[6] + P(1,21)*SF[10] + P(3,21)*SF[8] + P(12,21)*SF[14] - P(10,21)*SPP[10] - (P(11,21)*q0)*0.5f;
|
||||
nextP(3,21) = P(3,21) + P(0,21)*SF[7] + P(1,21)*SF[6] + P(2,21)*SF[9] + P(10,21)*SF[15] - P(11,21)*SF[14] - (P(12,21)*q0)*0.5f;
|
||||
nextP(4,21) = P(4,21) + P(0,21)*SF[5] + P(1,21)*SF[3] - P(3,21)*SF[4] + P(2,21)*SPP[0] + P(13,21)*SPP[3] + P(14,21)*SPP[6] - P(15,21)*SPP[9];
|
||||
nextP(5,21) = P(5,21) + P(0,21)*SF[4] + P(2,21)*SF[3] + P(3,21)*SF[5] - P(1,21)*SPP[0] - P(13,21)*SPP[8] + P(14,21)*SPP[2] + P(15,21)*SPP[5];
|
||||
nextP(6,21) = P(6,21) + P(1,21)*SF[4] - P(2,21)*SF[5] + P(3,21)*SF[3] + P(0,21)*SPP[0] + P(13,21)*SPP[4] - P(14,21)*SPP[7] - P(15,21)*SPP[1];
|
||||
nextP(7,21) = P(7,21) + P(4,21)*dt;
|
||||
nextP(8,21) = P(8,21) + P(5,21)*dt;
|
||||
nextP(9,21) = P(9,21) + P(6,21)*dt;
|
||||
nextP(10,21) = P(10,21);
|
||||
nextP(11,21) = P(11,21);
|
||||
nextP(12,21) = P(12,21);
|
||||
nextP(13,21) = P(13,21);
|
||||
nextP(14,21) = P(14,21);
|
||||
nextP(15,21) = P(15,21);
|
||||
nextP(16,21) = P(16,21);
|
||||
nextP(17,21) = P(17,21);
|
||||
nextP(18,21) = P(18,21);
|
||||
nextP(19,21) = P(19,21);
|
||||
nextP(20,21) = P(20,21);
|
||||
nextP(21,21) = P(21,21);
|
||||
nextP(0,22) = P(0,22) + P(1,22)*SF[9] + P(2,22)*SF[11] + P(3,22)*SF[10] + P(10,22)*SF[14] + P(11,22)*SF[15] + P(12,22)*SPP[10];
|
||||
nextP(1,22) = P(1,22) + P(0,22)*SF[8] + P(2,22)*SF[7] + P(3,22)*SF[11] - P(12,22)*SF[15] + P(11,22)*SPP[10] - (P(10,22)*q0)*0.5f;
|
||||
nextP(2,22) = P(2,22) + P(0,22)*SF[6] + P(1,22)*SF[10] + P(3,22)*SF[8] + P(12,22)*SF[14] - P(10,22)*SPP[10] - (P(11,22)*q0)*0.5f;
|
||||
nextP(3,22) = P(3,22) + P(0,22)*SF[7] + P(1,22)*SF[6] + P(2,22)*SF[9] + P(10,22)*SF[15] - P(11,22)*SF[14] - (P(12,22)*q0)*0.5f;
|
||||
nextP(4,22) = P(4,22) + P(0,22)*SF[5] + P(1,22)*SF[3] - P(3,22)*SF[4] + P(2,22)*SPP[0] + P(13,22)*SPP[3] + P(14,22)*SPP[6] - P(15,22)*SPP[9];
|
||||
nextP(5,22) = P(5,22) + P(0,22)*SF[4] + P(2,22)*SF[3] + P(3,22)*SF[5] - P(1,22)*SPP[0] - P(13,22)*SPP[8] + P(14,22)*SPP[2] + P(15,22)*SPP[5];
|
||||
nextP(6,22) = P(6,22) + P(1,22)*SF[4] - P(2,22)*SF[5] + P(3,22)*SF[3] + P(0,22)*SPP[0] + P(13,22)*SPP[4] - P(14,22)*SPP[7] - P(15,22)*SPP[1];
|
||||
nextP(7,22) = P(7,22) + P(4,22)*dt;
|
||||
nextP(8,22) = P(8,22) + P(5,22)*dt;
|
||||
nextP(9,22) = P(9,22) + P(6,22)*dt;
|
||||
nextP(10,22) = P(10,22);
|
||||
nextP(11,22) = P(11,22);
|
||||
nextP(12,22) = P(12,22);
|
||||
nextP(13,22) = P(13,22);
|
||||
nextP(14,22) = P(14,22);
|
||||
nextP(15,22) = P(15,22);
|
||||
nextP(16,22) = P(16,22);
|
||||
nextP(17,22) = P(17,22);
|
||||
nextP(18,22) = P(18,22);
|
||||
nextP(19,22) = P(19,22);
|
||||
nextP(20,22) = P(20,22);
|
||||
nextP(21,22) = P(21,22);
|
||||
nextP(22,22) = P(22,22);
|
||||
nextP(0,23) = P(0,23) + P(1,23)*SF[9] + P(2,23)*SF[11] + P(3,23)*SF[10] + P(10,23)*SF[14] + P(11,23)*SF[15] + P(12,23)*SPP[10];
|
||||
nextP(1,23) = P(1,23) + P(0,23)*SF[8] + P(2,23)*SF[7] + P(3,23)*SF[11] - P(12,23)*SF[15] + P(11,23)*SPP[10] - (P(10,23)*q0)*0.5f;
|
||||
nextP(2,23) = P(2,23) + P(0,23)*SF[6] + P(1,23)*SF[10] + P(3,23)*SF[8] + P(12,23)*SF[14] - P(10,23)*SPP[10] - (P(11,23)*q0)*0.5f;
|
||||
nextP(3,23) = P(3,23) + P(0,23)*SF[7] + P(1,23)*SF[6] + P(2,23)*SF[9] + P(10,23)*SF[15] - P(11,23)*SF[14] - (P(12,23)*q0)*0.5f;
|
||||
nextP(4,23) = P(4,23) + P(0,23)*SF[5] + P(1,23)*SF[3] - P(3,23)*SF[4] + P(2,23)*SPP[0] + P(13,23)*SPP[3] + P(14,23)*SPP[6] - P(15,23)*SPP[9];
|
||||
nextP(5,23) = P(5,23) + P(0,23)*SF[4] + P(2,23)*SF[3] + P(3,23)*SF[5] - P(1,23)*SPP[0] - P(13,23)*SPP[8] + P(14,23)*SPP[2] + P(15,23)*SPP[5];
|
||||
nextP(6,23) = P(6,23) + P(1,23)*SF[4] - P(2,23)*SF[5] + P(3,23)*SF[3] + P(0,23)*SPP[0] + P(13,23)*SPP[4] - P(14,23)*SPP[7] - P(15,23)*SPP[1];
|
||||
nextP(7,23) = P(7,23) + P(4,23)*dt;
|
||||
nextP(8,23) = P(8,23) + P(5,23)*dt;
|
||||
nextP(9,23) = P(9,23) + P(6,23)*dt;
|
||||
nextP(10,23) = P(10,23);
|
||||
nextP(11,23) = P(11,23);
|
||||
nextP(12,23) = P(12,23);
|
||||
nextP(13,23) = P(13,23);
|
||||
nextP(14,23) = P(14,23);
|
||||
nextP(15,23) = P(15,23);
|
||||
nextP(16,23) = P(16,23);
|
||||
nextP(17,23) = P(17,23);
|
||||
nextP(18,23) = P(18,23);
|
||||
nextP(19,23) = P(19,23);
|
||||
nextP(20,23) = P(20,23);
|
||||
nextP(21,23) = P(21,23);
|
||||
nextP(22,23) = P(22,23);
|
||||
nextP(23,23) = P(23,23);
|
||||
|
||||
// capture largest difference
|
||||
float max_diff_fraction = 0.0f;
|
||||
int max_row, max_col;
|
||||
float max_old, max_new;
|
||||
|
||||
for (int col=0; col<=23; col++) {
|
||||
for (int row=0; row<=col; row++) {
|
||||
float diff_fraction = fabsf(nextP_sympy(row,col)-nextP(row,col)) / fabsf(nextP(row,col));
|
||||
if (diff_fraction > max_diff_fraction) {
|
||||
max_diff_fraction = diff_fraction;
|
||||
max_row = row;
|
||||
max_col = col;
|
||||
max_old = nextP(row,col);
|
||||
max_new = nextP_sympy(row,col);
|
||||
}
|
||||
}
|
||||
}
|
||||
if (max_diff_fraction > 5E-5f) {
|
||||
printf("Fail: Covariance Prediction max diff fraction = %e , old = %e , new = %e , location index = %i,%i\n",max_diff_fraction, max_old, max_new, max_row, max_col);
|
||||
} else {
|
||||
printf("Pass: Covariance Prediction max diff fraction = %e , old = %e , new = %e , location index = %i,%i\n",max_diff_fraction, max_old, max_new, max_row, max_col);
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
+640
@@ -0,0 +1,640 @@
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <cstdlib>
|
||||
#include "../../../../../matrix/matrix/math.hpp"
|
||||
#include "util.h"
|
||||
|
||||
typedef matrix::Vector<float, 24> Vector24f;
|
||||
typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
|
||||
|
||||
template<int ... Idxs>
|
||||
using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
|
||||
|
||||
int main()
|
||||
{
|
||||
// Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations
|
||||
|
||||
SparseVector24f<0,1,2,3,4,5,6> Hfusion; // Optical flow observation Jacobians
|
||||
Vector24f Kfusion; // Optical flow observation Kalman gains
|
||||
|
||||
Vector24f Hfusion_sympy;
|
||||
Vector24f Kfusion_sympy;
|
||||
|
||||
Vector24f Hfusion_matlab;
|
||||
Vector24f Kfusion_matlab;
|
||||
|
||||
const float R_LOS = sq(0.15f);
|
||||
|
||||
float flow_innov_var;
|
||||
|
||||
// quaternion inputs must be normalised
|
||||
float q0 = 2.0f * ((float)rand() - 0.5f);
|
||||
float q1 = 2.0f * ((float)rand() - 0.5f);
|
||||
float q2 = 2.0f * ((float)rand() - 0.5f);
|
||||
float q3 = 2.0f * ((float)rand() - 0.5f);
|
||||
const float length = sqrtf(sq(q0) + sq(q1) + sq(q2) + sq(q3));
|
||||
q0 /= length;
|
||||
q1 /= length;
|
||||
q2 /= length;
|
||||
q3 /= length;
|
||||
|
||||
// get latest velocity in earth frame
|
||||
const float vn = 10.0f * 2.0f * ((float)rand() - 0.5f);
|
||||
const float ve = 10.0f * 2.0f * ((float)rand() - 0.5f);
|
||||
const float vd = 2.0f * ((float)rand() - 0.5f);
|
||||
|
||||
const float range = 5.0f;
|
||||
|
||||
matrix::Dcmf Tbs;
|
||||
Tbs.identity();
|
||||
|
||||
// create a symmetrical positive definite 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// evaluate sub expressions used by sympy code
|
||||
const float HK0 = -Tbs(1,0)*q2 + Tbs(1,1)*q1 + Tbs(1,2)*q0;
|
||||
const float HK1 = Tbs(1,0)*q3 + Tbs(1,1)*q0 - Tbs(1,2)*q1;
|
||||
const float HK2 = Tbs(1,0)*q0 - Tbs(1,1)*q3 + Tbs(1,2)*q2;
|
||||
const float HK3 = HK0*vd + HK1*ve + HK2*vn;
|
||||
const float HK4 = 1.0F/range;
|
||||
const float HK5 = 2*HK4;
|
||||
const float HK6 = Tbs(1,0)*q1 + Tbs(1,1)*q2 + Tbs(1,2)*q3;
|
||||
const float HK7 = -HK0*ve + HK1*vd + HK6*vn;
|
||||
const float HK8 = HK0*vn - HK2*vd + HK6*ve;
|
||||
const float HK9 = -HK1*vn + HK2*ve + HK6*vd;
|
||||
const float HK10 = q0*q2;
|
||||
const float HK11 = q1*q3;
|
||||
const float HK12 = HK10 + HK11;
|
||||
const float HK13 = 2*Tbs(1,2);
|
||||
const float HK14 = q0*q3;
|
||||
const float HK15 = q1*q2;
|
||||
const float HK16 = HK14 - HK15;
|
||||
const float HK17 = 2*Tbs(1,1);
|
||||
const float HK18 = ecl::powf(q1, 2);
|
||||
const float HK19 = ecl::powf(q2, 2);
|
||||
const float HK20 = -HK19;
|
||||
const float HK21 = ecl::powf(q0, 2);
|
||||
const float HK22 = ecl::powf(q3, 2);
|
||||
const float HK23 = HK21 - HK22;
|
||||
const float HK24 = HK18 + HK20 + HK23;
|
||||
const float HK25 = HK12*HK13 - HK16*HK17 + HK24*Tbs(1,0);
|
||||
const float HK26 = HK14 + HK15;
|
||||
const float HK27 = 2*Tbs(1,0);
|
||||
const float HK28 = q0*q1;
|
||||
const float HK29 = q2*q3;
|
||||
const float HK30 = HK28 - HK29;
|
||||
const float HK31 = -HK18;
|
||||
const float HK32 = HK19 + HK23 + HK31;
|
||||
const float HK33 = -HK13*HK30 + HK26*HK27 + HK32*Tbs(1,1);
|
||||
const float HK34 = HK28 + HK29;
|
||||
const float HK35 = HK10 - HK11;
|
||||
const float HK36 = HK20 + HK21 + HK22 + HK31;
|
||||
const float HK37 = HK17*HK34 - HK27*HK35 + HK36*Tbs(1,2);
|
||||
const float HK38 = 2*HK3;
|
||||
const float HK39 = 2*HK7;
|
||||
const float HK40 = 2*HK8;
|
||||
const float HK41 = 2*HK9;
|
||||
const float HK42 = HK25*P(0,4) + HK33*P(0,5) + HK37*P(0,6) + HK38*P(0,0) + HK39*P(0,1) + HK40*P(0,2) + HK41*P(0,3);
|
||||
const float HK43 = ecl::powf(range, -2);
|
||||
const float HK44 = HK25*P(4,6) + HK33*P(5,6) + HK37*P(6,6) + HK38*P(0,6) + HK39*P(1,6) + HK40*P(2,6) + HK41*P(3,6);
|
||||
const float HK45 = HK25*P(4,5) + HK33*P(5,5) + HK37*P(5,6) + HK38*P(0,5) + HK39*P(1,5) + HK40*P(2,5) + HK41*P(3,5);
|
||||
const float HK46 = HK25*P(4,4) + HK33*P(4,5) + HK37*P(4,6) + HK38*P(0,4) + HK39*P(1,4) + HK40*P(2,4) + HK41*P(3,4);
|
||||
const float HK47 = HK25*P(2,4) + HK33*P(2,5) + HK37*P(2,6) + HK38*P(0,2) + HK39*P(1,2) + HK40*P(2,2) + HK41*P(2,3);
|
||||
const float HK48 = HK25*P(3,4) + HK33*P(3,5) + HK37*P(3,6) + HK38*P(0,3) + HK39*P(1,3) + HK40*P(2,3) + HK41*P(3,3);
|
||||
const float HK49 = HK25*P(1,4) + HK33*P(1,5) + HK37*P(1,6) + HK38*P(0,1) + HK39*P(1,1) + HK40*P(1,2) + HK41*P(1,3);
|
||||
const float HK50 = HK4/(HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS);
|
||||
|
||||
const float HK51 = Tbs(0,1)*q1;
|
||||
const float HK52 = Tbs(0,2)*q0;
|
||||
const float HK53 = Tbs(0,0)*q2;
|
||||
const float HK54 = HK51 + HK52 - HK53;
|
||||
const float HK55 = Tbs(0,0)*q3;
|
||||
const float HK56 = Tbs(0,1)*q0;
|
||||
const float HK57 = Tbs(0,2)*q1;
|
||||
const float HK58 = HK55 + HK56 - HK57;
|
||||
const float HK59 = Tbs(0,0)*q0;
|
||||
const float HK60 = Tbs(0,2)*q2;
|
||||
const float HK61 = Tbs(0,1)*q3;
|
||||
const float HK62 = HK59 + HK60 - HK61;
|
||||
const float HK63 = HK54*vd + HK58*ve + HK62*vn;
|
||||
const float HK64 = Tbs(0,0)*q1 + Tbs(0,1)*q2 + Tbs(0,2)*q3;
|
||||
const float HK65 = HK58*vd + HK64*vn;
|
||||
const float HK66 = -HK54*ve + HK65;
|
||||
const float HK67 = HK54*vn + HK64*ve;
|
||||
const float HK68 = -HK62*vd + HK67;
|
||||
const float HK69 = HK62*ve + HK64*vd;
|
||||
const float HK70 = -HK58*vn + HK69;
|
||||
const float HK71 = 2*Tbs(0,1);
|
||||
const float HK72 = 2*Tbs(0,2);
|
||||
const float HK73 = HK12*HK72 + HK24*Tbs(0,0);
|
||||
const float HK74 = -HK16*HK71 + HK73;
|
||||
const float HK75 = 2*Tbs(0,0);
|
||||
const float HK76 = HK26*HK75 + HK32*Tbs(0,1);
|
||||
const float HK77 = -HK30*HK72 + HK76;
|
||||
const float HK78 = HK34*HK71 + HK36*Tbs(0,2);
|
||||
const float HK79 = -HK35*HK75 + HK78;
|
||||
const float HK80 = 2*HK63;
|
||||
const float HK81 = 2*HK65 + 2*ve*(-HK51 - HK52 + HK53);
|
||||
const float HK82 = 2*HK67 + 2*vd*(-HK59 - HK60 + HK61);
|
||||
const float HK83 = 2*HK69 + 2*vn*(-HK55 - HK56 + HK57);
|
||||
const float HK84 = HK71*(-HK14 + HK15) + HK73;
|
||||
const float HK85 = HK72*(-HK28 + HK29) + HK76;
|
||||
const float HK86 = HK75*(-HK10 + HK11) + HK78;
|
||||
const float HK87 = HK80*P(0,0) + HK81*P(0,1) + HK82*P(0,2) + HK83*P(0,3) + HK84*P(0,4) + HK85*P(0,5) + HK86*P(0,6);
|
||||
const float HK88 = HK80*P(0,6) + HK81*P(1,6) + HK82*P(2,6) + HK83*P(3,6) + HK84*P(4,6) + HK85*P(5,6) + HK86*P(6,6);
|
||||
const float HK89 = HK80*P(0,5) + HK81*P(1,5) + HK82*P(2,5) + HK83*P(3,5) + HK84*P(4,5) + HK85*P(5,5) + HK86*P(5,6);
|
||||
const float HK90 = HK80*P(0,4) + HK81*P(1,4) + HK82*P(2,4) + HK83*P(3,4) + HK84*P(4,4) + HK85*P(4,5) + HK86*P(4,6);
|
||||
const float HK91 = HK80*P(0,2) + HK81*P(1,2) + HK82*P(2,2) + HK83*P(2,3) + HK84*P(2,4) + HK85*P(2,5) + HK86*P(2,6);
|
||||
const float HK92 = 2*HK43;
|
||||
const float HK93 = HK80*P(0,3) + HK81*P(1,3) + HK82*P(2,3) + HK83*P(3,3) + HK84*P(3,4) + HK85*P(3,5) + HK86*P(3,6);
|
||||
const float HK94 = HK80*P(0,1) + HK81*P(1,1) + HK82*P(1,2) + HK83*P(1,3) + HK84*P(1,4) + HK85*P(1,5) + HK86*P(1,6);
|
||||
const float HK95 = HK4/(HK43*HK74*HK90 + HK43*HK77*HK89 + HK43*HK79*HK88 + HK43*HK80*HK87 + HK66*HK92*HK94 + HK68*HK91*HK92 + HK70*HK92*HK93 + R_LOS);
|
||||
|
||||
|
||||
// Compare X axis equations
|
||||
{
|
||||
// evaluate sympy genrated equations for observatio Jacobians and Kalman gains
|
||||
{
|
||||
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
|
||||
flow_innov_var = (HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS);
|
||||
|
||||
const float HK50 = HK4/flow_innov_var;
|
||||
|
||||
// Observation Jacobians - axis 0
|
||||
Hfusion.at<0>() = HK3*HK5;
|
||||
Hfusion.at<1>() = HK5*HK7;
|
||||
Hfusion.at<2>() = HK5*HK8;
|
||||
Hfusion.at<3>() = HK5*HK9;
|
||||
Hfusion.at<4>() = HK25*HK4;
|
||||
Hfusion.at<5>() = HK33*HK4;
|
||||
Hfusion.at<6>() = HK37*HK4;
|
||||
|
||||
// Kalman gains - axis 0
|
||||
Kfusion(0) = HK42*HK50;
|
||||
Kfusion(1) = HK49*HK50;
|
||||
Kfusion(2) = HK47*HK50;
|
||||
Kfusion(3) = HK48*HK50;
|
||||
Kfusion(4) = HK46*HK50;
|
||||
Kfusion(5) = HK45*HK50;
|
||||
Kfusion(6) = HK44*HK50;
|
||||
|
||||
for (unsigned row = 7; row <= 23; row++) {
|
||||
Kfusion(row) = HK50*(HK25*P(4,row) + HK33*P(5,row) + HK37*P(6,row) + HK38*P(0,row) + HK39*P(1,row) + HK40*P(2,row) + HK41*P(3,row));
|
||||
}
|
||||
|
||||
// copy to arrays used for comparison
|
||||
for (int row=0; row<7; row++) {
|
||||
Hfusion_sympy(row) = Hfusion.atCompressedIndex(row);
|
||||
}
|
||||
for (int row=0; row<24; row++) {
|
||||
Kfusion_sympy(row) = Kfusion(row);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// repeat calculation using matlab generated equations
|
||||
|
||||
{
|
||||
// calculate X axis observation Jacobian
|
||||
float t2 = 1.0f / range;
|
||||
float H_LOS[24] = {};
|
||||
H_LOS[0] = t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f);
|
||||
H_LOS[1] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f);
|
||||
H_LOS[2] = t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f);
|
||||
H_LOS[3] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f);
|
||||
H_LOS[4] = -t2*(q0*q3*2.0f-q1*q2*2.0f);
|
||||
H_LOS[5] = t2*(q0*q0-q1*q1+q2*q2-q3*q3);
|
||||
H_LOS[6] = t2*(q0*q1*2.0f+q2*q3*2.0f);
|
||||
|
||||
// calculate intermediate variables for the X observation innovatoin variance and Kalman gains
|
||||
float t3 = q1*vd*2.0f;
|
||||
float t4 = q0*ve*2.0f;
|
||||
float t11 = q3*vn*2.0f;
|
||||
float t5 = t3+t4-t11;
|
||||
float t6 = q0*q3*2.0f;
|
||||
float t29 = q1*q2*2.0f;
|
||||
float t7 = t6-t29;
|
||||
float t8 = q0*q1*2.0f;
|
||||
float t9 = q2*q3*2.0f;
|
||||
float t10 = t8+t9;
|
||||
float t12 = P(0,0)*t2*t5;
|
||||
float t13 = q0*vd*2.0f;
|
||||
float t14 = q2*vn*2.0f;
|
||||
float t28 = q1*ve*2.0f;
|
||||
float t15 = t13+t14-t28;
|
||||
float t16 = q3*vd*2.0f;
|
||||
float t17 = q2*ve*2.0f;
|
||||
float t18 = q1*vn*2.0f;
|
||||
float t19 = t16+t17+t18;
|
||||
float t20 = q3*ve*2.0f;
|
||||
float t21 = q0*vn*2.0f;
|
||||
float t30 = q2*vd*2.0f;
|
||||
float t22 = t20+t21-t30;
|
||||
float t23 = q0*q0;
|
||||
float t24 = q1*q1;
|
||||
float t25 = q2*q2;
|
||||
float t26 = q3*q3;
|
||||
float t27 = t23-t24+t25-t26;
|
||||
float t31 = P(1,1)*t2*t15;
|
||||
float t32 = P(6,0)*t2*t10;
|
||||
float t33 = P(1,0)*t2*t15;
|
||||
float t34 = P(2,0)*t2*t19;
|
||||
float t35 = P(5,0)*t2*t27;
|
||||
float t79 = P(4,0)*t2*t7;
|
||||
float t80 = P(3,0)*t2*t22;
|
||||
float t36 = t12+t32+t33+t34+t35-t79-t80;
|
||||
float t37 = t2*t5*t36;
|
||||
float t38 = P(6,1)*t2*t10;
|
||||
float t39 = P(0,1)*t2*t5;
|
||||
float t40 = P(2,1)*t2*t19;
|
||||
float t41 = P(5,1)*t2*t27;
|
||||
float t81 = P(4,1)*t2*t7;
|
||||
float t82 = P(3,1)*t2*t22;
|
||||
float t42 = t31+t38+t39+t40+t41-t81-t82;
|
||||
float t43 = t2*t15*t42;
|
||||
float t44 = P(6,2)*t2*t10;
|
||||
float t45 = P(0,2)*t2*t5;
|
||||
float t46 = P(1,2)*t2*t15;
|
||||
float t47 = P(2,2)*t2*t19;
|
||||
float t48 = P(5,2)*t2*t27;
|
||||
float t83 = P(4,2)*t2*t7;
|
||||
float t84 = P(3,2)*t2*t22;
|
||||
float t49 = t44+t45+t46+t47+t48-t83-t84;
|
||||
float t50 = t2*t19*t49;
|
||||
float t51 = P(6,3)*t2*t10;
|
||||
float t52 = P(0,3)*t2*t5;
|
||||
float t53 = P(1,3)*t2*t15;
|
||||
float t54 = P(2,3)*t2*t19;
|
||||
float t55 = P(5,3)*t2*t27;
|
||||
float t85 = P(4,3)*t2*t7;
|
||||
float t86 = P(3,3)*t2*t22;
|
||||
float t56 = t51+t52+t53+t54+t55-t85-t86;
|
||||
float t57 = P(6,5)*t2*t10;
|
||||
float t58 = P(0,5)*t2*t5;
|
||||
float t59 = P(1,5)*t2*t15;
|
||||
float t60 = P(2,5)*t2*t19;
|
||||
float t61 = P(5,5)*t2*t27;
|
||||
float t88 = P(4,5)*t2*t7;
|
||||
float t89 = P(3,5)*t2*t22;
|
||||
float t62 = t57+t58+t59+t60+t61-t88-t89;
|
||||
float t63 = t2*t27*t62;
|
||||
float t64 = P(6,4)*t2*t10;
|
||||
float t65 = P(0,4)*t2*t5;
|
||||
float t66 = P(1,4)*t2*t15;
|
||||
float t67 = P(2,4)*t2*t19;
|
||||
float t68 = P(5,4)*t2*t27;
|
||||
float t90 = P(4,4)*t2*t7;
|
||||
float t91 = P(3,4)*t2*t22;
|
||||
float t69 = t64+t65+t66+t67+t68-t90-t91;
|
||||
float t70 = P(6,6)*t2*t10;
|
||||
float t71 = P(0,6)*t2*t5;
|
||||
float t72 = P(1,6)*t2*t15;
|
||||
float t73 = P(2,6)*t2*t19;
|
||||
float t74 = P(5,6)*t2*t27;
|
||||
float t93 = P(4,6)*t2*t7;
|
||||
float t94 = P(3,6)*t2*t22;
|
||||
float t75 = t70+t71+t72+t73+t74-t93-t94;
|
||||
float t76 = t2*t10*t75;
|
||||
float t87 = t2*t22*t56;
|
||||
float t92 = t2*t7*t69;
|
||||
float t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92;
|
||||
float t78 = 1.0f / t77;
|
||||
flow_innov_var = t77;
|
||||
|
||||
// calculate Kalman gains for X-axis observation
|
||||
float Kfusion[24];
|
||||
Kfusion[0] = t78*(t12-P(0,4)*t2*t7+P(0,1)*t2*t15+P(0,6)*t2*t10+P(0,2)*t2*t19-P(0,3)*t2*t22+P(0,5)*t2*t27);
|
||||
Kfusion[1] = t78*(t31+P(1,0)*t2*t5-P(1,4)*t2*t7+P(1,6)*t2*t10+P(1,2)*t2*t19-P(1,3)*t2*t22+P(1,5)*t2*t27);
|
||||
Kfusion[2] = t78*(t47+P(2,0)*t2*t5-P(2,4)*t2*t7+P(2,1)*t2*t15+P(2,6)*t2*t10-P(2,3)*t2*t22+P(2,5)*t2*t27);
|
||||
Kfusion[3] = t78*(-t86+P(3,0)*t2*t5-P(3,4)*t2*t7+P(3,1)*t2*t15+P(3,6)*t2*t10+P(3,2)*t2*t19+P(3,5)*t2*t27);
|
||||
Kfusion[4] = t78*(-t90+P(4,0)*t2*t5+P(4,1)*t2*t15+P(4,6)*t2*t10+P(4,2)*t2*t19-P(4,3)*t2*t22+P(4,5)*t2*t27);
|
||||
Kfusion[5] = t78*(t61+P(5,0)*t2*t5-P(5,4)*t2*t7+P(5,1)*t2*t15+P(5,6)*t2*t10+P(5,2)*t2*t19-P(5,3)*t2*t22);
|
||||
Kfusion[6] = t78*(t70+P(6,0)*t2*t5-P(6,4)*t2*t7+P(6,1)*t2*t15+P(6,2)*t2*t19-P(6,3)*t2*t22+P(6,5)*t2*t27);
|
||||
Kfusion[7] = t78*(P(7,0)*t2*t5-P(7,4)*t2*t7+P(7,1)*t2*t15+P(7,6)*t2*t10+P(7,2)*t2*t19-P(7,3)*t2*t22+P(7,5)*t2*t27);
|
||||
Kfusion[8] = t78*(P(8,0)*t2*t5-P(8,4)*t2*t7+P(8,1)*t2*t15+P(8,6)*t2*t10+P(8,2)*t2*t19-P(8,3)*t2*t22+P(8,5)*t2*t27);
|
||||
Kfusion[9] = t78*(P(9,0)*t2*t5-P(9,4)*t2*t7+P(9,1)*t2*t15+P(9,6)*t2*t10+P(9,2)*t2*t19-P(9,3)*t2*t22+P(9,5)*t2*t27);
|
||||
Kfusion[10] = t78*(P(10,0)*t2*t5-P(10,4)*t2*t7+P(10,1)*t2*t15+P(10,6)*t2*t10+P(10,2)*t2*t19-P(10,3)*t2*t22+P(10,5)*t2*t27);
|
||||
Kfusion[11] = t78*(P(11,0)*t2*t5-P(11,4)*t2*t7+P(11,1)*t2*t15+P(11,6)*t2*t10+P(11,2)*t2*t19-P(11,3)*t2*t22+P(11,5)*t2*t27);
|
||||
Kfusion[12] = t78*(P(12,0)*t2*t5-P(12,4)*t2*t7+P(12,1)*t2*t15+P(12,6)*t2*t10+P(12,2)*t2*t19-P(12,3)*t2*t22+P(12,5)*t2*t27);
|
||||
Kfusion[13] = t78*(P(13,0)*t2*t5-P(13,4)*t2*t7+P(13,1)*t2*t15+P(13,6)*t2*t10+P(13,2)*t2*t19-P(13,3)*t2*t22+P(13,5)*t2*t27);
|
||||
Kfusion[14] = t78*(P(14,0)*t2*t5-P(14,4)*t2*t7+P(14,1)*t2*t15+P(14,6)*t2*t10+P(14,2)*t2*t19-P(14,3)*t2*t22+P(14,5)*t2*t27);
|
||||
Kfusion[15] = t78*(P(15,0)*t2*t5-P(15,4)*t2*t7+P(15,1)*t2*t15+P(15,6)*t2*t10+P(15,2)*t2*t19-P(15,3)*t2*t22+P(15,5)*t2*t27);
|
||||
Kfusion[16] = t78*(P(16,0)*t2*t5-P(16,4)*t2*t7+P(16,1)*t2*t15+P(16,6)*t2*t10+P(16,2)*t2*t19-P(16,3)*t2*t22+P(16,5)*t2*t27);
|
||||
Kfusion[17] = t78*(P(17,0)*t2*t5-P(17,4)*t2*t7+P(17,1)*t2*t15+P(17,6)*t2*t10+P(17,2)*t2*t19-P(17,3)*t2*t22+P(17,5)*t2*t27);
|
||||
Kfusion[18] = t78*(P(18,0)*t2*t5-P(18,4)*t2*t7+P(18,1)*t2*t15+P(18,6)*t2*t10+P(18,2)*t2*t19-P(18,3)*t2*t22+P(18,5)*t2*t27);
|
||||
Kfusion[19] = t78*(P(19,0)*t2*t5-P(19,4)*t2*t7+P(19,1)*t2*t15+P(19,6)*t2*t10+P(19,2)*t2*t19-P(19,3)*t2*t22+P(19,5)*t2*t27);
|
||||
Kfusion[20] = t78*(P(20,0)*t2*t5-P(20,4)*t2*t7+P(20,1)*t2*t15+P(20,6)*t2*t10+P(20,2)*t2*t19-P(20,3)*t2*t22+P(20,5)*t2*t27);
|
||||
Kfusion[21] = t78*(P(21,0)*t2*t5-P(21,4)*t2*t7+P(21,1)*t2*t15+P(21,6)*t2*t10+P(21,2)*t2*t19-P(21,3)*t2*t22+P(21,5)*t2*t27);
|
||||
Kfusion[22] = t78*(P(22,0)*t2*t5-P(22,4)*t2*t7+P(22,1)*t2*t15+P(22,6)*t2*t10+P(22,2)*t2*t19-P(22,3)*t2*t22+P(22,5)*t2*t27);
|
||||
Kfusion[23] = t78*(P(23,0)*t2*t5-P(23,4)*t2*t7+P(23,1)*t2*t15+P(23,6)*t2*t10+P(23,2)*t2*t19-P(23,3)*t2*t22+P(23,5)*t2*t27);
|
||||
|
||||
for (int row=0; row<24; row++) {
|
||||
Hfusion_matlab(row) = H_LOS[row];
|
||||
Kfusion_matlab(row) = Kfusion[row];
|
||||
}
|
||||
}
|
||||
|
||||
// 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: Optical Flow 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: Optical Flow X axis Hfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
|
||||
// find largest Kalman gain difference as a fraction of the matlab value
|
||||
max_diff_fraction = 0.0f;
|
||||
for (int row=0; row<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 (Hfusion_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: Optical Flow 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: Optical Flow X axis Kfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// Compare Y axis equations
|
||||
|
||||
{
|
||||
// evaluate sympy genrated equations for observatio Jacobians and Kalman gains
|
||||
{
|
||||
// calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
|
||||
flow_innov_var = (HK43*HK74*HK90 + HK43*HK77*HK89 + HK43*HK79*HK88 + HK43*HK80*HK87 + HK66*HK92*HK94 + HK68*HK91*HK92 + HK70*HK92*HK93 + R_LOS);
|
||||
|
||||
const float HK95 = HK4/flow_innov_var;
|
||||
|
||||
// Observation Jacobians - axis 1
|
||||
Hfusion.at<0>() = -HK5*HK63;
|
||||
Hfusion.at<1>() = -HK5*HK66;
|
||||
Hfusion.at<2>() = -HK5*HK68;
|
||||
Hfusion.at<3>() = -HK5*HK70;
|
||||
Hfusion.at<4>() = -HK4*HK74;
|
||||
Hfusion.at<5>() = -HK4*HK77;
|
||||
Hfusion.at<6>() = -HK4*HK79;
|
||||
|
||||
// Kalman gains - axis 1
|
||||
Kfusion(0) = -HK87*HK95;
|
||||
Kfusion(1) = -HK94*HK95;
|
||||
Kfusion(2) = -HK91*HK95;
|
||||
Kfusion(3) = -HK93*HK95;
|
||||
Kfusion(4) = -HK90*HK95;
|
||||
Kfusion(5) = -HK89*HK95;
|
||||
Kfusion(6) = -HK88*HK95;
|
||||
|
||||
for (unsigned row = 7; row <= 23; row++) {
|
||||
Kfusion(row) = -HK95*(HK80*P(0,row) + HK81*P(1,row) + HK82*P(2,row) + HK83*P(3,row) + HK84*P(4,row) + HK85*P(5,row) + HK86*P(6,row));
|
||||
}
|
||||
|
||||
// copy to arrays used for comparison
|
||||
for (int row=0; row<7; row++) {
|
||||
Hfusion_sympy(row) = Hfusion.atCompressedIndex(row);
|
||||
}
|
||||
for (int row=0; row<24; row++) {
|
||||
Kfusion_sympy(row) = Kfusion(row);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// repeat calculation using matlab generated equations
|
||||
|
||||
{
|
||||
// calculate Y axis observation Jacobian
|
||||
float t2 = 1.0f / range;
|
||||
float H_LOS[24] = {};
|
||||
H_LOS[0] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f);
|
||||
H_LOS[1] = -t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f);
|
||||
H_LOS[2] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f);
|
||||
H_LOS[3] = -t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f);
|
||||
H_LOS[4] = -t2*(q0*q0+q1*q1-q2*q2-q3*q3);
|
||||
H_LOS[5] = -t2*(q0*q3*2.0f+q1*q2*2.0f);
|
||||
H_LOS[6] = t2*(q0*q2*2.0f-q1*q3*2.0f);
|
||||
|
||||
// calculate intermediate variables for the Y observation innovatoin variance and Kalman gains
|
||||
float t3 = q3*ve*2.0f;
|
||||
float t4 = q0*vn*2.0f;
|
||||
float t11 = q2*vd*2.0f;
|
||||
float t5 = t3+t4-t11;
|
||||
float t6 = q0*q3*2.0f;
|
||||
float t7 = q1*q2*2.0f;
|
||||
float t8 = t6+t7;
|
||||
float t9 = q0*q2*2.0f;
|
||||
float t28 = q1*q3*2.0f;
|
||||
float t10 = t9-t28;
|
||||
float t12 = P(0,0)*t2*t5;
|
||||
float t13 = q3*vd*2.0f;
|
||||
float t14 = q2*ve*2.0f;
|
||||
float t15 = q1*vn*2.0f;
|
||||
float t16 = t13+t14+t15;
|
||||
float t17 = q0*vd*2.0f;
|
||||
float t18 = q2*vn*2.0f;
|
||||
float t29 = q1*ve*2.0f;
|
||||
float t19 = t17+t18-t29;
|
||||
float t20 = q1*vd*2.0f;
|
||||
float t21 = q0*ve*2.0f;
|
||||
float t30 = q3*vn*2.0f;
|
||||
float t22 = t20+t21-t30;
|
||||
float t23 = q0*q0;
|
||||
float t24 = q1*q1;
|
||||
float t25 = q2*q2;
|
||||
float t26 = q3*q3;
|
||||
float t27 = t23+t24-t25-t26;
|
||||
float t31 = P(1,1)*t2*t16;
|
||||
float t32 = P(5,0)*t2*t8;
|
||||
float t33 = P(1,0)*t2*t16;
|
||||
float t34 = P(3,0)*t2*t22;
|
||||
float t35 = P(4,0)*t2*t27;
|
||||
float t80 = P(6,0)*t2*t10;
|
||||
float t81 = P(2,0)*t2*t19;
|
||||
float t36 = t12+t32+t33+t34+t35-t80-t81;
|
||||
float t37 = t2*t5*t36;
|
||||
float t38 = P(5,1)*t2*t8;
|
||||
float t39 = P(0,1)*t2*t5;
|
||||
float t40 = P(3,1)*t2*t22;
|
||||
float t41 = P(4,1)*t2*t27;
|
||||
float t82 = P(6,1)*t2*t10;
|
||||
float t83 = P(2,1)*t2*t19;
|
||||
float t42 = t31+t38+t39+t40+t41-t82-t83;
|
||||
float t43 = t2*t16*t42;
|
||||
float t44 = P(5,2)*t2*t8;
|
||||
float t45 = P(0,2)*t2*t5;
|
||||
float t46 = P(1,2)*t2*t16;
|
||||
float t47 = P(3,2)*t2*t22;
|
||||
float t48 = P(4,2)*t2*t27;
|
||||
float t79 = P(2,2)*t2*t19;
|
||||
float t84 = P(6,2)*t2*t10;
|
||||
float t49 = t44+t45+t46+t47+t48-t79-t84;
|
||||
float t50 = P(5,3)*t2*t8;
|
||||
float t51 = P(0,3)*t2*t5;
|
||||
float t52 = P(1,3)*t2*t16;
|
||||
float t53 = P(3,3)*t2*t22;
|
||||
float t54 = P(4,3)*t2*t27;
|
||||
float t86 = P(6,3)*t2*t10;
|
||||
float t87 = P(2,3)*t2*t19;
|
||||
float t55 = t50+t51+t52+t53+t54-t86-t87;
|
||||
float t56 = t2*t22*t55;
|
||||
float t57 = P(5,4)*t2*t8;
|
||||
float t58 = P(0,4)*t2*t5;
|
||||
float t59 = P(1,4)*t2*t16;
|
||||
float t60 = P(3,4)*t2*t22;
|
||||
float t61 = P(4,4)*t2*t27;
|
||||
float t88 = P(6,4)*t2*t10;
|
||||
float t89 = P(2,4)*t2*t19;
|
||||
float t62 = t57+t58+t59+t60+t61-t88-t89;
|
||||
float t63 = t2*t27*t62;
|
||||
float t64 = P(5,5)*t2*t8;
|
||||
float t65 = P(0,5)*t2*t5;
|
||||
float t66 = P(1,5)*t2*t16;
|
||||
float t67 = P(3,5)*t2*t22;
|
||||
float t68 = P(4,5)*t2*t27;
|
||||
float t90 = P(6,5)*t2*t10;
|
||||
float t91 = P(2,5)*t2*t19;
|
||||
float t69 = t64+t65+t66+t67+t68-t90-t91;
|
||||
float t70 = t2*t8*t69;
|
||||
float t71 = P(5,6)*t2*t8;
|
||||
float t72 = P(0,6)*t2*t5;
|
||||
float t73 = P(1,6)*t2*t16;
|
||||
float t74 = P(3,6)*t2*t22;
|
||||
float t75 = P(4,6)*t2*t27;
|
||||
float t92 = P(6,6)*t2*t10;
|
||||
float t93 = P(2,6)*t2*t19;
|
||||
float t76 = t71+t72+t73+t74+t75-t92-t93;
|
||||
float t85 = t2*t19*t49;
|
||||
float t94 = t2*t10*t76;
|
||||
float t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94;
|
||||
|
||||
float t78 = 1.0f / t77;
|
||||
flow_innov_var = t77;
|
||||
|
||||
// calculate Kalman gains for Y-axis observation
|
||||
float Kfusion[24];
|
||||
Kfusion[0] = -t78*(t12+P(0,5)*t2*t8-P(0,6)*t2*t10+P(0,1)*t2*t16-P(0,2)*t2*t19+P(0,3)*t2*t22+P(0,4)*t2*t27);
|
||||
Kfusion[1] = -t78*(t31+P(1,0)*t2*t5+P(1,5)*t2*t8-P(1,6)*t2*t10-P(1,2)*t2*t19+P(1,3)*t2*t22+P(1,4)*t2*t27);
|
||||
Kfusion[2] = -t78*(-t79+P(2,0)*t2*t5+P(2,5)*t2*t8-P(2,6)*t2*t10+P(2,1)*t2*t16+P(2,3)*t2*t22+P(2,4)*t2*t27);
|
||||
Kfusion[3] = -t78*(t53+P(3,0)*t2*t5+P(3,5)*t2*t8-P(3,6)*t2*t10+P(3,1)*t2*t16-P(3,2)*t2*t19+P(3,4)*t2*t27);
|
||||
Kfusion[4] = -t78*(t61+P(4,0)*t2*t5+P(4,5)*t2*t8-P(4,6)*t2*t10+P(4,1)*t2*t16-P(4,2)*t2*t19+P(4,3)*t2*t22);
|
||||
Kfusion[5] = -t78*(t64+P(5,0)*t2*t5-P(5,6)*t2*t10+P(5,1)*t2*t16-P(5,2)*t2*t19+P(5,3)*t2*t22+P(5,4)*t2*t27);
|
||||
Kfusion[6] = -t78*(-t92+P(6,0)*t2*t5+P(6,5)*t2*t8+P(6,1)*t2*t16-P(6,2)*t2*t19+P(6,3)*t2*t22+P(6,4)*t2*t27);
|
||||
Kfusion[7] = -t78*(P(7,0)*t2*t5+P(7,5)*t2*t8-P(7,6)*t2*t10+P(7,1)*t2*t16-P(7,2)*t2*t19+P(7,3)*t2*t22+P(7,4)*t2*t27);
|
||||
Kfusion[8] = -t78*(P(8,0)*t2*t5+P(8,5)*t2*t8-P(8,6)*t2*t10+P(8,1)*t2*t16-P(8,2)*t2*t19+P(8,3)*t2*t22+P(8,4)*t2*t27);
|
||||
Kfusion[9] = -t78*(P(9,0)*t2*t5+P(9,5)*t2*t8-P(9,6)*t2*t10+P(9,1)*t2*t16-P(9,2)*t2*t19+P(9,3)*t2*t22+P(9,4)*t2*t27);
|
||||
Kfusion[10] = -t78*(P(10,0)*t2*t5+P(10,5)*t2*t8-P(10,6)*t2*t10+P(10,1)*t2*t16-P(10,2)*t2*t19+P(10,3)*t2*t22+P(10,4)*t2*t27);
|
||||
Kfusion[11] = -t78*(P(11,0)*t2*t5+P(11,5)*t2*t8-P(11,6)*t2*t10+P(11,1)*t2*t16-P(11,2)*t2*t19+P(11,3)*t2*t22+P(11,4)*t2*t27);
|
||||
Kfusion[12] = -t78*(P(12,0)*t2*t5+P(12,5)*t2*t8-P(12,6)*t2*t10+P(12,1)*t2*t16-P(12,2)*t2*t19+P(12,3)*t2*t22+P(12,4)*t2*t27);
|
||||
Kfusion[13] = -t78*(P(13,0)*t2*t5+P(13,5)*t2*t8-P(13,6)*t2*t10+P(13,1)*t2*t16-P(13,2)*t2*t19+P(13,3)*t2*t22+P(13,4)*t2*t27);
|
||||
Kfusion[14] = -t78*(P(14,0)*t2*t5+P(14,5)*t2*t8-P(14,6)*t2*t10+P(14,1)*t2*t16-P(14,2)*t2*t19+P(14,3)*t2*t22+P(14,4)*t2*t27);
|
||||
Kfusion[15] = -t78*(P(15,0)*t2*t5+P(15,5)*t2*t8-P(15,6)*t2*t10+P(15,1)*t2*t16-P(15,2)*t2*t19+P(15,3)*t2*t22+P(15,4)*t2*t27);
|
||||
Kfusion[16] = -t78*(P(16,0)*t2*t5+P(16,5)*t2*t8-P(16,6)*t2*t10+P(16,1)*t2*t16-P(16,2)*t2*t19+P(16,3)*t2*t22+P(16,4)*t2*t27);
|
||||
Kfusion[17] = -t78*(P(17,0)*t2*t5+P(17,5)*t2*t8-P(17,6)*t2*t10+P(17,1)*t2*t16-P(17,2)*t2*t19+P(17,3)*t2*t22+P(17,4)*t2*t27);
|
||||
Kfusion[18] = -t78*(P(18,0)*t2*t5+P(18,5)*t2*t8-P(18,6)*t2*t10+P(18,1)*t2*t16-P(18,2)*t2*t19+P(18,3)*t2*t22+P(18,4)*t2*t27);
|
||||
Kfusion[19] = -t78*(P(19,0)*t2*t5+P(19,5)*t2*t8-P(19,6)*t2*t10+P(19,1)*t2*t16-P(19,2)*t2*t19+P(19,3)*t2*t22+P(19,4)*t2*t27);
|
||||
Kfusion[20] = -t78*(P(20,0)*t2*t5+P(20,5)*t2*t8-P(20,6)*t2*t10+P(20,1)*t2*t16-P(20,2)*t2*t19+P(20,3)*t2*t22+P(20,4)*t2*t27);
|
||||
Kfusion[21] = -t78*(P(21,0)*t2*t5+P(21,5)*t2*t8-P(21,6)*t2*t10+P(21,1)*t2*t16-P(21,2)*t2*t19+P(21,3)*t2*t22+P(21,4)*t2*t27);
|
||||
Kfusion[22] = -t78*(P(22,0)*t2*t5+P(22,5)*t2*t8-P(22,6)*t2*t10+P(22,1)*t2*t16-P(22,2)*t2*t19+P(22,3)*t2*t22+P(22,4)*t2*t27);
|
||||
Kfusion[23] = -t78*(P(23,0)*t2*t5+P(23,5)*t2*t8-P(23,6)*t2*t10+P(23,1)*t2*t16-P(23,2)*t2*t19+P(23,3)*t2*t22+P(23,4)*t2*t27);
|
||||
|
||||
for (int row=0; row<24; row++) {
|
||||
Hfusion_matlab(row) = H_LOS[row];
|
||||
Kfusion_matlab(row) = Kfusion[row];
|
||||
}
|
||||
}
|
||||
|
||||
// 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: Optical Flow 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: Optical Flow 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<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 (Hfusion_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: Optical Flow 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: Optical Flow Y axis Kfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,230 @@
|
||||
// X Axis Equations
|
||||
// Sub Expressions
|
||||
const float HK0 = Tbs(1,0)*q2;
|
||||
const float HK1 = Tbs(1,1)*q1;
|
||||
const float HK2 = Tbs(1,1)*q3;
|
||||
const float HK3 = Tbs(1,2)*q2;
|
||||
const float HK4 = Tbs(1,0)*q3;
|
||||
const float HK5 = Tbs(1,2)*q1;
|
||||
const float HK6 = -HK5;
|
||||
const float HK7 = vd*(HK0 - HK1) - ve*(HK4 + HK6) + vn*(HK2 - HK3);
|
||||
const float HK8 = 1.0F/range;
|
||||
const float HK9 = 2*HK8;
|
||||
const float HK10 = Tbs(1,1)*q2;
|
||||
const float HK11 = Tbs(1,2)*q3;
|
||||
const float HK12 = Tbs(1,1)*q0;
|
||||
const float HK13 = Tbs(1,2)*q0;
|
||||
const float HK14 = vd*(HK12 + HK4 - 2*HK5) - ve*(-HK0 + 2*HK1 + HK13) + vn*(HK10 + HK11);
|
||||
const float HK15 = Tbs(1,0)*q1;
|
||||
const float HK16 = Tbs(1,0)*q0;
|
||||
const float HK17 = -vd*(HK16 - HK2 + 2*HK3) + ve*(HK11 + HK15) + vn*(-2*HK0 + HK1 + HK13);
|
||||
const float HK18 = vd*(HK10 + HK15) + ve*(HK16 - 2*HK2 + HK3) - vn*(HK12 + 2*HK4 + HK6);
|
||||
const float HK19 = q0*q2;
|
||||
const float HK20 = q1*q3;
|
||||
const float HK21 = 2*Tbs(1,2);
|
||||
const float HK22 = q0*q3;
|
||||
const float HK23 = q1*q2;
|
||||
const float HK24 = 2*Tbs(1,1);
|
||||
const float HK25 = 2*powf(q2, 2);
|
||||
const float HK26 = 2*powf(q3, 2) - 1;
|
||||
const float HK27 = -HK21*(HK19 + HK20) + HK24*(HK22 - HK23) + Tbs(1,0)*(HK25 + HK26);
|
||||
const float HK28 = 2*Tbs(1,0);
|
||||
const float HK29 = q0*q1;
|
||||
const float HK30 = q2*q3;
|
||||
const float HK31 = 2*powf(q1, 2);
|
||||
const float HK32 = HK21*(HK29 - HK30) - HK28*(HK22 + HK23) + Tbs(1,1)*(HK26 + HK31);
|
||||
const float HK33 = -HK24*(HK29 + HK30) + HK28*(HK19 - HK20) + Tbs(1,2)*(HK25 + HK31 - 1);
|
||||
const float HK34 = 2*HK7;
|
||||
const float HK35 = 2*HK14;
|
||||
const float HK36 = 2*HK17;
|
||||
const float HK37 = 2*HK18;
|
||||
const float HK38 = -HK27*P(0,4) - HK32*P(0,5) - HK33*P(0,6) - HK34*P(0,0) + HK35*P(0,1) + HK36*P(0,2) + HK37*P(0,3);
|
||||
const float HK39 = powf(range, -2);
|
||||
const float HK40 = -HK27*P(4,6) - HK32*P(5,6) - HK33*P(6,6) - HK34*P(0,6) + HK35*P(1,6) + HK36*P(2,6) + HK37*P(3,6);
|
||||
const float HK41 = -HK27*P(4,5) - HK32*P(5,5) - HK33*P(5,6) - HK34*P(0,5) + HK35*P(1,5) + HK36*P(2,5) + HK37*P(3,5);
|
||||
const float HK42 = -HK27*P(4,4) - HK32*P(4,5) - HK33*P(4,6) - HK34*P(0,4) + HK35*P(1,4) + HK36*P(2,4) + HK37*P(3,4);
|
||||
const float HK43 = -HK27*P(3,4) - HK32*P(3,5) - HK33*P(3,6) - HK34*P(0,3) + HK35*P(1,3) + HK36*P(2,3) + HK37*P(3,3);
|
||||
const float HK44 = -HK27*P(2,4) - HK32*P(2,5) - HK33*P(2,6) - HK34*P(0,2) + HK35*P(1,2) + HK36*P(2,2) + HK37*P(2,3);
|
||||
const float HK45 = -HK27*P(1,4) - HK32*P(1,5) - HK33*P(1,6) - HK34*P(0,1) + HK35*P(1,1) + HK36*P(1,2) + HK37*P(1,3);
|
||||
const float HK46 = HK8/(-HK27*HK39*HK42 - HK32*HK39*HK41 - HK33*HK39*HK40 - HK34*HK38*HK39 + HK35*HK39*HK45 + HK36*HK39*HK44 + HK37*HK39*HK43 + R_LOS);
|
||||
|
||||
|
||||
// Observation Jacobians
|
||||
Hfusion.at<0>() = -HK7*HK9;
|
||||
Hfusion.at<1>() = HK14*HK9;
|
||||
Hfusion.at<2>() = HK17*HK9;
|
||||
Hfusion.at<3>() = HK18*HK9;
|
||||
Hfusion.at<4>() = -HK27*HK8;
|
||||
Hfusion.at<5>() = -HK32*HK8;
|
||||
Hfusion.at<6>() = -HK33*HK8;
|
||||
Hfusion.at<7>() = 0;
|
||||
Hfusion.at<8>() = 0;
|
||||
Hfusion.at<9>() = 0;
|
||||
Hfusion.at<10>() = 0;
|
||||
Hfusion.at<11>() = 0;
|
||||
Hfusion.at<12>() = 0;
|
||||
Hfusion.at<13>() = 0;
|
||||
Hfusion.at<14>() = 0;
|
||||
Hfusion.at<15>() = 0;
|
||||
Hfusion.at<16>() = 0;
|
||||
Hfusion.at<17>() = 0;
|
||||
Hfusion.at<18>() = 0;
|
||||
Hfusion.at<19>() = 0;
|
||||
Hfusion.at<20>() = 0;
|
||||
Hfusion.at<21>() = 0;
|
||||
Hfusion.at<22>() = 0;
|
||||
Hfusion.at<23>() = 0;
|
||||
|
||||
|
||||
// Kalman gains
|
||||
Kfusion(0) = HK38*HK46;
|
||||
Kfusion(1) = HK45*HK46;
|
||||
Kfusion(2) = HK44*HK46;
|
||||
Kfusion(3) = HK43*HK46;
|
||||
Kfusion(4) = HK42*HK46;
|
||||
Kfusion(5) = HK41*HK46;
|
||||
Kfusion(6) = HK40*HK46;
|
||||
Kfusion(7) = HK46*(-HK27*P(4,7) - HK32*P(5,7) - HK33*P(6,7) - HK34*P(0,7) + HK35*P(1,7) + HK36*P(2,7) + HK37*P(3,7));
|
||||
Kfusion(8) = HK46*(-HK27*P(4,8) - HK32*P(5,8) - HK33*P(6,8) - HK34*P(0,8) + HK35*P(1,8) + HK36*P(2,8) + HK37*P(3,8));
|
||||
Kfusion(9) = HK46*(-HK27*P(4,9) - HK32*P(5,9) - HK33*P(6,9) - HK34*P(0,9) + HK35*P(1,9) + HK36*P(2,9) + HK37*P(3,9));
|
||||
Kfusion(10) = HK46*(-HK27*P(4,10) - HK32*P(5,10) - HK33*P(6,10) - HK34*P(0,10) + HK35*P(1,10) + HK36*P(2,10) + HK37*P(3,10));
|
||||
Kfusion(11) = HK46*(-HK27*P(4,11) - HK32*P(5,11) - HK33*P(6,11) - HK34*P(0,11) + HK35*P(1,11) + HK36*P(2,11) + HK37*P(3,11));
|
||||
Kfusion(12) = HK46*(-HK27*P(4,12) - HK32*P(5,12) - HK33*P(6,12) - HK34*P(0,12) + HK35*P(1,12) + HK36*P(2,12) + HK37*P(3,12));
|
||||
Kfusion(13) = HK46*(-HK27*P(4,13) - HK32*P(5,13) - HK33*P(6,13) - HK34*P(0,13) + HK35*P(1,13) + HK36*P(2,13) + HK37*P(3,13));
|
||||
Kfusion(14) = HK46*(-HK27*P(4,14) - HK32*P(5,14) - HK33*P(6,14) - HK34*P(0,14) + HK35*P(1,14) + HK36*P(2,14) + HK37*P(3,14));
|
||||
Kfusion(15) = HK46*(-HK27*P(4,15) - HK32*P(5,15) - HK33*P(6,15) - HK34*P(0,15) + HK35*P(1,15) + HK36*P(2,15) + HK37*P(3,15));
|
||||
Kfusion(16) = HK46*(-HK27*P(4,16) - HK32*P(5,16) - HK33*P(6,16) - HK34*P(0,16) + HK35*P(1,16) + HK36*P(2,16) + HK37*P(3,16));
|
||||
Kfusion(17) = HK46*(-HK27*P(4,17) - HK32*P(5,17) - HK33*P(6,17) - HK34*P(0,17) + HK35*P(1,17) + HK36*P(2,17) + HK37*P(3,17));
|
||||
Kfusion(18) = HK46*(-HK27*P(4,18) - HK32*P(5,18) - HK33*P(6,18) - HK34*P(0,18) + HK35*P(1,18) + HK36*P(2,18) + HK37*P(3,18));
|
||||
Kfusion(19) = HK46*(-HK27*P(4,19) - HK32*P(5,19) - HK33*P(6,19) - HK34*P(0,19) + HK35*P(1,19) + HK36*P(2,19) + HK37*P(3,19));
|
||||
Kfusion(20) = HK46*(-HK27*P(4,20) - HK32*P(5,20) - HK33*P(6,20) - HK34*P(0,20) + HK35*P(1,20) + HK36*P(2,20) + HK37*P(3,20));
|
||||
Kfusion(21) = HK46*(-HK27*P(4,21) - HK32*P(5,21) - HK33*P(6,21) - HK34*P(0,21) + HK35*P(1,21) + HK36*P(2,21) + HK37*P(3,21));
|
||||
Kfusion(22) = HK46*(-HK27*P(4,22) - HK32*P(5,22) - HK33*P(6,22) - HK34*P(0,22) + HK35*P(1,22) + HK36*P(2,22) + HK37*P(3,22));
|
||||
Kfusion(23) = HK46*(-HK27*P(4,23) - HK32*P(5,23) - HK33*P(6,23) - HK34*P(0,23) + HK35*P(1,23) + HK36*P(2,23) + HK37*P(3,23));
|
||||
|
||||
|
||||
// Y Axis Equations
|
||||
// Sub Expressions
|
||||
const float HK0 = Tbs(0,0)*q3;
|
||||
const float HK1 = Tbs(0,2)*q1;
|
||||
const float HK2 = -HK1;
|
||||
const float HK3 = ve*(HK0 + HK2);
|
||||
const float HK4 = Tbs(0,0)*q2;
|
||||
const float HK5 = Tbs(0,1)*q1;
|
||||
const float HK6 = Tbs(0,1)*q3;
|
||||
const float HK7 = Tbs(0,2)*q2;
|
||||
const float HK8 = HK3 - vd*(HK4 - HK5) - vn*(HK6 - HK7);
|
||||
const float HK9 = 1.0F/range;
|
||||
const float HK10 = 2*HK9;
|
||||
const float HK11 = Tbs(0,2)*q0;
|
||||
const float HK12 = -HK4;
|
||||
const float HK13 = 2*HK5;
|
||||
const float HK14 = Tbs(0,1)*q0;
|
||||
const float HK15 = Tbs(0,1)*q2;
|
||||
const float HK16 = Tbs(0,2)*q3;
|
||||
const float HK17 = vd*(HK0 - 2*HK1 + HK14) + vn*(HK15 + HK16);
|
||||
const float HK18 = HK17 - ve*(HK11 + HK12 + HK13);
|
||||
const float HK19 = Tbs(0,0)*q0;
|
||||
const float HK20 = -HK6;
|
||||
const float HK21 = 2*HK7;
|
||||
const float HK22 = Tbs(0,0)*q1;
|
||||
const float HK23 = ve*(HK16 + HK22) + vn*(HK11 - 2*HK4 + HK5);
|
||||
const float HK24 = HK23 - vd*(HK19 + HK20 + HK21);
|
||||
const float HK25 = 2*HK0;
|
||||
const float HK26 = vd*(HK15 + HK22) + ve*(HK19 - 2*HK6 + HK7);
|
||||
const float HK27 = HK26 - vn*(HK14 + HK2 + HK25);
|
||||
const float HK28 = q0*q2;
|
||||
const float HK29 = q1*q3;
|
||||
const float HK30 = 2*Tbs(0,2);
|
||||
const float HK31 = HK30*(HK28 + HK29);
|
||||
const float HK32 = q0*q3;
|
||||
const float HK33 = q1*q2;
|
||||
const float HK34 = 2*Tbs(0,1);
|
||||
const float HK35 = 2*powf(q2, 2);
|
||||
const float HK36 = 2*powf(q3, 2);
|
||||
const float HK37 = HK36 - 1;
|
||||
const float HK38 = HK31 - HK34*(HK32 - HK33) - Tbs(0,0)*(HK35 + HK37);
|
||||
const float HK39 = 2*Tbs(0,0);
|
||||
const float HK40 = HK39*(HK32 + HK33);
|
||||
const float HK41 = q0*q1;
|
||||
const float HK42 = q2*q3;
|
||||
const float HK43 = 2*powf(q1, 2);
|
||||
const float HK44 = -HK30*(HK41 - HK42) + HK40 - Tbs(0,1)*(HK37 + HK43);
|
||||
const float HK45 = HK34*(HK41 + HK42);
|
||||
const float HK46 = -HK39*(HK28 - HK29) + HK45 - Tbs(0,2)*(HK35 + HK43 - 1);
|
||||
const float HK47 = 2*HK3 + 2*vd*(HK12 + HK5) + 2*vn*(HK20 + HK7);
|
||||
const float HK48 = -HK35;
|
||||
const float HK49 = 1 - HK36;
|
||||
const float HK50 = HK31 + HK34*(-HK32 + HK33) + Tbs(0,0)*(HK48 + HK49);
|
||||
const float HK51 = -HK43;
|
||||
const float HK52 = HK30*(-HK41 + HK42) + HK40 + Tbs(0,1)*(HK49 + HK51);
|
||||
const float HK53 = HK39*(-HK28 + HK29) + HK45 + Tbs(0,2)*(HK48 + HK51 + 1);
|
||||
const float HK54 = 2*HK17 + 2*ve*(-HK11 - HK13 + HK4);
|
||||
const float HK55 = 2*HK23 + 2*vd*(-HK19 - HK21 + HK6);
|
||||
const float HK56 = 2*HK26 + 2*vn*(HK1 - HK14 - HK25);
|
||||
const float HK57 = HK47*P(0,0) + HK50*P(0,4) + HK52*P(0,5) + HK53*P(0,6) + HK54*P(0,1) + HK55*P(0,2) + HK56*P(0,3);
|
||||
const float HK58 = powf(range, -2);
|
||||
const float HK59 = 2*HK58;
|
||||
const float HK60 = HK47*P(0,6) + HK50*P(4,6) + HK52*P(5,6) + HK53*P(6,6) + HK54*P(1,6) + HK55*P(2,6) + HK56*P(3,6);
|
||||
const float HK61 = HK47*P(0,5) + HK50*P(4,5) + HK52*P(5,5) + HK53*P(5,6) + HK54*P(1,5) + HK55*P(2,5) + HK56*P(3,5);
|
||||
const float HK62 = HK47*P(0,4) + HK50*P(4,4) + HK52*P(4,5) + HK53*P(4,6) + HK54*P(1,4) + HK55*P(2,4) + HK56*P(3,4);
|
||||
const float HK63 = HK47*P(0,3) + HK50*P(3,4) + HK52*P(3,5) + HK53*P(3,6) + HK54*P(1,3) + HK55*P(2,3) + HK56*P(3,3);
|
||||
const float HK64 = HK47*P(0,2) + HK50*P(2,4) + HK52*P(2,5) + HK53*P(2,6) + HK54*P(1,2) + HK55*P(2,2) + HK56*P(2,3);
|
||||
const float HK65 = HK47*P(0,1) + HK50*P(1,4) + HK52*P(1,5) + HK53*P(1,6) + HK54*P(1,1) + HK55*P(1,2) + HK56*P(1,3);
|
||||
const float HK66 = HK9/(HK18*HK59*HK65 + HK24*HK59*HK64 + HK27*HK59*HK63 + HK38*HK58*HK62 + HK44*HK58*HK61 + HK46*HK58*HK60 + HK57*HK59*HK8 + R_LOS);
|
||||
|
||||
|
||||
// Observation Jacobians
|
||||
Hfusion.at<0>() = -HK10*HK8;
|
||||
Hfusion.at<1>() = -HK10*HK18;
|
||||
Hfusion.at<2>() = -HK10*HK24;
|
||||
Hfusion.at<3>() = -HK10*HK27;
|
||||
Hfusion.at<4>() = -HK38*HK9;
|
||||
Hfusion.at<5>() = -HK44*HK9;
|
||||
Hfusion.at<6>() = -HK46*HK9;
|
||||
Hfusion.at<7>() = 0;
|
||||
Hfusion.at<8>() = 0;
|
||||
Hfusion.at<9>() = 0;
|
||||
Hfusion.at<10>() = 0;
|
||||
Hfusion.at<11>() = 0;
|
||||
Hfusion.at<12>() = 0;
|
||||
Hfusion.at<13>() = 0;
|
||||
Hfusion.at<14>() = 0;
|
||||
Hfusion.at<15>() = 0;
|
||||
Hfusion.at<16>() = 0;
|
||||
Hfusion.at<17>() = 0;
|
||||
Hfusion.at<18>() = 0;
|
||||
Hfusion.at<19>() = 0;
|
||||
Hfusion.at<20>() = 0;
|
||||
Hfusion.at<21>() = 0;
|
||||
Hfusion.at<22>() = 0;
|
||||
Hfusion.at<23>() = 0;
|
||||
|
||||
|
||||
// Kalman gains
|
||||
Kfusion(0) = -HK57*HK66;
|
||||
Kfusion(1) = -HK65*HK66;
|
||||
Kfusion(2) = -HK64*HK66;
|
||||
Kfusion(3) = -HK63*HK66;
|
||||
Kfusion(4) = -HK62*HK66;
|
||||
Kfusion(5) = -HK61*HK66;
|
||||
Kfusion(6) = -HK60*HK66;
|
||||
Kfusion(7) = -HK66*(HK47*P(0,7) + HK50*P(4,7) + HK52*P(5,7) + HK53*P(6,7) + HK54*P(1,7) + HK55*P(2,7) + HK56*P(3,7));
|
||||
Kfusion(8) = -HK66*(HK47*P(0,8) + HK50*P(4,8) + HK52*P(5,8) + HK53*P(6,8) + HK54*P(1,8) + HK55*P(2,8) + HK56*P(3,8));
|
||||
Kfusion(9) = -HK66*(HK47*P(0,9) + HK50*P(4,9) + HK52*P(5,9) + HK53*P(6,9) + HK54*P(1,9) + HK55*P(2,9) + HK56*P(3,9));
|
||||
Kfusion(10) = -HK66*(HK47*P(0,10) + HK50*P(4,10) + HK52*P(5,10) + HK53*P(6,10) + HK54*P(1,10) + HK55*P(2,10) + HK56*P(3,10));
|
||||
Kfusion(11) = -HK66*(HK47*P(0,11) + HK50*P(4,11) + HK52*P(5,11) + HK53*P(6,11) + HK54*P(1,11) + HK55*P(2,11) + HK56*P(3,11));
|
||||
Kfusion(12) = -HK66*(HK47*P(0,12) + HK50*P(4,12) + HK52*P(5,12) + HK53*P(6,12) + HK54*P(1,12) + HK55*P(2,12) + HK56*P(3,12));
|
||||
Kfusion(13) = -HK66*(HK47*P(0,13) + HK50*P(4,13) + HK52*P(5,13) + HK53*P(6,13) + HK54*P(1,13) + HK55*P(2,13) + HK56*P(3,13));
|
||||
Kfusion(14) = -HK66*(HK47*P(0,14) + HK50*P(4,14) + HK52*P(5,14) + HK53*P(6,14) + HK54*P(1,14) + HK55*P(2,14) + HK56*P(3,14));
|
||||
Kfusion(15) = -HK66*(HK47*P(0,15) + HK50*P(4,15) + HK52*P(5,15) + HK53*P(6,15) + HK54*P(1,15) + HK55*P(2,15) + HK56*P(3,15));
|
||||
Kfusion(16) = -HK66*(HK47*P(0,16) + HK50*P(4,16) + HK52*P(5,16) + HK53*P(6,16) + HK54*P(1,16) + HK55*P(2,16) + HK56*P(3,16));
|
||||
Kfusion(17) = -HK66*(HK47*P(0,17) + HK50*P(4,17) + HK52*P(5,17) + HK53*P(6,17) + HK54*P(1,17) + HK55*P(2,17) + HK56*P(3,17));
|
||||
Kfusion(18) = -HK66*(HK47*P(0,18) + HK50*P(4,18) + HK52*P(5,18) + HK53*P(6,18) + HK54*P(1,18) + HK55*P(2,18) + HK56*P(3,18));
|
||||
Kfusion(19) = -HK66*(HK47*P(0,19) + HK50*P(4,19) + HK52*P(5,19) + HK53*P(6,19) + HK54*P(1,19) + HK55*P(2,19) + HK56*P(3,19));
|
||||
Kfusion(20) = -HK66*(HK47*P(0,20) + HK50*P(4,20) + HK52*P(5,20) + HK53*P(6,20) + HK54*P(1,20) + HK55*P(2,20) + HK56*P(3,20));
|
||||
Kfusion(21) = -HK66*(HK47*P(0,21) + HK50*P(4,21) + HK52*P(5,21) + HK53*P(6,21) + HK54*P(1,21) + HK55*P(2,21) + HK56*P(3,21));
|
||||
Kfusion(22) = -HK66*(HK47*P(0,22) + HK50*P(4,22) + HK52*P(5,22) + HK53*P(6,22) + HK54*P(1,22) + HK55*P(2,22) + HK56*P(3,22));
|
||||
Kfusion(23) = -HK66*(HK47*P(0,23) + HK50*P(4,23) + HK52*P(5,23) + HK53*P(6,23) + HK54*P(1,23) + HK55*P(2,23) + HK56*P(3,23));
|
||||
|
||||
|
||||
@@ -0,0 +1,222 @@
|
||||
// Sub Expressions
|
||||
const float HK0 = Tbs(1,0)*q2;
|
||||
const float HK1 = Tbs(1,1)*q1;
|
||||
const float HK2 = Tbs(1,1)*q3;
|
||||
const float HK3 = Tbs(1,2)*q2;
|
||||
const float HK4 = Tbs(1,0)*q3;
|
||||
const float HK5 = Tbs(1,2)*q1;
|
||||
const float HK6 = -HK5;
|
||||
const float HK7 = vd*(HK0 - HK1) - ve*(HK4 + HK6) + vn*(HK2 - HK3);
|
||||
const float HK8 = 1.0F/range;
|
||||
const float HK9 = 2*HK8;
|
||||
const float HK10 = Tbs(1,1)*q2;
|
||||
const float HK11 = Tbs(1,2)*q3;
|
||||
const float HK12 = Tbs(1,1)*q0;
|
||||
const float HK13 = Tbs(1,2)*q0;
|
||||
const float HK14 = vd*(HK12 + HK4 - 2*HK5) - ve*(-HK0 + 2*HK1 + HK13) + vn*(HK10 + HK11);
|
||||
const float HK15 = Tbs(1,0)*q1;
|
||||
const float HK16 = Tbs(1,0)*q0;
|
||||
const float HK17 = -vd*(HK16 - HK2 + 2*HK3) + ve*(HK11 + HK15) + vn*(-2*HK0 + HK1 + HK13);
|
||||
const float HK18 = vd*(HK10 + HK15) + ve*(HK16 - 2*HK2 + HK3) - vn*(HK12 + 2*HK4 + HK6);
|
||||
const float HK19 = q0*q2;
|
||||
const float HK20 = q1*q3;
|
||||
const float HK21 = HK19 + HK20;
|
||||
const float HK22 = 2*Tbs(1,2);
|
||||
const float HK23 = q0*q3;
|
||||
const float HK24 = q1*q2;
|
||||
const float HK25 = HK23 - HK24;
|
||||
const float HK26 = 2*Tbs(1,1);
|
||||
const float HK27 = 2*powf(q2, 2);
|
||||
const float HK28 = 2*powf(q3, 2);
|
||||
const float HK29 = HK28 - 1;
|
||||
const float HK30 = HK27 + HK29;
|
||||
const float HK31 = -HK21*HK22 + HK25*HK26 + HK30*Tbs(1,0);
|
||||
const float HK32 = HK23 + HK24;
|
||||
const float HK33 = 2*Tbs(1,0);
|
||||
const float HK34 = q0*q1;
|
||||
const float HK35 = q2*q3;
|
||||
const float HK36 = HK34 - HK35;
|
||||
const float HK37 = 2*powf(q1, 2);
|
||||
const float HK38 = HK29 + HK37;
|
||||
const float HK39 = HK22*HK36 - HK32*HK33 + HK38*Tbs(1,1);
|
||||
const float HK40 = HK34 + HK35;
|
||||
const float HK41 = HK19 - HK20;
|
||||
const float HK42 = HK27 + HK37 - 1;
|
||||
const float HK43 = -HK26*HK40 + HK33*HK41 + HK42*Tbs(1,2);
|
||||
const float HK44 = 2*HK7;
|
||||
const float HK45 = 2*HK14;
|
||||
const float HK46 = 2*HK17;
|
||||
const float HK47 = 2*HK18;
|
||||
const float HK48 = -HK31*P(0,4) - HK39*P(0,5) - HK43*P(0,6) - HK44*P(0,0) + HK45*P(0,1) + HK46*P(0,2) + HK47*P(0,3);
|
||||
const float HK49 = powf(range, -2);
|
||||
const float HK50 = -HK31*P(4,6) - HK39*P(5,6) - HK43*P(6,6) - HK44*P(0,6) + HK45*P(1,6) + HK46*P(2,6) + HK47*P(3,6);
|
||||
const float HK51 = -HK31*P(4,5) - HK39*P(5,5) - HK43*P(5,6) - HK44*P(0,5) + HK45*P(1,5) + HK46*P(2,5) + HK47*P(3,5);
|
||||
const float HK52 = -HK31*P(4,4) - HK39*P(4,5) - HK43*P(4,6) - HK44*P(0,4) + HK45*P(1,4) + HK46*P(2,4) + HK47*P(3,4);
|
||||
const float HK53 = -HK31*P(3,4) - HK39*P(3,5) - HK43*P(3,6) - HK44*P(0,3) + HK45*P(1,3) + HK46*P(2,3) + HK47*P(3,3);
|
||||
const float HK54 = -HK31*P(2,4) - HK39*P(2,5) - HK43*P(2,6) - HK44*P(0,2) + HK45*P(1,2) + HK46*P(2,2) + HK47*P(2,3);
|
||||
const float HK55 = -HK31*P(1,4) - HK39*P(1,5) - HK43*P(1,6) - HK44*P(0,1) + HK45*P(1,1) + HK46*P(1,2) + HK47*P(1,3);
|
||||
const float HK56 = HK8/(-HK31*HK49*HK52 - HK39*HK49*HK51 - HK43*HK49*HK50 - HK44*HK48*HK49 + HK45*HK49*HK55 + HK46*HK49*HK54 + HK47*HK49*HK53 + R_LOS);
|
||||
const float HK57 = Tbs(0,0)*q3;
|
||||
const float HK58 = Tbs(0,2)*q1;
|
||||
const float HK59 = -HK58;
|
||||
const float HK60 = ve*(HK57 + HK59);
|
||||
const float HK61 = Tbs(0,0)*q2;
|
||||
const float HK62 = Tbs(0,1)*q1;
|
||||
const float HK63 = Tbs(0,1)*q3;
|
||||
const float HK64 = Tbs(0,2)*q2;
|
||||
const float HK65 = HK60 - vd*(HK61 - HK62) - vn*(HK63 - HK64);
|
||||
const float HK66 = Tbs(0,2)*q0;
|
||||
const float HK67 = -HK61;
|
||||
const float HK68 = 2*HK62;
|
||||
const float HK69 = Tbs(0,1)*q0;
|
||||
const float HK70 = Tbs(0,1)*q2;
|
||||
const float HK71 = Tbs(0,2)*q3;
|
||||
const float HK72 = vd*(HK57 - 2*HK58 + HK69) + vn*(HK70 + HK71);
|
||||
const float HK73 = HK72 - ve*(HK66 + HK67 + HK68);
|
||||
const float HK74 = Tbs(0,0)*q0;
|
||||
const float HK75 = -HK63;
|
||||
const float HK76 = 2*HK64;
|
||||
const float HK77 = Tbs(0,0)*q1;
|
||||
const float HK78 = ve*(HK71 + HK77) + vn*(-2*HK61 + HK62 + HK66);
|
||||
const float HK79 = HK78 - vd*(HK74 + HK75 + HK76);
|
||||
const float HK80 = 2*HK57;
|
||||
const float HK81 = vd*(HK70 + HK77) + ve*(-2*HK63 + HK64 + HK74);
|
||||
const float HK82 = HK81 - vn*(HK59 + HK69 + HK80);
|
||||
const float HK83 = 2*Tbs(0,2);
|
||||
const float HK84 = HK21*HK83;
|
||||
const float HK85 = 2*Tbs(0,1);
|
||||
const float HK86 = -HK25*HK85 - HK30*Tbs(0,0) + HK84;
|
||||
const float HK87 = 2*Tbs(0,0);
|
||||
const float HK88 = HK32*HK87;
|
||||
const float HK89 = -HK36*HK83 - HK38*Tbs(0,1) + HK88;
|
||||
const float HK90 = HK40*HK85;
|
||||
const float HK91 = -HK41*HK87 - HK42*Tbs(0,2) + HK90;
|
||||
const float HK92 = 2*HK60 + 2*vd*(HK62 + HK67) + 2*vn*(HK64 + HK75);
|
||||
const float HK93 = -HK27;
|
||||
const float HK94 = 1 - HK28;
|
||||
const float HK95 = HK84 + HK85*(-HK23 + HK24) + Tbs(0,0)*(HK93 + HK94);
|
||||
const float HK96 = -HK37;
|
||||
const float HK97 = HK83*(-HK34 + HK35) + HK88 + Tbs(0,1)*(HK94 + HK96);
|
||||
const float HK98 = HK87*(-HK19 + HK20) + HK90 + Tbs(0,2)*(HK93 + HK96 + 1);
|
||||
const float HK99 = 2*HK72 + 2*ve*(HK61 - HK66 - HK68);
|
||||
const float HK100 = 2*HK78 + 2*vd*(HK63 - HK74 - HK76);
|
||||
const float HK101 = 2*HK81 + 2*vn*(HK58 - HK69 - HK80);
|
||||
const float HK102 = HK100*P(0,2) + HK101*P(0,3) + HK92*P(0,0) + HK95*P(0,4) + HK97*P(0,5) + HK98*P(0,6) + HK99*P(0,1);
|
||||
const float HK103 = 2*HK49;
|
||||
const float HK104 = HK100*P(2,6) + HK101*P(3,6) + HK92*P(0,6) + HK95*P(4,6) + HK97*P(5,6) + HK98*P(6,6) + HK99*P(1,6);
|
||||
const float HK105 = HK100*P(2,5) + HK101*P(3,5) + HK92*P(0,5) + HK95*P(4,5) + HK97*P(5,5) + HK98*P(5,6) + HK99*P(1,5);
|
||||
const float HK106 = HK100*P(2,4) + HK101*P(3,4) + HK92*P(0,4) + HK95*P(4,4) + HK97*P(4,5) + HK98*P(4,6) + HK99*P(1,4);
|
||||
const float HK107 = HK100*P(2,3) + HK101*P(3,3) + HK92*P(0,3) + HK95*P(3,4) + HK97*P(3,5) + HK98*P(3,6) + HK99*P(1,3);
|
||||
const float HK108 = HK100*P(2,2) + HK101*P(2,3) + HK92*P(0,2) + HK95*P(2,4) + HK97*P(2,5) + HK98*P(2,6) + HK99*P(1,2);
|
||||
const float HK109 = HK100*P(1,2) + HK101*P(1,3) + HK92*P(0,1) + HK95*P(1,4) + HK97*P(1,5) + HK98*P(1,6) + HK99*P(1,1);
|
||||
const float HK110 = HK8/(HK102*HK103*HK65 + HK103*HK107*HK82 + HK103*HK108*HK79 + HK103*HK109*HK73 + HK104*HK49*HK91 + HK105*HK49*HK89 + HK106*HK49*HK86 + R_LOS);
|
||||
|
||||
|
||||
// Observation Jacobians - axis 0
|
||||
Hfusion.at<0>() = -HK7*HK9;
|
||||
Hfusion.at<1>() = HK14*HK9;
|
||||
Hfusion.at<2>() = HK17*HK9;
|
||||
Hfusion.at<3>() = HK18*HK9;
|
||||
Hfusion.at<4>() = -HK31*HK8;
|
||||
Hfusion.at<5>() = -HK39*HK8;
|
||||
Hfusion.at<6>() = -HK43*HK8;
|
||||
Hfusion.at<7>() = 0;
|
||||
Hfusion.at<8>() = 0;
|
||||
Hfusion.at<9>() = 0;
|
||||
Hfusion.at<10>() = 0;
|
||||
Hfusion.at<11>() = 0;
|
||||
Hfusion.at<12>() = 0;
|
||||
Hfusion.at<13>() = 0;
|
||||
Hfusion.at<14>() = 0;
|
||||
Hfusion.at<15>() = 0;
|
||||
Hfusion.at<16>() = 0;
|
||||
Hfusion.at<17>() = 0;
|
||||
Hfusion.at<18>() = 0;
|
||||
Hfusion.at<19>() = 0;
|
||||
Hfusion.at<20>() = 0;
|
||||
Hfusion.at<21>() = 0;
|
||||
Hfusion.at<22>() = 0;
|
||||
Hfusion.at<23>() = 0;
|
||||
|
||||
|
||||
// Kalman gains - axis 0
|
||||
Kfusion(0) = HK48*HK56;
|
||||
Kfusion(1) = HK55*HK56;
|
||||
Kfusion(2) = HK54*HK56;
|
||||
Kfusion(3) = HK53*HK56;
|
||||
Kfusion(4) = HK52*HK56;
|
||||
Kfusion(5) = HK51*HK56;
|
||||
Kfusion(6) = HK50*HK56;
|
||||
Kfusion(7) = HK56*(-HK31*P(4,7) - HK39*P(5,7) - HK43*P(6,7) - HK44*P(0,7) + HK45*P(1,7) + HK46*P(2,7) + HK47*P(3,7));
|
||||
Kfusion(8) = HK56*(-HK31*P(4,8) - HK39*P(5,8) - HK43*P(6,8) - HK44*P(0,8) + HK45*P(1,8) + HK46*P(2,8) + HK47*P(3,8));
|
||||
Kfusion(9) = HK56*(-HK31*P(4,9) - HK39*P(5,9) - HK43*P(6,9) - HK44*P(0,9) + HK45*P(1,9) + HK46*P(2,9) + HK47*P(3,9));
|
||||
Kfusion(10) = HK56*(-HK31*P(4,10) - HK39*P(5,10) - HK43*P(6,10) - HK44*P(0,10) + HK45*P(1,10) + HK46*P(2,10) + HK47*P(3,10));
|
||||
Kfusion(11) = HK56*(-HK31*P(4,11) - HK39*P(5,11) - HK43*P(6,11) - HK44*P(0,11) + HK45*P(1,11) + HK46*P(2,11) + HK47*P(3,11));
|
||||
Kfusion(12) = HK56*(-HK31*P(4,12) - HK39*P(5,12) - HK43*P(6,12) - HK44*P(0,12) + HK45*P(1,12) + HK46*P(2,12) + HK47*P(3,12));
|
||||
Kfusion(13) = HK56*(-HK31*P(4,13) - HK39*P(5,13) - HK43*P(6,13) - HK44*P(0,13) + HK45*P(1,13) + HK46*P(2,13) + HK47*P(3,13));
|
||||
Kfusion(14) = HK56*(-HK31*P(4,14) - HK39*P(5,14) - HK43*P(6,14) - HK44*P(0,14) + HK45*P(1,14) + HK46*P(2,14) + HK47*P(3,14));
|
||||
Kfusion(15) = HK56*(-HK31*P(4,15) - HK39*P(5,15) - HK43*P(6,15) - HK44*P(0,15) + HK45*P(1,15) + HK46*P(2,15) + HK47*P(3,15));
|
||||
Kfusion(16) = HK56*(-HK31*P(4,16) - HK39*P(5,16) - HK43*P(6,16) - HK44*P(0,16) + HK45*P(1,16) + HK46*P(2,16) + HK47*P(3,16));
|
||||
Kfusion(17) = HK56*(-HK31*P(4,17) - HK39*P(5,17) - HK43*P(6,17) - HK44*P(0,17) + HK45*P(1,17) + HK46*P(2,17) + HK47*P(3,17));
|
||||
Kfusion(18) = HK56*(-HK31*P(4,18) - HK39*P(5,18) - HK43*P(6,18) - HK44*P(0,18) + HK45*P(1,18) + HK46*P(2,18) + HK47*P(3,18));
|
||||
Kfusion(19) = HK56*(-HK31*P(4,19) - HK39*P(5,19) - HK43*P(6,19) - HK44*P(0,19) + HK45*P(1,19) + HK46*P(2,19) + HK47*P(3,19));
|
||||
Kfusion(20) = HK56*(-HK31*P(4,20) - HK39*P(5,20) - HK43*P(6,20) - HK44*P(0,20) + HK45*P(1,20) + HK46*P(2,20) + HK47*P(3,20));
|
||||
Kfusion(21) = HK56*(-HK31*P(4,21) - HK39*P(5,21) - HK43*P(6,21) - HK44*P(0,21) + HK45*P(1,21) + HK46*P(2,21) + HK47*P(3,21));
|
||||
Kfusion(22) = HK56*(-HK31*P(4,22) - HK39*P(5,22) - HK43*P(6,22) - HK44*P(0,22) + HK45*P(1,22) + HK46*P(2,22) + HK47*P(3,22));
|
||||
Kfusion(23) = HK56*(-HK31*P(4,23) - HK39*P(5,23) - HK43*P(6,23) - HK44*P(0,23) + HK45*P(1,23) + HK46*P(2,23) + HK47*P(3,23));
|
||||
|
||||
|
||||
// Observation Jacobians - axis 1
|
||||
Hfusion.at<0>() = -HK65*HK9;
|
||||
Hfusion.at<1>() = -HK73*HK9;
|
||||
Hfusion.at<2>() = -HK79*HK9;
|
||||
Hfusion.at<3>() = -HK82*HK9;
|
||||
Hfusion.at<4>() = -HK8*HK86;
|
||||
Hfusion.at<5>() = -HK8*HK89;
|
||||
Hfusion.at<6>() = -HK8*HK91;
|
||||
Hfusion.at<7>() = 0;
|
||||
Hfusion.at<8>() = 0;
|
||||
Hfusion.at<9>() = 0;
|
||||
Hfusion.at<10>() = 0;
|
||||
Hfusion.at<11>() = 0;
|
||||
Hfusion.at<12>() = 0;
|
||||
Hfusion.at<13>() = 0;
|
||||
Hfusion.at<14>() = 0;
|
||||
Hfusion.at<15>() = 0;
|
||||
Hfusion.at<16>() = 0;
|
||||
Hfusion.at<17>() = 0;
|
||||
Hfusion.at<18>() = 0;
|
||||
Hfusion.at<19>() = 0;
|
||||
Hfusion.at<20>() = 0;
|
||||
Hfusion.at<21>() = 0;
|
||||
Hfusion.at<22>() = 0;
|
||||
Hfusion.at<23>() = 0;
|
||||
|
||||
|
||||
// Kalman gains - axis 1
|
||||
Kfusion(0) = -HK102*HK110;
|
||||
Kfusion(1) = -HK109*HK110;
|
||||
Kfusion(2) = -HK108*HK110;
|
||||
Kfusion(3) = -HK107*HK110;
|
||||
Kfusion(4) = -HK106*HK110;
|
||||
Kfusion(5) = -HK105*HK110;
|
||||
Kfusion(6) = -HK104*HK110;
|
||||
Kfusion(7) = -HK110*(HK100*P(2,7) + HK101*P(3,7) + HK92*P(0,7) + HK95*P(4,7) + HK97*P(5,7) + HK98*P(6,7) + HK99*P(1,7));
|
||||
Kfusion(8) = -HK110*(HK100*P(2,8) + HK101*P(3,8) + HK92*P(0,8) + HK95*P(4,8) + HK97*P(5,8) + HK98*P(6,8) + HK99*P(1,8));
|
||||
Kfusion(9) = -HK110*(HK100*P(2,9) + HK101*P(3,9) + HK92*P(0,9) + HK95*P(4,9) + HK97*P(5,9) + HK98*P(6,9) + HK99*P(1,9));
|
||||
Kfusion(10) = -HK110*(HK100*P(2,10) + HK101*P(3,10) + HK92*P(0,10) + HK95*P(4,10) + HK97*P(5,10) + HK98*P(6,10) + HK99*P(1,10));
|
||||
Kfusion(11) = -HK110*(HK100*P(2,11) + HK101*P(3,11) + HK92*P(0,11) + HK95*P(4,11) + HK97*P(5,11) + HK98*P(6,11) + HK99*P(1,11));
|
||||
Kfusion(12) = -HK110*(HK100*P(2,12) + HK101*P(3,12) + HK92*P(0,12) + HK95*P(4,12) + HK97*P(5,12) + HK98*P(6,12) + HK99*P(1,12));
|
||||
Kfusion(13) = -HK110*(HK100*P(2,13) + HK101*P(3,13) + HK92*P(0,13) + HK95*P(4,13) + HK97*P(5,13) + HK98*P(6,13) + HK99*P(1,13));
|
||||
Kfusion(14) = -HK110*(HK100*P(2,14) + HK101*P(3,14) + HK92*P(0,14) + HK95*P(4,14) + HK97*P(5,14) + HK98*P(6,14) + HK99*P(1,14));
|
||||
Kfusion(15) = -HK110*(HK100*P(2,15) + HK101*P(3,15) + HK92*P(0,15) + HK95*P(4,15) + HK97*P(5,15) + HK98*P(6,15) + HK99*P(1,15));
|
||||
Kfusion(16) = -HK110*(HK100*P(2,16) + HK101*P(3,16) + HK92*P(0,16) + HK95*P(4,16) + HK97*P(5,16) + HK98*P(6,16) + HK99*P(1,16));
|
||||
Kfusion(17) = -HK110*(HK100*P(2,17) + HK101*P(3,17) + HK92*P(0,17) + HK95*P(4,17) + HK97*P(5,17) + HK98*P(6,17) + HK99*P(1,17));
|
||||
Kfusion(18) = -HK110*(HK100*P(2,18) + HK101*P(3,18) + HK92*P(0,18) + HK95*P(4,18) + HK97*P(5,18) + HK98*P(6,18) + HK99*P(1,18));
|
||||
Kfusion(19) = -HK110*(HK100*P(2,19) + HK101*P(3,19) + HK92*P(0,19) + HK95*P(4,19) + HK97*P(5,19) + HK98*P(6,19) + HK99*P(1,19));
|
||||
Kfusion(20) = -HK110*(HK100*P(2,20) + HK101*P(3,20) + HK92*P(0,20) + HK95*P(4,20) + HK97*P(5,20) + HK98*P(6,20) + HK99*P(1,20));
|
||||
Kfusion(21) = -HK110*(HK100*P(2,21) + HK101*P(3,21) + HK92*P(0,21) + HK95*P(4,21) + HK97*P(5,21) + HK98*P(6,21) + HK99*P(1,21));
|
||||
Kfusion(22) = -HK110*(HK100*P(2,22) + HK101*P(3,22) + HK92*P(0,22) + HK95*P(4,22) + HK97*P(5,22) + HK98*P(6,22) + HK99*P(1,22));
|
||||
Kfusion(23) = -HK110*(HK100*P(2,23) + HK101*P(3,23) + HK92*P(0,23) + HK95*P(4,23) + HK97*P(5,23) + HK98*P(6,23) + HK99*P(1,23));
|
||||
|
||||
|
||||
+266
@@ -0,0 +1,266 @@
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <cstdlib>
|
||||
#include "../../../../../matrix/matrix/math.hpp"
|
||||
|
||||
typedef matrix::Vector<float, 24> Vector24f;
|
||||
typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
|
||||
template<int ... Idxs>
|
||||
using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
|
||||
|
||||
float sq(float in) {
|
||||
return in * in;
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
// Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations
|
||||
|
||||
float H_YAW[24];
|
||||
Vector24f Kfusion;
|
||||
float _heading_innov_var;
|
||||
|
||||
const float R_YAW = sq(0.3f);
|
||||
|
||||
const float _gps_yaw_offset = 1.5f;
|
||||
|
||||
// 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;
|
||||
|
||||
// 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// First calculate observationjacobians and Kalman gains using sympy generated equations
|
||||
|
||||
// calculate intermediate variables
|
||||
const float HK0 = sinf(_gps_yaw_offset);
|
||||
const float HK1 = q0*q3;
|
||||
const float HK2 = q1*q2;
|
||||
const float HK3 = 2*HK0*(HK1 - HK2);
|
||||
const float HK4 = cosf(_gps_yaw_offset);
|
||||
const float HK5 = powf(q1, 2);
|
||||
const float HK6 = powf(q2, 2);
|
||||
const float HK7 = powf(q0, 2) - powf(q3, 2);
|
||||
const float HK8 = HK4*(HK5 - HK6 + HK7);
|
||||
const float HK9 = HK3 - HK8;
|
||||
if (fabsf(HK9) < 1e-3f) {
|
||||
return 0;
|
||||
}
|
||||
const float HK10 = 1.0F/HK9;
|
||||
const float HK11 = HK4*q0;
|
||||
const float HK12 = HK0*q3;
|
||||
const float HK13 = HK0*(-HK5 + HK6 + HK7) + 2*HK4*(HK1 + HK2);
|
||||
const float HK14 = HK10*HK13;
|
||||
const float HK15 = HK0*q0 + HK4*q3;
|
||||
const float HK16 = HK10*(HK14*(HK11 - HK12) + HK15);
|
||||
const float HK17 = powf(HK13, 2)/powf(HK9, 2) + 1;
|
||||
if (fabsf(HK17) < 1e-3f) {
|
||||
return 0;
|
||||
}
|
||||
const float HK18 = 2/HK17;
|
||||
// const float HK19 = 1.0F/(-HK3 + HK8);
|
||||
const float HK19_inverse = -HK3 + HK8;
|
||||
if (fabsf(HK19_inverse) < 1e-6f) {
|
||||
return 0;
|
||||
}
|
||||
const float HK19 = 1.0F/HK19_inverse;
|
||||
const float HK20 = HK4*q1;
|
||||
const float HK21 = HK0*q2;
|
||||
const float HK22 = HK13*HK19;
|
||||
const float HK23 = HK0*q1 - HK4*q2;
|
||||
const float HK24 = HK19*(HK22*(HK20 + HK21) + HK23);
|
||||
const float HK25 = HK19*(-HK20 - HK21 + HK22*HK23);
|
||||
const float HK26 = HK10*(-HK11 + HK12 + HK14*HK15);
|
||||
const float HK27 = -HK16*P(0,0) - HK24*P(0,1) - HK25*P(0,2) + HK26*P(0,3);
|
||||
const float HK28 = -HK16*P(0,1) - HK24*P(1,1) - HK25*P(1,2) + HK26*P(1,3);
|
||||
const float HK29 = 4/powf(HK17, 2);
|
||||
const float HK30 = -HK16*P(0,2) - HK24*P(1,2) - HK25*P(2,2) + HK26*P(2,3);
|
||||
const float HK31 = -HK16*P(0,3) - HK24*P(1,3) - HK25*P(2,3) + HK26*P(3,3);
|
||||
const float HK32 = HK18/(-HK16*HK27*HK29 - HK24*HK28*HK29 - HK25*HK29*HK30 + HK26*HK29*HK31 + R_YAW);
|
||||
|
||||
// calculate observation jacobian
|
||||
// Observation jacobian and Kalman gain vectors
|
||||
SparseVector24f<0,1,2,3> Hfusion;
|
||||
Hfusion.at<0>() = -HK16*HK18;
|
||||
Hfusion.at<1>() = -HK18*HK24;
|
||||
Hfusion.at<2>() = -HK18*HK25;
|
||||
Hfusion.at<3>() = HK18*HK26;
|
||||
|
||||
// calculate the Kalman gains
|
||||
// only calculate gains for states we are using
|
||||
Kfusion(0) = HK27*HK32;
|
||||
Kfusion(1) = HK28*HK32;
|
||||
Kfusion(2) = HK30*HK32;
|
||||
Kfusion(3) = HK31*HK32;
|
||||
for (unsigned row = 4; row <= 23; row++) {
|
||||
Kfusion(row) = HK32*(-HK16*P(0,row) - HK24*P(1,row) - HK25*P(2,row) + HK26*P(3,row));
|
||||
}
|
||||
|
||||
// save output and repeat calculation using legacy matlab generated code
|
||||
float Hfusion_sympy[24] = {};
|
||||
Vector24f Kfusion_sympy;
|
||||
Hfusion_sympy[0] = Hfusion.at<0>();
|
||||
Hfusion_sympy[1] = Hfusion.at<1>();
|
||||
Hfusion_sympy[2] = Hfusion.at<2>();
|
||||
Hfusion_sympy[3] = Hfusion.at<3>();
|
||||
|
||||
for (int row=0; row<24; row++) {
|
||||
Kfusion_sympy(row) = Kfusion(row);
|
||||
}
|
||||
|
||||
// repeat calculation using matlab generated equations
|
||||
// calculate observation jacobian
|
||||
float t2 = sinf(_gps_yaw_offset);
|
||||
float t3 = cosf(_gps_yaw_offset);
|
||||
float t4 = q0*q3*2.0f;
|
||||
float t5 = q0*q0;
|
||||
float t6 = q1*q1;
|
||||
float t7 = q2*q2;
|
||||
float t8 = q3*q3;
|
||||
float t9 = q1*q2*2.0f;
|
||||
float t10 = t5+t6-t7-t8;
|
||||
float t11 = t3*t10;
|
||||
float t12 = t4+t9;
|
||||
float t13 = t3*t12;
|
||||
float t14 = t5-t6+t7-t8;
|
||||
float t15 = t2*t14;
|
||||
float t16 = t13+t15;
|
||||
float t17 = t4-t9;
|
||||
float t19 = t2*t17;
|
||||
float t20 = t11-t19;
|
||||
float t18 = (t20*t20);
|
||||
|
||||
t18 = 1.0f / t18;
|
||||
float t21 = t16*t16;
|
||||
float t22 = sq(t11-t19);
|
||||
|
||||
t22 = 1.0f/t22;
|
||||
float t23 = q1*t3*2.0f;
|
||||
float t24 = q2*t2*2.0f;
|
||||
float t25 = t23+t24;
|
||||
float t26 = 1.0f/t20;
|
||||
float t27 = q1*t2*2.0f;
|
||||
float t28 = t21*t22;
|
||||
float t29 = t28+1.0f;
|
||||
|
||||
float t30 = 1.0f/t29;
|
||||
float t31 = q0*t3*2.0f;
|
||||
float t32 = t31-q3*t2*2.0f;
|
||||
float t33 = q3*t3*2.0f;
|
||||
float t34 = q0*t2*2.0f;
|
||||
float t35 = t33+t34;
|
||||
|
||||
memset(&H_YAW, 0, sizeof(H_YAW));
|
||||
H_YAW[0] = (t35/(t11-t2*(t4-q1*q2*2.0f))-t16*t18*t32)/(t18*t21+1.0f);
|
||||
H_YAW[1] = -t30*(t26*(t27-q2*t3*2.0f)+t16*t22*t25);
|
||||
H_YAW[2] = t30*(t25*t26-t16*t22*(t27-q2*t3*2.0f));
|
||||
H_YAW[3] = t30*(t26*t32+t16*t22*t35);
|
||||
|
||||
// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 3 elements in H are non zero
|
||||
// calculate the innovation variance
|
||||
float PH[4];
|
||||
_heading_innov_var = R_YAW;
|
||||
|
||||
for (unsigned row = 0; row <= 3; row++) {
|
||||
PH[row] = 0.0f;
|
||||
|
||||
for (uint8_t col = 0; col <= 3; col++) {
|
||||
PH[row] += P(row,col) * H_YAW[col];
|
||||
}
|
||||
|
||||
_heading_innov_var += H_YAW[row] * PH[row];
|
||||
}
|
||||
|
||||
const float heading_innov_var_inv = 1.f / _heading_innov_var;
|
||||
|
||||
// calculate the Kalman gains
|
||||
// only calculate gains for states we are using
|
||||
memset(&Kfusion, 0, sizeof(Kfusion));
|
||||
|
||||
for (uint8_t row = 0; row <= 15; row++) {
|
||||
for (uint8_t col = 0; col <= 3; col++) {
|
||||
Kfusion(row) += P(row,col) * H_YAW[col];
|
||||
}
|
||||
Kfusion(row) *= heading_innov_var_inv;
|
||||
}
|
||||
|
||||
if (true) {
|
||||
for (uint8_t row = 22; row <= 23; row++) {
|
||||
for (uint8_t col = 0; col <= 3; col++) {
|
||||
Kfusion(row) += P(row,col) * H_YAW[col];
|
||||
}
|
||||
Kfusion(row) *= heading_innov_var_inv;
|
||||
}
|
||||
}
|
||||
|
||||
// find largest observation variance difference as a fraction of the matlab value
|
||||
float max_diff_fraction = 0.0f;
|
||||
int max_row;
|
||||
float max_old, max_new;
|
||||
for (int row=0; row<24; row++) {
|
||||
float diff_fraction;
|
||||
if (H_YAW[row] != 0.0f) {
|
||||
diff_fraction = fabsf(Hfusion_sympy[row] - H_YAW[row]) / fabsf(H_YAW[row]);
|
||||
} else if (Hfusion_sympy[row] != 0.0f) {
|
||||
diff_fraction = fabsf(Hfusion_sympy[row] - H_YAW[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_YAW[row];
|
||||
max_new = Hfusion_sympy[row];
|
||||
}
|
||||
}
|
||||
|
||||
if (max_diff_fraction > 1e-5f) {
|
||||
printf("Fail: GPS yaw 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: GPS yaw 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: GPS yaw 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: GPS yaw Kfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,84 @@
|
||||
// Sub Expressions
|
||||
const float HK0 = cosf(ant_yaw);
|
||||
const float HK1 = sinf(ant_yaw);
|
||||
const float HK2 = q0*q3;
|
||||
const float HK3 = q1*q2;
|
||||
const float HK4 = 2*HK1;
|
||||
const float HK5 = 2*powf(q3, 2) - 1;
|
||||
const float HK6 = HK0*(HK5 + 2*powf(q2, 2)) + HK4*(HK2 - HK3);
|
||||
const float HK7 = 1.0F/HK6;
|
||||
const float HK8 = 2*HK0;
|
||||
const float HK9 = -HK1*(HK5 + 2*powf(q1, 2)) + HK8*(HK2 + HK3);
|
||||
const float HK10 = HK7*HK9;
|
||||
const float HK11 = HK1*HK10;
|
||||
const float HK12 = q3*(-HK0 + HK11);
|
||||
const float HK13 = powf(HK6, -2);
|
||||
const float HK14 = HK13*powf(HK9, 2) + 1;
|
||||
const float HK15 = 2*HK7/HK14;
|
||||
const float HK16 = HK0*q2;
|
||||
const float HK17 = HK1*q1;
|
||||
const float HK18 = HK11*q2 + HK16 - 2*HK17;
|
||||
const float HK19 = HK0*q1 + HK10*(-2*HK16 + HK17);
|
||||
const float HK20 = -HK0*q0 + HK10*(HK1*q0 + HK8*q3) + HK4*q3;
|
||||
const float HK21 = HK12*P(0,0) - HK18*P(0,1) - HK19*P(0,2) + HK20*P(0,3);
|
||||
const float HK22 = 4*HK13/powf(HK14, 2);
|
||||
const float HK23 = HK12*P(0,1) - HK18*P(1,1) - HK19*P(1,2) + HK20*P(1,3);
|
||||
const float HK24 = HK12*P(0,2) - HK18*P(1,2) - HK19*P(2,2) + HK20*P(2,3);
|
||||
const float HK25 = HK12*P(0,3) - HK18*P(1,3) - HK19*P(2,3) + HK20*P(3,3);
|
||||
const float HK26 = HK15/(HK12*HK21*HK22 - HK18*HK22*HK23 - HK19*HK22*HK24 + HK20*HK22*HK25 + R_YAW);
|
||||
|
||||
|
||||
// Observation Jacobians
|
||||
Hfusion.at<0>() = HK12*HK15;
|
||||
Hfusion.at<1>() = -HK15*HK18;
|
||||
Hfusion.at<2>() = -HK15*HK19;
|
||||
Hfusion.at<3>() = HK15*HK20;
|
||||
Hfusion.at<4>() = 0;
|
||||
Hfusion.at<5>() = 0;
|
||||
Hfusion.at<6>() = 0;
|
||||
Hfusion.at<7>() = 0;
|
||||
Hfusion.at<8>() = 0;
|
||||
Hfusion.at<9>() = 0;
|
||||
Hfusion.at<10>() = 0;
|
||||
Hfusion.at<11>() = 0;
|
||||
Hfusion.at<12>() = 0;
|
||||
Hfusion.at<13>() = 0;
|
||||
Hfusion.at<14>() = 0;
|
||||
Hfusion.at<15>() = 0;
|
||||
Hfusion.at<16>() = 0;
|
||||
Hfusion.at<17>() = 0;
|
||||
Hfusion.at<18>() = 0;
|
||||
Hfusion.at<19>() = 0;
|
||||
Hfusion.at<20>() = 0;
|
||||
Hfusion.at<21>() = 0;
|
||||
Hfusion.at<22>() = 0;
|
||||
Hfusion.at<23>() = 0;
|
||||
|
||||
|
||||
// Kalman gains
|
||||
Kfusion(0) = HK21*HK26;
|
||||
Kfusion(1) = HK23*HK26;
|
||||
Kfusion(2) = HK24*HK26;
|
||||
Kfusion(3) = HK25*HK26;
|
||||
Kfusion(4) = HK26*(HK12*P(0,4) - HK18*P(1,4) - HK19*P(2,4) + HK20*P(3,4));
|
||||
Kfusion(5) = HK26*(HK12*P(0,5) - HK18*P(1,5) - HK19*P(2,5) + HK20*P(3,5));
|
||||
Kfusion(6) = HK26*(HK12*P(0,6) - HK18*P(1,6) - HK19*P(2,6) + HK20*P(3,6));
|
||||
Kfusion(7) = HK26*(HK12*P(0,7) - HK18*P(1,7) - HK19*P(2,7) + HK20*P(3,7));
|
||||
Kfusion(8) = HK26*(HK12*P(0,8) - HK18*P(1,8) - HK19*P(2,8) + HK20*P(3,8));
|
||||
Kfusion(9) = HK26*(HK12*P(0,9) - HK18*P(1,9) - HK19*P(2,9) + HK20*P(3,9));
|
||||
Kfusion(10) = HK26*(HK12*P(0,10) - HK18*P(1,10) - HK19*P(2,10) + HK20*P(3,10));
|
||||
Kfusion(11) = HK26*(HK12*P(0,11) - HK18*P(1,11) - HK19*P(2,11) + HK20*P(3,11));
|
||||
Kfusion(12) = HK26*(HK12*P(0,12) - HK18*P(1,12) - HK19*P(2,12) + HK20*P(3,12));
|
||||
Kfusion(13) = HK26*(HK12*P(0,13) - HK18*P(1,13) - HK19*P(2,13) + HK20*P(3,13));
|
||||
Kfusion(14) = HK26*(HK12*P(0,14) - HK18*P(1,14) - HK19*P(2,14) + HK20*P(3,14));
|
||||
Kfusion(15) = HK26*(HK12*P(0,15) - HK18*P(1,15) - HK19*P(2,15) + HK20*P(3,15));
|
||||
Kfusion(16) = HK26*(HK12*P(0,16) - HK18*P(1,16) - HK19*P(2,16) + HK20*P(3,16));
|
||||
Kfusion(17) = HK26*(HK12*P(0,17) - HK18*P(1,17) - HK19*P(2,17) + HK20*P(3,17));
|
||||
Kfusion(18) = HK26*(HK12*P(0,18) - HK18*P(1,18) - HK19*P(2,18) + HK20*P(3,18));
|
||||
Kfusion(19) = HK26*(HK12*P(0,19) - HK18*P(1,19) - HK19*P(2,19) + HK20*P(3,19));
|
||||
Kfusion(20) = HK26*(HK12*P(0,20) - HK18*P(1,20) - HK19*P(2,20) + HK20*P(3,20));
|
||||
Kfusion(21) = HK26*(HK12*P(0,21) - HK18*P(1,21) - HK19*P(2,21) + HK20*P(3,21));
|
||||
Kfusion(22) = HK26*(HK12*P(0,22) - HK18*P(1,22) - HK19*P(2,22) + HK20*P(3,22));
|
||||
Kfusion(23) = HK26*(HK12*P(0,23) - HK18*P(1,23) - HK19*P(2,23) + HK20*P(3,23));
|
||||
|
||||
|
||||
+227
@@ -0,0 +1,227 @@
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <cstdlib>
|
||||
#include "../../../../../matrix/matrix/math.hpp"
|
||||
|
||||
typedef matrix::Vector<float, 24> Vector24f;
|
||||
typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
|
||||
|
||||
float sq(float in) {
|
||||
return in * in;
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
// Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations
|
||||
|
||||
float Hfusion[24] = {};
|
||||
Vector24f H_DECL;
|
||||
Vector24f Kfusion;
|
||||
float decl_innov_var;
|
||||
|
||||
const float R_DECL = sq(0.3f);
|
||||
|
||||
const float _gps_yaw_offset = 1.5f;
|
||||
|
||||
// quaternion inputs must be normalised
|
||||
float q0 = 2.0f * ((float)rand() - 0.5f);
|
||||
float q1 = 2.0f * ((float)rand() - 0.5f);
|
||||
float q2 = 2.0f * ((float)rand() - 0.5f);
|
||||
float q3 = 2.0f * ((float)rand() - 0.5f);
|
||||
const float length = sqrtf(sq(q0) + sq(q1) + sq(q2) + sq(q3));
|
||||
q0 /= length;
|
||||
q1 /= length;
|
||||
q2 /= length;
|
||||
q3 /= length;
|
||||
|
||||
const float magN = 0.04f;
|
||||
const float magE = -0.03f;
|
||||
|
||||
const float h_field_min = 1e-3f;
|
||||
|
||||
// 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// First calculate observationjacobians and Kalman gains using sympy generated equations
|
||||
|
||||
// Calculate intermediate variables
|
||||
const float magN_sq = sq(magN);
|
||||
if (magN_sq < sq(h_field_min)) {
|
||||
printf("bad numerical conditioning\n");
|
||||
return 0;
|
||||
}
|
||||
const float HK0 = 1.0F / magN_sq;
|
||||
const float HK1 = HK0*sq(magE) + 1.0F;
|
||||
const float HK2 = 1.0F/HK1;
|
||||
const float HK3 = 1.0F/magN;
|
||||
const float HK4 = HK2*HK3;
|
||||
const float HK5 = HK3*magE;
|
||||
const float HK6 = HK5*P(16,17) - P(17,17);
|
||||
const float HK7 = 1.0F/sq(HK1);
|
||||
const float HK8 = HK5*P(16,16) - P(16,17);
|
||||
const float innovation_variance = -HK0*HK6*HK7 + HK7*HK8*magE/(magN * magN_sq) + R_DECL;
|
||||
float HK9;
|
||||
if (innovation_variance > R_DECL) {
|
||||
HK9 = HK4/innovation_variance;
|
||||
} else {
|
||||
printf("bad numerical conditioning\n");
|
||||
}
|
||||
|
||||
// Calculate the observation Jacobian
|
||||
// Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost
|
||||
Hfusion[16] = -HK0*HK2*magE;
|
||||
Hfusion[17] = HK4;
|
||||
|
||||
// Calculate the Kalman gains
|
||||
for (unsigned row = 0; row <= 15; row++) {
|
||||
Kfusion(row) = -HK9*(HK5*P(row,16) - P(row,17));
|
||||
}
|
||||
|
||||
Kfusion(16) = -HK8*HK9;
|
||||
Kfusion(17) = -HK6*HK9;
|
||||
|
||||
for (unsigned row = 18; row <= 23; row++) {
|
||||
Kfusion(row) = -HK9*(HK5*P(16,row) - P(17,row));
|
||||
}
|
||||
|
||||
// save output and repeat calculation using legacy matlab generated code
|
||||
float Hfusion_sympy[24];
|
||||
Vector24f Kfusion_sympy;
|
||||
for (int row=0; row<24; row++) {
|
||||
Hfusion_sympy[row] = Hfusion[row];
|
||||
Kfusion_sympy(row) = Kfusion(row);
|
||||
}
|
||||
|
||||
// repeat calculation using matlab generated equations
|
||||
// Calculate intermediate variables
|
||||
float t2 = magE*magE;
|
||||
float t3 = magN*magN;
|
||||
float t4 = t2+t3;
|
||||
// if the horizontal magnetic field is too small, this calculation will be badly conditioned
|
||||
if (t4 < h_field_min*h_field_min) {
|
||||
printf("bad numerical conditioning\n");
|
||||
return 0;
|
||||
}
|
||||
float t5 = P(16,16)*t2;
|
||||
float t6 = P(17,17)*t3;
|
||||
float t7 = t2*t2;
|
||||
float t8 = R_DECL*t7;
|
||||
float t9 = t3*t3;
|
||||
float t10 = R_DECL*t9;
|
||||
float t11 = R_DECL*t2*t3*2.0f;
|
||||
float t14 = P(16,17)*magE*magN;
|
||||
float t15 = P(17,16)*magE*magN;
|
||||
float t12 = t5+t6+t8+t10+t11-t14-t15;
|
||||
float t13;
|
||||
if (fabsf(t12) > 1e-6f) {
|
||||
t13 = 1.0f / t12;
|
||||
} else {
|
||||
printf("bad numerical conditioning\n");
|
||||
return 0;
|
||||
}
|
||||
float t18 = magE*magE;
|
||||
float t19 = magN*magN;
|
||||
float t20 = t18+t19;
|
||||
float t21;
|
||||
if (fabsf(t20) > 1e-6f) {
|
||||
t21 = 1.0f/t20;
|
||||
} else {
|
||||
printf("bad numerical conditioning\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Calculate the observation Jacobian
|
||||
// Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost
|
||||
memset(&H_DECL, 0, sizeof(H_DECL));
|
||||
H_DECL(16) = -magE*t21;
|
||||
H_DECL(17) = magN*t21;
|
||||
|
||||
// Calculate the Kalman gains
|
||||
Kfusion(0) = -t4*t13*(P(0,16)*magE-P(0,17)*magN);
|
||||
Kfusion(1) = -t4*t13*(P(1,16)*magE-P(1,17)*magN);
|
||||
Kfusion(2) = -t4*t13*(P(2,16)*magE-P(2,17)*magN);
|
||||
Kfusion(3) = -t4*t13*(P(3,16)*magE-P(3,17)*magN);
|
||||
Kfusion(4) = -t4*t13*(P(4,16)*magE-P(4,17)*magN);
|
||||
Kfusion(5) = -t4*t13*(P(5,16)*magE-P(5,17)*magN);
|
||||
Kfusion(6) = -t4*t13*(P(6,16)*magE-P(6,17)*magN);
|
||||
Kfusion(7) = -t4*t13*(P(7,16)*magE-P(7,17)*magN);
|
||||
Kfusion(8) = -t4*t13*(P(8,16)*magE-P(8,17)*magN);
|
||||
Kfusion(9) = -t4*t13*(P(9,16)*magE-P(9,17)*magN);
|
||||
Kfusion(10) = -t4*t13*(P(10,16)*magE-P(10,17)*magN);
|
||||
Kfusion(11) = -t4*t13*(P(11,16)*magE-P(11,17)*magN);
|
||||
Kfusion(12) = -t4*t13*(P(12,16)*magE-P(12,17)*magN);
|
||||
Kfusion(13) = -t4*t13*(P(13,16)*magE-P(13,17)*magN);
|
||||
Kfusion(14) = -t4*t13*(P(14,16)*magE-P(14,17)*magN);
|
||||
Kfusion(15) = -t4*t13*(P(15,16)*magE-P(15,17)*magN);
|
||||
Kfusion(16) = -t4*t13*(P(16,16)*magE-P(16,17)*magN);
|
||||
Kfusion(17) = -t4*t13*(P(17,16)*magE-P(17,17)*magN);
|
||||
Kfusion(18) = -t4*t13*(P(18,16)*magE-P(18,17)*magN);
|
||||
Kfusion(19) = -t4*t13*(P(19,16)*magE-P(19,17)*magN);
|
||||
Kfusion(20) = -t4*t13*(P(20,16)*magE-P(20,17)*magN);
|
||||
Kfusion(21) = -t4*t13*(P(21,16)*magE-P(21,17)*magN);
|
||||
Kfusion(22) = -t4*t13*(P(22,16)*magE-P(22,17)*magN);
|
||||
Kfusion(23) = -t4*t13*(P(23,16)*magE-P(23,17)*magN);
|
||||
|
||||
// find largest observation variance difference as a fraction of the matlab value
|
||||
float max_diff_fraction = 0.0f;
|
||||
int max_row;
|
||||
float max_old, max_new;
|
||||
for (int row=0; row<24; row++) {
|
||||
float diff_fraction;
|
||||
if (H_DECL(row) != 0.0f) {
|
||||
diff_fraction = fabsf(Hfusion_sympy[row] - H_DECL(row)) / fabsf(H_DECL(row));
|
||||
} else if (Hfusion_sympy[row] != 0.0f) {
|
||||
diff_fraction = fabsf(Hfusion_sympy[row] - H_DECL(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_DECL(row);
|
||||
max_new = Hfusion_sympy[row];
|
||||
}
|
||||
}
|
||||
|
||||
if (max_diff_fraction > 1e-5f) {
|
||||
printf("Fail: Mag Declination 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: Mag Declination 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: Mag Declination 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: Mag Declination Kfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,67 @@
|
||||
// Sub Expressions
|
||||
const float HK0 = powf(magN, -2);
|
||||
const float HK1 = HK0*powf(magE, 2) + 1;
|
||||
const float HK2 = 1.0F/HK1;
|
||||
const float HK3 = 1.0F/magN;
|
||||
const float HK4 = HK2*HK3;
|
||||
const float HK5 = HK3*magE;
|
||||
const float HK6 = HK5*P(16,17) - P(17,17);
|
||||
const float HK7 = powf(HK1, -2);
|
||||
const float HK8 = HK5*P(16,16) - P(16,17);
|
||||
const float HK9 = HK4/(-HK0*HK6*HK7 + HK7*HK8*magE/powf(magN, 3) + R_DECL);
|
||||
|
||||
|
||||
// Observation Jacobians
|
||||
Hfusion.at<0>() = 0;
|
||||
Hfusion.at<1>() = 0;
|
||||
Hfusion.at<2>() = 0;
|
||||
Hfusion.at<3>() = 0;
|
||||
Hfusion.at<4>() = 0;
|
||||
Hfusion.at<5>() = 0;
|
||||
Hfusion.at<6>() = 0;
|
||||
Hfusion.at<7>() = 0;
|
||||
Hfusion.at<8>() = 0;
|
||||
Hfusion.at<9>() = 0;
|
||||
Hfusion.at<10>() = 0;
|
||||
Hfusion.at<11>() = 0;
|
||||
Hfusion.at<12>() = 0;
|
||||
Hfusion.at<13>() = 0;
|
||||
Hfusion.at<14>() = 0;
|
||||
Hfusion.at<15>() = 0;
|
||||
Hfusion.at<16>() = -HK0*HK2*magE;
|
||||
Hfusion.at<17>() = HK4;
|
||||
Hfusion.at<18>() = 0;
|
||||
Hfusion.at<19>() = 0;
|
||||
Hfusion.at<20>() = 0;
|
||||
Hfusion.at<21>() = 0;
|
||||
Hfusion.at<22>() = 0;
|
||||
Hfusion.at<23>() = 0;
|
||||
|
||||
|
||||
// Kalman gains
|
||||
Kfusion(0) = -HK9*(HK5*P(0,16) - P(0,17));
|
||||
Kfusion(1) = -HK9*(HK5*P(1,16) - P(1,17));
|
||||
Kfusion(2) = -HK9*(HK5*P(2,16) - P(2,17));
|
||||
Kfusion(3) = -HK9*(HK5*P(3,16) - P(3,17));
|
||||
Kfusion(4) = -HK9*(HK5*P(4,16) - P(4,17));
|
||||
Kfusion(5) = -HK9*(HK5*P(5,16) - P(5,17));
|
||||
Kfusion(6) = -HK9*(HK5*P(6,16) - P(6,17));
|
||||
Kfusion(7) = -HK9*(HK5*P(7,16) - P(7,17));
|
||||
Kfusion(8) = -HK9*(HK5*P(8,16) - P(8,17));
|
||||
Kfusion(9) = -HK9*(HK5*P(9,16) - P(9,17));
|
||||
Kfusion(10) = -HK9*(HK5*P(10,16) - P(10,17));
|
||||
Kfusion(11) = -HK9*(HK5*P(11,16) - P(11,17));
|
||||
Kfusion(12) = -HK9*(HK5*P(12,16) - P(12,17));
|
||||
Kfusion(13) = -HK9*(HK5*P(13,16) - P(13,17));
|
||||
Kfusion(14) = -HK9*(HK5*P(14,16) - P(14,17));
|
||||
Kfusion(15) = -HK9*(HK5*P(15,16) - P(15,17));
|
||||
Kfusion(16) = -HK8*HK9;
|
||||
Kfusion(17) = -HK6*HK9;
|
||||
Kfusion(18) = -HK9*(HK5*P(16,18) - P(17,18));
|
||||
Kfusion(19) = -HK9*(HK5*P(16,19) - P(17,19));
|
||||
Kfusion(20) = -HK9*(HK5*P(16,20) - P(17,20));
|
||||
Kfusion(21) = -HK9*(HK5*P(16,21) - P(17,21));
|
||||
Kfusion(22) = -HK9*(HK5*P(16,22) - P(17,22));
|
||||
Kfusion(23) = -HK9*(HK5*P(16,23) - P(17,23));
|
||||
|
||||
|
||||
+227
@@ -0,0 +1,227 @@
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <cstdlib>
|
||||
#include "../../../../../matrix/matrix/math.hpp"
|
||||
#include "util.h"
|
||||
|
||||
typedef matrix::Vector<float, 24> Vector24f;
|
||||
typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
|
||||
template<int ... Idxs>
|
||||
using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
|
||||
|
||||
int main()
|
||||
{
|
||||
// Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations
|
||||
|
||||
float airspeed_innov_var;
|
||||
|
||||
Vector24f Kfusion; // Kalman gain vector
|
||||
|
||||
Vector24f Hfusion_sympy;
|
||||
Vector24f Kfusion_sympy;
|
||||
|
||||
Vector24f Hfusion_matlab;
|
||||
Vector24f Kfusion_matlab;
|
||||
|
||||
const float R_TAS = sq(1.5f);
|
||||
|
||||
const bool update_wind_only = false;
|
||||
|
||||
// get latest velocity in earth frame
|
||||
const float vn = 9.0f;
|
||||
const float ve = 12.0f;
|
||||
const float vd = -1.5f;
|
||||
|
||||
// get latest wind velocity in earth frame
|
||||
const float vwn = -4.0f;
|
||||
const float vwe = 3.0f;
|
||||
|
||||
// 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// First calculate observationjacobians and Kalman gains using sympy generated equations
|
||||
|
||||
{
|
||||
// Intermediate variables
|
||||
const float HK0 = vn - vwn;
|
||||
const float HK1 = ve - vwe;
|
||||
const float HK2 = ecl::powf(HK0, 2) + ecl::powf(HK1, 2) + ecl::powf(vd, 2);
|
||||
const float v_tas_pred = sqrtf(HK2); // predicted airspeed
|
||||
//const float HK3 = powf(HK2, -1.0F/2.0F);
|
||||
if (v_tas_pred < 1.0f) {
|
||||
// calculation can be badly conditioned for very low airspeed values so don't fuse this time
|
||||
return 0;
|
||||
}
|
||||
const float HK3 = 1.0f / v_tas_pred;
|
||||
const float HK4 = HK0*HK3;
|
||||
const float HK5 = HK1*HK3;
|
||||
const float HK6 = 1.0F/HK2;
|
||||
const float HK7 = HK0*P(4,6) - HK0*P(6,22) + HK1*P(5,6) - HK1*P(6,23) + P(6,6)*vd;
|
||||
const float HK8 = HK1*P(5,23);
|
||||
const float HK9 = HK0*P(4,5) - HK0*P(5,22) + HK1*P(5,5) - HK8 + P(5,6)*vd;
|
||||
const float HK10 = HK1*HK6;
|
||||
const float HK11 = HK0*P(4,22);
|
||||
const float HK12 = HK0*P(4,4) - HK1*P(4,23) + HK1*P(4,5) - HK11 + P(4,6)*vd;
|
||||
const float HK13 = HK0*HK6;
|
||||
const float HK14 = -HK0*P(22,23) + HK0*P(4,23) - HK1*P(23,23) + HK8 + P(6,23)*vd;
|
||||
const float HK15 = -HK0*P(22,22) - HK1*P(22,23) + HK1*P(5,22) + HK11 + P(6,22)*vd;
|
||||
const float HK16 = HK3/(-HK10*HK14 + HK10*HK9 + HK12*HK13 - HK13*HK15 + HK6*HK7*vd + R_TAS);
|
||||
|
||||
// Observation Jacobians
|
||||
SparseVector24f<4,5,6,22,23> Hfusion;
|
||||
Hfusion.at<4>() = HK4;
|
||||
Hfusion.at<5>() = HK5;
|
||||
Hfusion.at<6>() = HK3*vd;
|
||||
Hfusion.at<22>() = -HK4;
|
||||
Hfusion.at<23>() = -HK5;
|
||||
|
||||
if (true) {
|
||||
// we have no other source of aiding, so use airspeed measurements to correct states
|
||||
for (unsigned row = 0; row <= 3; row++) {
|
||||
Kfusion(row) = HK16*(HK0*P(4,row) - HK0*P(row,22) + HK1*P(5,row) - HK1*P(row,23) + P(6,row)*vd);
|
||||
}
|
||||
|
||||
Kfusion(4) = HK12*HK16;
|
||||
Kfusion(5) = HK16*HK9;
|
||||
Kfusion(6) = HK16*HK7;
|
||||
|
||||
for (unsigned row = 7; row <= 21; row++) {
|
||||
Kfusion(row) = HK16*(HK0*P(4,row) - HK0*P(row,22) + HK1*P(5,row) - HK1*P(row,23) + P(6,row)*vd);
|
||||
}
|
||||
}
|
||||
|
||||
Kfusion(22) = HK15*HK16;
|
||||
Kfusion(23) = HK14*HK16;
|
||||
|
||||
// save output
|
||||
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 v_tas_pred = sqrtf((ve - vwe) * (ve - vwe) + (vn - vwn) * (vn - vwn) + vd * vd);
|
||||
|
||||
// intermediate variable from algebraic optimisation
|
||||
float SH_TAS[3];
|
||||
SH_TAS[0] = 1.0f/v_tas_pred;
|
||||
SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2.0f*vwe))*0.5f;
|
||||
SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2.0f*vwn))*0.5f;
|
||||
|
||||
// Observation Jacobian
|
||||
Vector24f H_TAS = {};
|
||||
H_TAS(4) = SH_TAS[2];
|
||||
H_TAS(5) = SH_TAS[1];
|
||||
H_TAS(6) = vd*SH_TAS[0];
|
||||
H_TAS(22) = -SH_TAS[2];
|
||||
H_TAS(23) = -SH_TAS[1];
|
||||
|
||||
airspeed_innov_var = (R_TAS + SH_TAS[2]*(P(4,4)*SH_TAS[2] + P(5,4)*SH_TAS[1] - P(22,4)*SH_TAS[2] - P(23,4)*SH_TAS[1] + P(6,4)*vd*SH_TAS[0]) + SH_TAS[1]*(P(4,5)*SH_TAS[2] + P(5,5)*SH_TAS[1] - P(22,5)*SH_TAS[2] - P(23,5)*SH_TAS[1] + P(6,5)*vd*SH_TAS[0]) - SH_TAS[2]*(P(4,22)*SH_TAS[2] + P(5,22)*SH_TAS[1] - P(22,22)*SH_TAS[2] - P(23,22)*SH_TAS[1] + P(6,22)*vd*SH_TAS[0]) - SH_TAS[1]*(P(4,23)*SH_TAS[2] + P(5,23)*SH_TAS[1] - P(22,23)*SH_TAS[2] - P(23,23)*SH_TAS[1] + P(6,23)*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P(4,6)*SH_TAS[2] + P(5,6)*SH_TAS[1] - P(22,6)*SH_TAS[2] - P(23,6)*SH_TAS[1] + P(6,6)*vd*SH_TAS[0]));
|
||||
|
||||
float SK_TAS[2];
|
||||
SK_TAS[0] = 1.0f / airspeed_innov_var;
|
||||
SK_TAS[1] = SH_TAS[1];
|
||||
|
||||
// Kalman gain
|
||||
Kfusion(0) = SK_TAS[0]*(P(0,4)*SH_TAS[2] - P(0,22)*SH_TAS[2] + P(0,5)*SK_TAS[1] - P(0,23)*SK_TAS[1] + P(0,6)*vd*SH_TAS[0]);
|
||||
Kfusion(1) = SK_TAS[0]*(P(1,4)*SH_TAS[2] - P(1,22)*SH_TAS[2] + P(1,5)*SK_TAS[1] - P(1,23)*SK_TAS[1] + P(1,6)*vd*SH_TAS[0]);
|
||||
Kfusion(2) = SK_TAS[0]*(P(2,4)*SH_TAS[2] - P(2,22)*SH_TAS[2] + P(2,5)*SK_TAS[1] - P(2,23)*SK_TAS[1] + P(2,6)*vd*SH_TAS[0]);
|
||||
Kfusion(3) = SK_TAS[0]*(P(3,4)*SH_TAS[2] - P(3,22)*SH_TAS[2] + P(3,5)*SK_TAS[1] - P(3,23)*SK_TAS[1] + P(3,6)*vd*SH_TAS[0]);
|
||||
Kfusion(4) = SK_TAS[0]*(P(4,4)*SH_TAS[2] - P(4,22)*SH_TAS[2] + P(4,5)*SK_TAS[1] - P(4,23)*SK_TAS[1] + P(4,6)*vd*SH_TAS[0]);
|
||||
Kfusion(5) = SK_TAS[0]*(P(5,4)*SH_TAS[2] - P(5,22)*SH_TAS[2] + P(5,5)*SK_TAS[1] - P(5,23)*SK_TAS[1] + P(5,6)*vd*SH_TAS[0]);
|
||||
Kfusion(6) = SK_TAS[0]*(P(6,4)*SH_TAS[2] - P(6,22)*SH_TAS[2] + P(6,5)*SK_TAS[1] - P(6,23)*SK_TAS[1] + P(6,6)*vd*SH_TAS[0]);
|
||||
Kfusion(7) = SK_TAS[0]*(P(7,4)*SH_TAS[2] - P(7,22)*SH_TAS[2] + P(7,5)*SK_TAS[1] - P(7,23)*SK_TAS[1] + P(7,6)*vd*SH_TAS[0]);
|
||||
Kfusion(8) = SK_TAS[0]*(P(8,4)*SH_TAS[2] - P(8,22)*SH_TAS[2] + P(8,5)*SK_TAS[1] - P(8,23)*SK_TAS[1] + P(8,6)*vd*SH_TAS[0]);
|
||||
Kfusion(9) = SK_TAS[0]*(P(9,4)*SH_TAS[2] - P(9,22)*SH_TAS[2] + P(9,5)*SK_TAS[1] - P(9,23)*SK_TAS[1] + P(9,6)*vd*SH_TAS[0]);
|
||||
Kfusion(10) = SK_TAS[0]*(P(10,4)*SH_TAS[2] - P(10,22)*SH_TAS[2] + P(10,5)*SK_TAS[1] - P(10,23)*SK_TAS[1] + P(10,6)*vd*SH_TAS[0]);
|
||||
Kfusion(11) = SK_TAS[0]*(P(11,4)*SH_TAS[2] - P(11,22)*SH_TAS[2] + P(11,5)*SK_TAS[1] - P(11,23)*SK_TAS[1] + P(11,6)*vd*SH_TAS[0]);
|
||||
Kfusion(12) = SK_TAS[0]*(P(12,4)*SH_TAS[2] - P(12,22)*SH_TAS[2] + P(12,5)*SK_TAS[1] - P(12,23)*SK_TAS[1] + P(12,6)*vd*SH_TAS[0]);
|
||||
Kfusion(13) = SK_TAS[0]*(P(13,4)*SH_TAS[2] - P(13,22)*SH_TAS[2] + P(13,5)*SK_TAS[1] - P(13,23)*SK_TAS[1] + P(13,6)*vd*SH_TAS[0]);
|
||||
Kfusion(14) = SK_TAS[0]*(P(14,4)*SH_TAS[2] - P(14,22)*SH_TAS[2] + P(14,5)*SK_TAS[1] - P(14,23)*SK_TAS[1] + P(14,6)*vd*SH_TAS[0]);
|
||||
Kfusion(15) = SK_TAS[0]*(P(15,4)*SH_TAS[2] - P(15,22)*SH_TAS[2] + P(15,5)*SK_TAS[1] - P(15,23)*SK_TAS[1] + P(15,6)*vd*SH_TAS[0]);
|
||||
Kfusion(16) = SK_TAS[0]*(P(16,4)*SH_TAS[2] - P(16,22)*SH_TAS[2] + P(16,5)*SK_TAS[1] - P(16,23)*SK_TAS[1] + P(16,6)*vd*SH_TAS[0]);
|
||||
Kfusion(17) = SK_TAS[0]*(P(17,4)*SH_TAS[2] - P(17,22)*SH_TAS[2] + P(17,5)*SK_TAS[1] - P(17,23)*SK_TAS[1] + P(17,6)*vd*SH_TAS[0]);
|
||||
Kfusion(18) = SK_TAS[0]*(P(18,4)*SH_TAS[2] - P(18,22)*SH_TAS[2] + P(18,5)*SK_TAS[1] - P(18,23)*SK_TAS[1] + P(18,6)*vd*SH_TAS[0]);
|
||||
Kfusion(19) = SK_TAS[0]*(P(19,4)*SH_TAS[2] - P(19,22)*SH_TAS[2] + P(19,5)*SK_TAS[1] - P(19,23)*SK_TAS[1] + P(19,6)*vd*SH_TAS[0]);
|
||||
Kfusion(20) = SK_TAS[0]*(P(20,4)*SH_TAS[2] - P(20,22)*SH_TAS[2] + P(20,5)*SK_TAS[1] - P(20,23)*SK_TAS[1] + P(20,6)*vd*SH_TAS[0]);
|
||||
Kfusion(21) = SK_TAS[0]*(P(21,4)*SH_TAS[2] - P(21,22)*SH_TAS[2] + P(21,5)*SK_TAS[1] - P(21,23)*SK_TAS[1] + P(21,6)*vd*SH_TAS[0]);
|
||||
Kfusion(22) = SK_TAS[0]*(P(22,4)*SH_TAS[2] - P(22,22)*SH_TAS[2] + P(22,5)*SK_TAS[1] - P(22,23)*SK_TAS[1] + P(22,6)*vd*SH_TAS[0]);
|
||||
Kfusion(23) = SK_TAS[0]*(P(23,4)*SH_TAS[2] - P(23,22)*SH_TAS[2] + P(23,5)*SK_TAS[1] - P(23,23)*SK_TAS[1] + P(23,6)*vd*SH_TAS[0]);
|
||||
|
||||
// save output;
|
||||
Hfusion_matlab = H_TAS;
|
||||
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 (Hfusion_sympy(row) - Hfusion_matlab(row) != 0.0f) {
|
||||
printf("new,old Hfusion(%i) = %e,%e\n",row,Hfusion_sympy(row),Hfusion_matlab(row));
|
||||
}
|
||||
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: Airspeed 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: Airspeed 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 (Kfusion_sympy(row) - Kfusion(row) != 0.0f) {
|
||||
// printf("new,old Kfusion(%i) = %e,%e\n",row,Kfusion_sympy(row),Kfusion(row));
|
||||
// }
|
||||
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: Airspeed 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: Airspeed Kfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,74 @@
|
||||
// Sub Expressions
|
||||
const float HK0 = vn - vwn;
|
||||
const float HK1 = ve - vwe;
|
||||
const float HK2 = powf(HK0, 2) + powf(HK1, 2) + powf(vd, 2);
|
||||
const float HK3 = powf(HK2, -1.0F/2.0F);
|
||||
const float HK4 = HK0*HK3;
|
||||
const float HK5 = HK1*HK3;
|
||||
const float HK6 = 1.0F/HK2;
|
||||
const float HK7 = HK0*P(4,6) - HK0*P(6,22) + HK1*P(5,6) - HK1*P(6,23) + P(6,6)*vd;
|
||||
const float HK8 = HK1*P(5,23);
|
||||
const float HK9 = HK0*P(4,5) - HK0*P(5,22) + HK1*P(5,5) - HK8 + P(5,6)*vd;
|
||||
const float HK10 = HK1*HK6;
|
||||
const float HK11 = HK0*P(4,22);
|
||||
const float HK12 = HK0*P(4,4) - HK1*P(4,23) + HK1*P(4,5) - HK11 + P(4,6)*vd;
|
||||
const float HK13 = HK0*HK6;
|
||||
const float HK14 = -HK0*P(22,23) + HK0*P(4,23) - HK1*P(23,23) + HK8 + P(6,23)*vd;
|
||||
const float HK15 = -HK0*P(22,22) - HK1*P(22,23) + HK1*P(5,22) + HK11 + P(6,22)*vd;
|
||||
const float HK16 = HK3/(-HK10*HK14 + HK10*HK9 + HK12*HK13 - HK13*HK15 + HK6*HK7*vd + R_TAS);
|
||||
|
||||
|
||||
// Observation Jacobians
|
||||
Hfusion.at<0>() = 0;
|
||||
Hfusion.at<1>() = 0;
|
||||
Hfusion.at<2>() = 0;
|
||||
Hfusion.at<3>() = 0;
|
||||
Hfusion.at<4>() = HK4;
|
||||
Hfusion.at<5>() = HK5;
|
||||
Hfusion.at<6>() = HK3*vd;
|
||||
Hfusion.at<7>() = 0;
|
||||
Hfusion.at<8>() = 0;
|
||||
Hfusion.at<9>() = 0;
|
||||
Hfusion.at<10>() = 0;
|
||||
Hfusion.at<11>() = 0;
|
||||
Hfusion.at<12>() = 0;
|
||||
Hfusion.at<13>() = 0;
|
||||
Hfusion.at<14>() = 0;
|
||||
Hfusion.at<15>() = 0;
|
||||
Hfusion.at<16>() = 0;
|
||||
Hfusion.at<17>() = 0;
|
||||
Hfusion.at<18>() = 0;
|
||||
Hfusion.at<19>() = 0;
|
||||
Hfusion.at<20>() = 0;
|
||||
Hfusion.at<21>() = 0;
|
||||
Hfusion.at<22>() = -HK4;
|
||||
Hfusion.at<23>() = -HK5;
|
||||
|
||||
|
||||
// Kalman gains
|
||||
Kfusion(0) = HK16*(-HK0*P(0,22) + HK0*P(0,4) - HK1*P(0,23) + HK1*P(0,5) + P(0,6)*vd);
|
||||
Kfusion(1) = HK16*(-HK0*P(1,22) + HK0*P(1,4) - HK1*P(1,23) + HK1*P(1,5) + P(1,6)*vd);
|
||||
Kfusion(2) = HK16*(-HK0*P(2,22) + HK0*P(2,4) - HK1*P(2,23) + HK1*P(2,5) + P(2,6)*vd);
|
||||
Kfusion(3) = HK16*(-HK0*P(3,22) + HK0*P(3,4) - HK1*P(3,23) + HK1*P(3,5) + P(3,6)*vd);
|
||||
Kfusion(4) = HK12*HK16;
|
||||
Kfusion(5) = HK16*HK9;
|
||||
Kfusion(6) = HK16*HK7;
|
||||
Kfusion(7) = HK16*(HK0*P(4,7) - HK0*P(7,22) + HK1*P(5,7) - HK1*P(7,23) + P(6,7)*vd);
|
||||
Kfusion(8) = HK16*(HK0*P(4,8) - HK0*P(8,22) + HK1*P(5,8) - HK1*P(8,23) + P(6,8)*vd);
|
||||
Kfusion(9) = HK16*(HK0*P(4,9) - HK0*P(9,22) + HK1*P(5,9) - HK1*P(9,23) + P(6,9)*vd);
|
||||
Kfusion(10) = HK16*(-HK0*P(10,22) + HK0*P(4,10) - HK1*P(10,23) + HK1*P(5,10) + P(6,10)*vd);
|
||||
Kfusion(11) = HK16*(-HK0*P(11,22) + HK0*P(4,11) - HK1*P(11,23) + HK1*P(5,11) + P(6,11)*vd);
|
||||
Kfusion(12) = HK16*(-HK0*P(12,22) + HK0*P(4,12) - HK1*P(12,23) + HK1*P(5,12) + P(6,12)*vd);
|
||||
Kfusion(13) = HK16*(-HK0*P(13,22) + HK0*P(4,13) - HK1*P(13,23) + HK1*P(5,13) + P(6,13)*vd);
|
||||
Kfusion(14) = HK16*(-HK0*P(14,22) + HK0*P(4,14) - HK1*P(14,23) + HK1*P(5,14) + P(6,14)*vd);
|
||||
Kfusion(15) = HK16*(-HK0*P(15,22) + HK0*P(4,15) - HK1*P(15,23) + HK1*P(5,15) + P(6,15)*vd);
|
||||
Kfusion(16) = HK16*(-HK0*P(16,22) + HK0*P(4,16) - HK1*P(16,23) + HK1*P(5,16) + P(6,16)*vd);
|
||||
Kfusion(17) = HK16*(-HK0*P(17,22) + HK0*P(4,17) - HK1*P(17,23) + HK1*P(5,17) + P(6,17)*vd);
|
||||
Kfusion(18) = HK16*(-HK0*P(18,22) + HK0*P(4,18) - HK1*P(18,23) + HK1*P(5,18) + P(6,18)*vd);
|
||||
Kfusion(19) = HK16*(-HK0*P(19,22) + HK0*P(4,19) - HK1*P(19,23) + HK1*P(5,19) + P(6,19)*vd);
|
||||
Kfusion(20) = HK16*(-HK0*P(20,22) + HK0*P(4,20) - HK1*P(20,23) + HK1*P(5,20) + P(6,20)*vd);
|
||||
Kfusion(21) = HK16*(-HK0*P(21,22) + HK0*P(4,21) - HK1*P(21,23) + HK1*P(5,21) + P(6,21)*vd);
|
||||
Kfusion(22) = HK15*HK16;
|
||||
Kfusion(23) = HK14*HK16;
|
||||
|
||||
|
||||
@@ -0,0 +1,24 @@
|
||||
#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;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,248 @@
|
||||
// axis 0
|
||||
const float HK0 = q3*ve;
|
||||
const float HK1 = q2*vd;
|
||||
const float HK2 = q2*ve + q3*vd;
|
||||
const float HK3 = q1*ve;
|
||||
const float HK4 = q0*vd;
|
||||
const float HK5 = 2*vn;
|
||||
const float HK6 = HK5*q2;
|
||||
const float HK7 = -HK5*q3 + q0*ve + q1*vd;
|
||||
const float HK8 = 2*powf(q2, 2);
|
||||
const float HK9 = 2*powf(q3, 2);
|
||||
const float HK10 = q0*q3 + q1*q2;
|
||||
const float HK11 = q1*q3;
|
||||
const float HK12 = q0*q2;
|
||||
const float HK13 = 2*HK2;
|
||||
const float HK14 = 2*HK10;
|
||||
const float HK15 = -2*HK0 + 2*HK1;
|
||||
const float HK16 = -2*HK11 + 2*HK12;
|
||||
const float HK17 = 2*HK7;
|
||||
const float HK18 = HK8 + HK9 - 1;
|
||||
const float HK19 = -2*HK3 + 2*HK4 + 2*HK6;
|
||||
const float HK20 = -HK13*P(0,1) - HK14*P(0,5) + HK15*P(0,0) + HK16*P(0,6) - HK17*P(0,3) + HK18*P(0,4) + HK19*P(0,2);
|
||||
const float HK21 = -HK13*P(1,5) - HK14*P(5,5) + HK15*P(0,5) + HK16*P(5,6) - HK17*P(3,5) + HK18*P(4,5) + HK19*P(2,5);
|
||||
const float HK22 = -HK13*P(1,1) - HK14*P(1,5) + HK15*P(0,1) + HK16*P(1,6) - HK17*P(1,3) + HK18*P(1,4) + HK19*P(1,2);
|
||||
const float HK23 = -HK13*P(1,6) - HK14*P(5,6) + HK15*P(0,6) + HK16*P(6,6) - HK17*P(3,6) + HK18*P(4,6) + HK19*P(2,6);
|
||||
const float HK24 = -HK13*P(1,4) - HK14*P(4,5) + HK15*P(0,4) + HK16*P(4,6) - HK17*P(3,4) + HK18*P(4,4) + HK19*P(2,4);
|
||||
const float HK25 = -HK13*P(1,3) - HK14*P(3,5) + HK15*P(0,3) + HK16*P(3,6) - HK17*P(3,3) + HK18*P(3,4) + HK19*P(2,3);
|
||||
const float HK26 = -HK13*P(1,2) - HK14*P(2,5) + HK15*P(0,2) + HK16*P(2,6) - HK17*P(2,3) + HK18*P(2,4) + HK19*P(2,2);
|
||||
const float HK27 = 1.0F/(-HK13*HK22 - HK14*HK21 + HK15*HK20 + HK16*HK23 - HK17*HK25 + HK18*HK24 + HK19*HK26 + R_VEL);
|
||||
|
||||
|
||||
H_VEL(0) = 2*HK0 - 2*HK1;
|
||||
H_VEL(1) = 2*HK2;
|
||||
H_VEL(2) = 2*HK3 - 2*HK4 - 2*HK6;
|
||||
H_VEL(3) = 2*HK7;
|
||||
H_VEL(4) = -HK8 - HK9 + 1;
|
||||
H_VEL(5) = 2*HK10;
|
||||
H_VEL(6) = 2*HK11 - 2*HK12;
|
||||
H_VEL(7) = 0;
|
||||
H_VEL(8) = 0;
|
||||
H_VEL(9) = 0;
|
||||
H_VEL(10) = 0;
|
||||
H_VEL(11) = 0;
|
||||
H_VEL(12) = 0;
|
||||
H_VEL(13) = 0;
|
||||
H_VEL(14) = 0;
|
||||
H_VEL(15) = 0;
|
||||
H_VEL(16) = 0;
|
||||
H_VEL(17) = 0;
|
||||
H_VEL(18) = 0;
|
||||
H_VEL(19) = 0;
|
||||
H_VEL(20) = 0;
|
||||
H_VEL(21) = 0;
|
||||
H_VEL(22) = 0;
|
||||
H_VEL(23) = 0;
|
||||
|
||||
|
||||
Kfusion(0) = -HK20*HK27;
|
||||
Kfusion(1) = -HK22*HK27;
|
||||
Kfusion(2) = -HK26*HK27;
|
||||
Kfusion(3) = -HK25*HK27;
|
||||
Kfusion(4) = -HK24*HK27;
|
||||
Kfusion(5) = -HK21*HK27;
|
||||
Kfusion(6) = -HK23*HK27;
|
||||
Kfusion(7) = -HK27*(-HK13*P(1,7) - HK14*P(5,7) + HK15*P(0,7) + HK16*P(6,7) - HK17*P(3,7) + HK18*P(4,7) + HK19*P(2,7));
|
||||
Kfusion(8) = -HK27*(-HK13*P(1,8) - HK14*P(5,8) + HK15*P(0,8) + HK16*P(6,8) - HK17*P(3,8) + HK18*P(4,8) + HK19*P(2,8));
|
||||
Kfusion(9) = -HK27*(-HK13*P(1,9) - HK14*P(5,9) + HK15*P(0,9) + HK16*P(6,9) - HK17*P(3,9) + HK18*P(4,9) + HK19*P(2,9));
|
||||
Kfusion(10) = -HK27*(-HK13*P(1,10) - HK14*P(5,10) + HK15*P(0,10) + HK16*P(6,10) - HK17*P(3,10) + HK18*P(4,10) + HK19*P(2,10));
|
||||
Kfusion(11) = -HK27*(-HK13*P(1,11) - HK14*P(5,11) + HK15*P(0,11) + HK16*P(6,11) - HK17*P(3,11) + HK18*P(4,11) + HK19*P(2,11));
|
||||
Kfusion(12) = -HK27*(-HK13*P(1,12) - HK14*P(5,12) + HK15*P(0,12) + HK16*P(6,12) - HK17*P(3,12) + HK18*P(4,12) + HK19*P(2,12));
|
||||
Kfusion(13) = -HK27*(-HK13*P(1,13) - HK14*P(5,13) + HK15*P(0,13) + HK16*P(6,13) - HK17*P(3,13) + HK18*P(4,13) + HK19*P(2,13));
|
||||
Kfusion(14) = -HK27*(-HK13*P(1,14) - HK14*P(5,14) + HK15*P(0,14) + HK16*P(6,14) - HK17*P(3,14) + HK18*P(4,14) + HK19*P(2,14));
|
||||
Kfusion(15) = -HK27*(-HK13*P(1,15) - HK14*P(5,15) + HK15*P(0,15) + HK16*P(6,15) - HK17*P(3,15) + HK18*P(4,15) + HK19*P(2,15));
|
||||
Kfusion(16) = -HK27*(-HK13*P(1,16) - HK14*P(5,16) + HK15*P(0,16) + HK16*P(6,16) - HK17*P(3,16) + HK18*P(4,16) + HK19*P(2,16));
|
||||
Kfusion(17) = -HK27*(-HK13*P(1,17) - HK14*P(5,17) + HK15*P(0,17) + HK16*P(6,17) - HK17*P(3,17) + HK18*P(4,17) + HK19*P(2,17));
|
||||
Kfusion(18) = -HK27*(-HK13*P(1,18) - HK14*P(5,18) + HK15*P(0,18) + HK16*P(6,18) - HK17*P(3,18) + HK18*P(4,18) + HK19*P(2,18));
|
||||
Kfusion(19) = -HK27*(-HK13*P(1,19) - HK14*P(5,19) + HK15*P(0,19) + HK16*P(6,19) - HK17*P(3,19) + HK18*P(4,19) + HK19*P(2,19));
|
||||
Kfusion(20) = -HK27*(-HK13*P(1,20) - HK14*P(5,20) + HK15*P(0,20) + HK16*P(6,20) - HK17*P(3,20) + HK18*P(4,20) + HK19*P(2,20));
|
||||
Kfusion(21) = -HK27*(-HK13*P(1,21) - HK14*P(5,21) + HK15*P(0,21) + HK16*P(6,21) - HK17*P(3,21) + HK18*P(4,21) + HK19*P(2,21));
|
||||
Kfusion(22) = -HK27*(-HK13*P(1,22) - HK14*P(5,22) + HK15*P(0,22) + HK16*P(6,22) - HK17*P(3,22) + HK18*P(4,22) + HK19*P(2,22));
|
||||
Kfusion(23) = -HK27*(-HK13*P(1,23) - HK14*P(5,23) + HK15*P(0,23) + HK16*P(6,23) - HK17*P(3,23) + HK18*P(4,23) + HK19*P(2,23));
|
||||
|
||||
|
||||
// axis 1
|
||||
const float HK0 = q1*vd - q3*vn;
|
||||
const float HK1 = 2*ve;
|
||||
const float HK2 = -HK1*q1 + q0*vd + q2*vn;
|
||||
const float HK3 = q1*vn + q3*vd;
|
||||
const float HK4 = q2*vd;
|
||||
const float HK5 = q0*vn;
|
||||
const float HK6 = HK1*q3;
|
||||
const float HK7 = q1*q2;
|
||||
const float HK8 = q0*q3;
|
||||
const float HK9 = 2*powf(q1, 2);
|
||||
const float HK10 = 2*powf(q3, 2);
|
||||
const float HK11 = q0*q1 + q2*q3;
|
||||
const float HK12 = 2*HK3;
|
||||
const float HK13 = 2*HK11;
|
||||
const float HK14 = 2*HK0;
|
||||
const float HK15 = -2*HK7 + 2*HK8;
|
||||
const float HK16 = 2*HK2;
|
||||
const float HK17 = -2*HK4 + 2*HK5 + 2*HK6;
|
||||
const float HK18 = HK10 + HK9 - 1;
|
||||
const float HK19 = HK12*P(0,2) + HK13*P(0,6) + HK14*P(0,0) - HK15*P(0,4) + HK16*P(0,1) - HK17*P(0,3) - HK18*P(0,5);
|
||||
const float HK20 = HK12*P(2,6) + HK13*P(6,6) + HK14*P(0,6) - HK15*P(4,6) + HK16*P(1,6) - HK17*P(3,6) - HK18*P(5,6);
|
||||
const float HK21 = HK12*P(2,2) + HK13*P(2,6) + HK14*P(0,2) - HK15*P(2,4) + HK16*P(1,2) - HK17*P(2,3) - HK18*P(2,5);
|
||||
const float HK22 = HK12*P(2,4) + HK13*P(4,6) + HK14*P(0,4) - HK15*P(4,4) + HK16*P(1,4) - HK17*P(3,4) - HK18*P(4,5);
|
||||
const float HK23 = HK12*P(1,2) + HK13*P(1,6) + HK14*P(0,1) - HK15*P(1,4) + HK16*P(1,1) - HK17*P(1,3) - HK18*P(1,5);
|
||||
const float HK24 = HK12*P(2,5) + HK13*P(5,6) + HK14*P(0,5) - HK15*P(4,5) + HK16*P(1,5) - HK17*P(3,5) - HK18*P(5,5);
|
||||
const float HK25 = HK12*P(2,3) + HK13*P(3,6) + HK14*P(0,3) - HK15*P(3,4) + HK16*P(1,3) - HK17*P(3,3) - HK18*P(3,5);
|
||||
const float HK26 = 1.0F/(HK12*HK21 + HK13*HK20 + HK14*HK19 - HK15*HK22 + HK16*HK23 - HK17*HK25 - HK18*HK24 + R_VEL);
|
||||
|
||||
|
||||
H_VEL(0) = 2*HK0;
|
||||
H_VEL(1) = 2*HK2;
|
||||
H_VEL(2) = 2*HK3;
|
||||
H_VEL(3) = 2*HK4 - 2*HK5 - 2*HK6;
|
||||
H_VEL(4) = 2*HK7 - 2*HK8;
|
||||
H_VEL(5) = -HK10 - HK9 + 1;
|
||||
H_VEL(6) = 2*HK11;
|
||||
H_VEL(7) = 0;
|
||||
H_VEL(8) = 0;
|
||||
H_VEL(9) = 0;
|
||||
H_VEL(10) = 0;
|
||||
H_VEL(11) = 0;
|
||||
H_VEL(12) = 0;
|
||||
H_VEL(13) = 0;
|
||||
H_VEL(14) = 0;
|
||||
H_VEL(15) = 0;
|
||||
H_VEL(16) = 0;
|
||||
H_VEL(17) = 0;
|
||||
H_VEL(18) = 0;
|
||||
H_VEL(19) = 0;
|
||||
H_VEL(20) = 0;
|
||||
H_VEL(21) = 0;
|
||||
H_VEL(22) = 0;
|
||||
H_VEL(23) = 0;
|
||||
|
||||
|
||||
Kfusion(0) = HK19*HK26;
|
||||
Kfusion(1) = HK23*HK26;
|
||||
Kfusion(2) = HK21*HK26;
|
||||
Kfusion(3) = HK25*HK26;
|
||||
Kfusion(4) = HK22*HK26;
|
||||
Kfusion(5) = HK24*HK26;
|
||||
Kfusion(6) = HK20*HK26;
|
||||
Kfusion(7) = HK26*(HK12*P(2,7) + HK13*P(6,7) + HK14*P(0,7) - HK15*P(4,7) + HK16*P(1,7) - HK17*P(3,7) - HK18*P(5,7));
|
||||
Kfusion(8) = HK26*(HK12*P(2,8) + HK13*P(6,8) + HK14*P(0,8) - HK15*P(4,8) + HK16*P(1,8) - HK17*P(3,8) - HK18*P(5,8));
|
||||
Kfusion(9) = HK26*(HK12*P(2,9) + HK13*P(6,9) + HK14*P(0,9) - HK15*P(4,9) + HK16*P(1,9) - HK17*P(3,9) - HK18*P(5,9));
|
||||
Kfusion(10) = HK26*(HK12*P(2,10) + HK13*P(6,10) + HK14*P(0,10) - HK15*P(4,10) + HK16*P(1,10) - HK17*P(3,10) - HK18*P(5,10));
|
||||
Kfusion(11) = HK26*(HK12*P(2,11) + HK13*P(6,11) + HK14*P(0,11) - HK15*P(4,11) + HK16*P(1,11) - HK17*P(3,11) - HK18*P(5,11));
|
||||
Kfusion(12) = HK26*(HK12*P(2,12) + HK13*P(6,12) + HK14*P(0,12) - HK15*P(4,12) + HK16*P(1,12) - HK17*P(3,12) - HK18*P(5,12));
|
||||
Kfusion(13) = HK26*(HK12*P(2,13) + HK13*P(6,13) + HK14*P(0,13) - HK15*P(4,13) + HK16*P(1,13) - HK17*P(3,13) - HK18*P(5,13));
|
||||
Kfusion(14) = HK26*(HK12*P(2,14) + HK13*P(6,14) + HK14*P(0,14) - HK15*P(4,14) + HK16*P(1,14) - HK17*P(3,14) - HK18*P(5,14));
|
||||
Kfusion(15) = HK26*(HK12*P(2,15) + HK13*P(6,15) + HK14*P(0,15) - HK15*P(4,15) + HK16*P(1,15) - HK17*P(3,15) - HK18*P(5,15));
|
||||
Kfusion(16) = HK26*(HK12*P(2,16) + HK13*P(6,16) + HK14*P(0,16) - HK15*P(4,16) + HK16*P(1,16) - HK17*P(3,16) - HK18*P(5,16));
|
||||
Kfusion(17) = HK26*(HK12*P(2,17) + HK13*P(6,17) + HK14*P(0,17) - HK15*P(4,17) + HK16*P(1,17) - HK17*P(3,17) - HK18*P(5,17));
|
||||
Kfusion(18) = HK26*(HK12*P(2,18) + HK13*P(6,18) + HK14*P(0,18) - HK15*P(4,18) + HK16*P(1,18) - HK17*P(3,18) - HK18*P(5,18));
|
||||
Kfusion(19) = HK26*(HK12*P(2,19) + HK13*P(6,19) + HK14*P(0,19) - HK15*P(4,19) + HK16*P(1,19) - HK17*P(3,19) - HK18*P(5,19));
|
||||
Kfusion(20) = HK26*(HK12*P(2,20) + HK13*P(6,20) + HK14*P(0,20) - HK15*P(4,20) + HK16*P(1,20) - HK17*P(3,20) - HK18*P(5,20));
|
||||
Kfusion(21) = HK26*(HK12*P(2,21) + HK13*P(6,21) + HK14*P(0,21) - HK15*P(4,21) + HK16*P(1,21) - HK17*P(3,21) - HK18*P(5,21));
|
||||
Kfusion(22) = HK26*(HK12*P(2,22) + HK13*P(6,22) + HK14*P(0,22) - HK15*P(4,22) + HK16*P(1,22) - HK17*P(3,22) - HK18*P(5,22));
|
||||
Kfusion(23) = HK26*(HK12*P(2,23) + HK13*P(6,23) + HK14*P(0,23) - HK15*P(4,23) + HK16*P(1,23) - HK17*P(3,23) - HK18*P(5,23));
|
||||
|
||||
|
||||
// axis 2
|
||||
const float HK0 = q2*vn;
|
||||
const float HK1 = q1*ve;
|
||||
const float HK2 = q3*vn;
|
||||
const float HK3 = q0*ve;
|
||||
const float HK4 = 2*vd;
|
||||
const float HK5 = HK4*q1;
|
||||
const float HK6 = -HK4*q2 + q0*vn + q3*ve;
|
||||
const float HK7 = q1*vn + q2*ve;
|
||||
const float HK8 = q0*q2 + q1*q3;
|
||||
const float HK9 = q2*q3;
|
||||
const float HK10 = q0*q1;
|
||||
const float HK11 = 2*powf(q1, 2);
|
||||
const float HK12 = 2*powf(q2, 2);
|
||||
const float HK13 = 2*HK7;
|
||||
const float HK14 = 2*HK8;
|
||||
const float HK15 = -2*HK0 + 2*HK1;
|
||||
const float HK16 = 2*HK10 - 2*HK9;
|
||||
const float HK17 = 2*HK6;
|
||||
const float HK18 = HK11 + HK12 - 1;
|
||||
const float HK19 = -2*HK2 + 2*HK3 + 2*HK5;
|
||||
const float HK20 = -HK13*P(0,3) - HK14*P(0,4) + HK15*P(0,0) + HK16*P(0,5) - HK17*P(0,2) + HK18*P(0,6) + HK19*P(0,1);
|
||||
const float HK21 = -HK13*P(3,4) - HK14*P(4,4) + HK15*P(0,4) + HK16*P(4,5) - HK17*P(2,4) + HK18*P(4,6) + HK19*P(1,4);
|
||||
const float HK22 = -HK13*P(3,3) - HK14*P(3,4) + HK15*P(0,3) + HK16*P(3,5) - HK17*P(2,3) + HK18*P(3,6) + HK19*P(1,3);
|
||||
const float HK23 = -HK13*P(3,5) - HK14*P(4,5) + HK15*P(0,5) + HK16*P(5,5) - HK17*P(2,5) + HK18*P(5,6) + HK19*P(1,5);
|
||||
const float HK24 = -HK13*P(3,6) - HK14*P(4,6) + HK15*P(0,6) + HK16*P(5,6) - HK17*P(2,6) + HK18*P(6,6) + HK19*P(1,6);
|
||||
const float HK25 = -HK13*P(2,3) - HK14*P(2,4) + HK15*P(0,2) + HK16*P(2,5) - HK17*P(2,2) + HK18*P(2,6) + HK19*P(1,2);
|
||||
const float HK26 = -HK13*P(1,3) - HK14*P(1,4) + HK15*P(0,1) + HK16*P(1,5) - HK17*P(1,2) + HK18*P(1,6) + HK19*P(1,1);
|
||||
const float HK27 = 1.0F/(-HK13*HK22 - HK14*HK21 + HK15*HK20 + HK16*HK23 - HK17*HK25 + HK18*HK24 + HK19*HK26 + R_VEL);
|
||||
|
||||
|
||||
H_VEL(0) = 2*HK0 - 2*HK1;
|
||||
H_VEL(1) = 2*HK2 - 2*HK3 - 2*HK5;
|
||||
H_VEL(2) = 2*HK6;
|
||||
H_VEL(3) = 2*HK7;
|
||||
H_VEL(4) = 2*HK8;
|
||||
H_VEL(5) = -2*HK10 + 2*HK9;
|
||||
H_VEL(6) = -HK11 - HK12 + 1;
|
||||
H_VEL(7) = 0;
|
||||
H_VEL(8) = 0;
|
||||
H_VEL(9) = 0;
|
||||
H_VEL(10) = 0;
|
||||
H_VEL(11) = 0;
|
||||
H_VEL(12) = 0;
|
||||
H_VEL(13) = 0;
|
||||
H_VEL(14) = 0;
|
||||
H_VEL(15) = 0;
|
||||
H_VEL(16) = 0;
|
||||
H_VEL(17) = 0;
|
||||
H_VEL(18) = 0;
|
||||
H_VEL(19) = 0;
|
||||
H_VEL(20) = 0;
|
||||
H_VEL(21) = 0;
|
||||
H_VEL(22) = 0;
|
||||
H_VEL(23) = 0;
|
||||
|
||||
|
||||
Kfusion(0) = -HK20*HK27;
|
||||
Kfusion(1) = -HK26*HK27;
|
||||
Kfusion(2) = -HK25*HK27;
|
||||
Kfusion(3) = -HK22*HK27;
|
||||
Kfusion(4) = -HK21*HK27;
|
||||
Kfusion(5) = -HK23*HK27;
|
||||
Kfusion(6) = -HK24*HK27;
|
||||
Kfusion(7) = -HK27*(-HK13*P(3,7) - HK14*P(4,7) + HK15*P(0,7) + HK16*P(5,7) - HK17*P(2,7) + HK18*P(6,7) + HK19*P(1,7));
|
||||
Kfusion(8) = -HK27*(-HK13*P(3,8) - HK14*P(4,8) + HK15*P(0,8) + HK16*P(5,8) - HK17*P(2,8) + HK18*P(6,8) + HK19*P(1,8));
|
||||
Kfusion(9) = -HK27*(-HK13*P(3,9) - HK14*P(4,9) + HK15*P(0,9) + HK16*P(5,9) - HK17*P(2,9) + HK18*P(6,9) + HK19*P(1,9));
|
||||
Kfusion(10) = -HK27*(-HK13*P(3,10) - HK14*P(4,10) + HK15*P(0,10) + HK16*P(5,10) - HK17*P(2,10) + HK18*P(6,10) + HK19*P(1,10));
|
||||
Kfusion(11) = -HK27*(-HK13*P(3,11) - HK14*P(4,11) + HK15*P(0,11) + HK16*P(5,11) - HK17*P(2,11) + HK18*P(6,11) + HK19*P(1,11));
|
||||
Kfusion(12) = -HK27*(-HK13*P(3,12) - HK14*P(4,12) + HK15*P(0,12) + HK16*P(5,12) - HK17*P(2,12) + HK18*P(6,12) + HK19*P(1,12));
|
||||
Kfusion(13) = -HK27*(-HK13*P(3,13) - HK14*P(4,13) + HK15*P(0,13) + HK16*P(5,13) - HK17*P(2,13) + HK18*P(6,13) + HK19*P(1,13));
|
||||
Kfusion(14) = -HK27*(-HK13*P(3,14) - HK14*P(4,14) + HK15*P(0,14) + HK16*P(5,14) - HK17*P(2,14) + HK18*P(6,14) + HK19*P(1,14));
|
||||
Kfusion(15) = -HK27*(-HK13*P(3,15) - HK14*P(4,15) + HK15*P(0,15) + HK16*P(5,15) - HK17*P(2,15) + HK18*P(6,15) + HK19*P(1,15));
|
||||
Kfusion(16) = -HK27*(-HK13*P(3,16) - HK14*P(4,16) + HK15*P(0,16) + HK16*P(5,16) - HK17*P(2,16) + HK18*P(6,16) + HK19*P(1,16));
|
||||
Kfusion(17) = -HK27*(-HK13*P(3,17) - HK14*P(4,17) + HK15*P(0,17) + HK16*P(5,17) - HK17*P(2,17) + HK18*P(6,17) + HK19*P(1,17));
|
||||
Kfusion(18) = -HK27*(-HK13*P(3,18) - HK14*P(4,18) + HK15*P(0,18) + HK16*P(5,18) - HK17*P(2,18) + HK18*P(6,18) + HK19*P(1,18));
|
||||
Kfusion(19) = -HK27*(-HK13*P(3,19) - HK14*P(4,19) + HK15*P(0,19) + HK16*P(5,19) - HK17*P(2,19) + HK18*P(6,19) + HK19*P(1,19));
|
||||
Kfusion(20) = -HK27*(-HK13*P(3,20) - HK14*P(4,20) + HK15*P(0,20) + HK16*P(5,20) - HK17*P(2,20) + HK18*P(6,20) + HK19*P(1,20));
|
||||
Kfusion(21) = -HK27*(-HK13*P(3,21) - HK14*P(4,21) + HK15*P(0,21) + HK16*P(5,21) - HK17*P(2,21) + HK18*P(6,21) + HK19*P(1,21));
|
||||
Kfusion(22) = -HK27*(-HK13*P(3,22) - HK14*P(4,22) + HK15*P(0,22) + HK16*P(5,22) - HK17*P(2,22) + HK18*P(6,22) + HK19*P(1,22));
|
||||
Kfusion(23) = -HK27*(-HK13*P(3,23) - HK14*P(4,23) + HK15*P(0,23) + HK16*P(5,23) - HK17*P(2,23) + HK18*P(6,23) + HK19*P(1,23));
|
||||
|
||||
|
||||
@@ -0,0 +1,251 @@
|
||||
// Sub Expressions
|
||||
const float HK0 = q3*ve;
|
||||
const float HK1 = q2*vd;
|
||||
const float HK2 = -HK1;
|
||||
const float HK3 = q2*ve;
|
||||
const float HK4 = q3*vd;
|
||||
const float HK5 = HK3 + HK4;
|
||||
const float HK6 = q1*ve;
|
||||
const float HK7 = q0*vd;
|
||||
const float HK8 = q2*vn;
|
||||
const float HK9 = 2*HK8;
|
||||
const float HK10 = q0*ve;
|
||||
const float HK11 = q1*vd;
|
||||
const float HK12 = q3*vn;
|
||||
const float HK13 = HK10 + HK11 - 2*HK12;
|
||||
const float HK14 = 2*powf(q2, 2);
|
||||
const float HK15 = -HK14;
|
||||
const float HK16 = 2*powf(q3, 2);
|
||||
const float HK17 = 1 - HK16;
|
||||
const float HK18 = q0*q3;
|
||||
const float HK19 = q1*q2;
|
||||
const float HK20 = HK18 + HK19;
|
||||
const float HK21 = q1*q3;
|
||||
const float HK22 = q0*q2;
|
||||
const float HK23 = 2*HK5;
|
||||
const float HK24 = 2*HK20;
|
||||
const float HK25 = -2*HK0 + 2*HK1;
|
||||
const float HK26 = -2*HK21 + 2*HK22;
|
||||
const float HK27 = 2*HK13;
|
||||
const float HK28 = HK16 - 1;
|
||||
const float HK29 = HK14 + HK28;
|
||||
const float HK30 = -HK6;
|
||||
const float HK31 = 2*HK30 + 2*HK7 + 2*HK9;
|
||||
const float HK32 = -HK23*P(0,1) - HK24*P(0,5) + HK25*P(0,0) + HK26*P(0,6) - HK27*P(0,3) + HK29*P(0,4) + HK31*P(0,2);
|
||||
const float HK33 = -HK23*P(1,5) - HK24*P(5,5) + HK25*P(0,5) + HK26*P(5,6) - HK27*P(3,5) + HK29*P(4,5) + HK31*P(2,5);
|
||||
const float HK34 = -HK23*P(1,1) - HK24*P(1,5) + HK25*P(0,1) + HK26*P(1,6) - HK27*P(1,3) + HK29*P(1,4) + HK31*P(1,2);
|
||||
const float HK35 = -HK23*P(1,6) - HK24*P(5,6) + HK25*P(0,6) + HK26*P(6,6) - HK27*P(3,6) + HK29*P(4,6) + HK31*P(2,6);
|
||||
const float HK36 = -HK23*P(1,4) - HK24*P(4,5) + HK25*P(0,4) + HK26*P(4,6) - HK27*P(3,4) + HK29*P(4,4) + HK31*P(2,4);
|
||||
const float HK37 = -HK23*P(1,3) - HK24*P(3,5) + HK25*P(0,3) + HK26*P(3,6) - HK27*P(3,3) + HK29*P(3,4) + HK31*P(2,3);
|
||||
const float HK38 = -HK23*P(1,2) - HK24*P(2,5) + HK25*P(0,2) + HK26*P(2,6) - HK27*P(2,3) + HK29*P(2,4) + HK31*P(2,2);
|
||||
const float HK39 = 1.0F/(-HK23*HK34 - HK24*HK33 + HK25*HK32 + HK26*HK35 - HK27*HK37 + HK29*HK36 + HK31*HK38 + R_VEL);
|
||||
const float HK40 = -HK12;
|
||||
const float HK41 = HK11 + HK40;
|
||||
const float HK42 = -2*HK6 + HK7 + HK8;
|
||||
const float HK43 = q1*vn;
|
||||
const float HK44 = HK4 + HK43;
|
||||
const float HK45 = q0*vn;
|
||||
const float HK46 = 2*HK0;
|
||||
const float HK47 = 2*powf(q1, 2);
|
||||
const float HK48 = -HK47;
|
||||
const float HK49 = q0*q1;
|
||||
const float HK50 = q2*q3;
|
||||
const float HK51 = HK49 + HK50;
|
||||
const float HK52 = 2*HK44;
|
||||
const float HK53 = 2*HK51;
|
||||
const float HK54 = 2*HK41;
|
||||
const float HK55 = 2*HK18 - 2*HK19;
|
||||
const float HK56 = 2*HK42;
|
||||
const float HK57 = 2*HK2 + 2*HK45 + 2*HK46;
|
||||
const float HK58 = HK28 + HK47;
|
||||
const float HK59 = HK52*P(0,2) + HK53*P(0,6) + HK54*P(0,0) - HK55*P(0,4) + HK56*P(0,1) - HK57*P(0,3) - HK58*P(0,5);
|
||||
const float HK60 = HK52*P(2,6) + HK53*P(6,6) + HK54*P(0,6) - HK55*P(4,6) + HK56*P(1,6) - HK57*P(3,6) - HK58*P(5,6);
|
||||
const float HK61 = HK52*P(2,2) + HK53*P(2,6) + HK54*P(0,2) - HK55*P(2,4) + HK56*P(1,2) - HK57*P(2,3) - HK58*P(2,5);
|
||||
const float HK62 = HK52*P(2,4) + HK53*P(4,6) + HK54*P(0,4) - HK55*P(4,4) + HK56*P(1,4) - HK57*P(3,4) - HK58*P(4,5);
|
||||
const float HK63 = HK52*P(1,2) + HK53*P(1,6) + HK54*P(0,1) - HK55*P(1,4) + HK56*P(1,1) - HK57*P(1,3) - HK58*P(1,5);
|
||||
const float HK64 = HK52*P(2,5) + HK53*P(5,6) + HK54*P(0,5) - HK55*P(4,5) + HK56*P(1,5) - HK57*P(3,5) - HK58*P(5,5);
|
||||
const float HK65 = HK52*P(2,3) + HK53*P(3,6) + HK54*P(0,3) - HK55*P(3,4) + HK56*P(1,3) - HK57*P(3,3) - HK58*P(3,5);
|
||||
const float HK66 = 1.0F/(HK52*HK61 + HK53*HK60 + HK54*HK59 - HK55*HK62 + HK56*HK63 - HK57*HK65 - HK58*HK64 + R_VEL);
|
||||
const float HK67 = 2*HK11;
|
||||
const float HK68 = HK0 - 2*HK1 + HK45;
|
||||
const float HK69 = HK3 + HK43;
|
||||
const float HK70 = HK21 + HK22;
|
||||
const float HK71 = 2*HK69;
|
||||
const float HK72 = 2*HK70;
|
||||
const float HK73 = 2*HK6 - 2*HK8;
|
||||
const float HK74 = 2*HK49 - 2*HK50;
|
||||
const float HK75 = 2*HK68;
|
||||
const float HK76 = HK14 + HK47 - 1;
|
||||
const float HK77 = 2*HK10 + 2*HK40 + 2*HK67;
|
||||
const float HK78 = -HK71*P(0,3) - HK72*P(0,4) + HK73*P(0,0) + HK74*P(0,5) - HK75*P(0,2) + HK76*P(0,6) + HK77*P(0,1);
|
||||
const float HK79 = -HK71*P(3,4) - HK72*P(4,4) + HK73*P(0,4) + HK74*P(4,5) - HK75*P(2,4) + HK76*P(4,6) + HK77*P(1,4);
|
||||
const float HK80 = -HK71*P(3,3) - HK72*P(3,4) + HK73*P(0,3) + HK74*P(3,5) - HK75*P(2,3) + HK76*P(3,6) + HK77*P(1,3);
|
||||
const float HK81 = -HK71*P(3,5) - HK72*P(4,5) + HK73*P(0,5) + HK74*P(5,5) - HK75*P(2,5) + HK76*P(5,6) + HK77*P(1,5);
|
||||
const float HK82 = -HK71*P(3,6) - HK72*P(4,6) + HK73*P(0,6) + HK74*P(5,6) - HK75*P(2,6) + HK76*P(6,6) + HK77*P(1,6);
|
||||
const float HK83 = -HK71*P(2,3) - HK72*P(2,4) + HK73*P(0,2) + HK74*P(2,5) - HK75*P(2,2) + HK76*P(2,6) + HK77*P(1,2);
|
||||
const float HK84 = -HK71*P(1,3) - HK72*P(1,4) + HK73*P(0,1) + HK74*P(1,5) - HK75*P(1,2) + HK76*P(1,6) + HK77*P(1,1);
|
||||
const float HK85 = 1.0F/(-HK71*HK80 - HK72*HK79 + HK73*HK78 + HK74*HK81 - HK75*HK83 + HK76*HK82 + HK77*HK84 + R_VEL);
|
||||
|
||||
|
||||
// Observation Jacobians - axis 0
|
||||
Hfusion.at<0>() = 2*HK0 + 2*HK2;
|
||||
Hfusion.at<1>() = 2*HK5;
|
||||
Hfusion.at<2>() = 2*HK6 - 2*HK7 - 2*HK9;
|
||||
Hfusion.at<3>() = 2*HK13;
|
||||
Hfusion.at<4>() = HK15 + HK17;
|
||||
Hfusion.at<5>() = 2*HK20;
|
||||
Hfusion.at<6>() = 2*HK21 - 2*HK22;
|
||||
Hfusion.at<7>() = 0;
|
||||
Hfusion.at<8>() = 0;
|
||||
Hfusion.at<9>() = 0;
|
||||
Hfusion.at<10>() = 0;
|
||||
Hfusion.at<11>() = 0;
|
||||
Hfusion.at<12>() = 0;
|
||||
Hfusion.at<13>() = 0;
|
||||
Hfusion.at<14>() = 0;
|
||||
Hfusion.at<15>() = 0;
|
||||
Hfusion.at<16>() = 0;
|
||||
Hfusion.at<17>() = 0;
|
||||
Hfusion.at<18>() = 0;
|
||||
Hfusion.at<19>() = 0;
|
||||
Hfusion.at<20>() = 0;
|
||||
Hfusion.at<21>() = 0;
|
||||
Hfusion.at<22>() = 0;
|
||||
Hfusion.at<23>() = 0;
|
||||
|
||||
|
||||
// Kalman gains - axis 0
|
||||
Kfusion(0) = -HK32*HK39;
|
||||
Kfusion(1) = -HK34*HK39;
|
||||
Kfusion(2) = -HK38*HK39;
|
||||
Kfusion(3) = -HK37*HK39;
|
||||
Kfusion(4) = -HK36*HK39;
|
||||
Kfusion(5) = -HK33*HK39;
|
||||
Kfusion(6) = -HK35*HK39;
|
||||
Kfusion(7) = -HK39*(-HK23*P(1,7) - HK24*P(5,7) + HK25*P(0,7) + HK26*P(6,7) - HK27*P(3,7) + HK29*P(4,7) + HK31*P(2,7));
|
||||
Kfusion(8) = -HK39*(-HK23*P(1,8) - HK24*P(5,8) + HK25*P(0,8) + HK26*P(6,8) - HK27*P(3,8) + HK29*P(4,8) + HK31*P(2,8));
|
||||
Kfusion(9) = -HK39*(-HK23*P(1,9) - HK24*P(5,9) + HK25*P(0,9) + HK26*P(6,9) - HK27*P(3,9) + HK29*P(4,9) + HK31*P(2,9));
|
||||
Kfusion(10) = -HK39*(-HK23*P(1,10) - HK24*P(5,10) + HK25*P(0,10) + HK26*P(6,10) - HK27*P(3,10) + HK29*P(4,10) + HK31*P(2,10));
|
||||
Kfusion(11) = -HK39*(-HK23*P(1,11) - HK24*P(5,11) + HK25*P(0,11) + HK26*P(6,11) - HK27*P(3,11) + HK29*P(4,11) + HK31*P(2,11));
|
||||
Kfusion(12) = -HK39*(-HK23*P(1,12) - HK24*P(5,12) + HK25*P(0,12) + HK26*P(6,12) - HK27*P(3,12) + HK29*P(4,12) + HK31*P(2,12));
|
||||
Kfusion(13) = -HK39*(-HK23*P(1,13) - HK24*P(5,13) + HK25*P(0,13) + HK26*P(6,13) - HK27*P(3,13) + HK29*P(4,13) + HK31*P(2,13));
|
||||
Kfusion(14) = -HK39*(-HK23*P(1,14) - HK24*P(5,14) + HK25*P(0,14) + HK26*P(6,14) - HK27*P(3,14) + HK29*P(4,14) + HK31*P(2,14));
|
||||
Kfusion(15) = -HK39*(-HK23*P(1,15) - HK24*P(5,15) + HK25*P(0,15) + HK26*P(6,15) - HK27*P(3,15) + HK29*P(4,15) + HK31*P(2,15));
|
||||
Kfusion(16) = -HK39*(-HK23*P(1,16) - HK24*P(5,16) + HK25*P(0,16) + HK26*P(6,16) - HK27*P(3,16) + HK29*P(4,16) + HK31*P(2,16));
|
||||
Kfusion(17) = -HK39*(-HK23*P(1,17) - HK24*P(5,17) + HK25*P(0,17) + HK26*P(6,17) - HK27*P(3,17) + HK29*P(4,17) + HK31*P(2,17));
|
||||
Kfusion(18) = -HK39*(-HK23*P(1,18) - HK24*P(5,18) + HK25*P(0,18) + HK26*P(6,18) - HK27*P(3,18) + HK29*P(4,18) + HK31*P(2,18));
|
||||
Kfusion(19) = -HK39*(-HK23*P(1,19) - HK24*P(5,19) + HK25*P(0,19) + HK26*P(6,19) - HK27*P(3,19) + HK29*P(4,19) + HK31*P(2,19));
|
||||
Kfusion(20) = -HK39*(-HK23*P(1,20) - HK24*P(5,20) + HK25*P(0,20) + HK26*P(6,20) - HK27*P(3,20) + HK29*P(4,20) + HK31*P(2,20));
|
||||
Kfusion(21) = -HK39*(-HK23*P(1,21) - HK24*P(5,21) + HK25*P(0,21) + HK26*P(6,21) - HK27*P(3,21) + HK29*P(4,21) + HK31*P(2,21));
|
||||
Kfusion(22) = -HK39*(-HK23*P(1,22) - HK24*P(5,22) + HK25*P(0,22) + HK26*P(6,22) - HK27*P(3,22) + HK29*P(4,22) + HK31*P(2,22));
|
||||
Kfusion(23) = -HK39*(-HK23*P(1,23) - HK24*P(5,23) + HK25*P(0,23) + HK26*P(6,23) - HK27*P(3,23) + HK29*P(4,23) + HK31*P(2,23));
|
||||
|
||||
|
||||
// Observation Jacobians - axis 1
|
||||
Hfusion.at<0>() = 2*HK41;
|
||||
Hfusion.at<1>() = 2*HK42;
|
||||
Hfusion.at<2>() = 2*HK44;
|
||||
Hfusion.at<3>() = 2*HK1 - 2*HK45 - 2*HK46;
|
||||
Hfusion.at<4>() = -2*HK18 + 2*HK19;
|
||||
Hfusion.at<5>() = HK17 + HK48;
|
||||
Hfusion.at<6>() = 2*HK51;
|
||||
Hfusion.at<7>() = 0;
|
||||
Hfusion.at<8>() = 0;
|
||||
Hfusion.at<9>() = 0;
|
||||
Hfusion.at<10>() = 0;
|
||||
Hfusion.at<11>() = 0;
|
||||
Hfusion.at<12>() = 0;
|
||||
Hfusion.at<13>() = 0;
|
||||
Hfusion.at<14>() = 0;
|
||||
Hfusion.at<15>() = 0;
|
||||
Hfusion.at<16>() = 0;
|
||||
Hfusion.at<17>() = 0;
|
||||
Hfusion.at<18>() = 0;
|
||||
Hfusion.at<19>() = 0;
|
||||
Hfusion.at<20>() = 0;
|
||||
Hfusion.at<21>() = 0;
|
||||
Hfusion.at<22>() = 0;
|
||||
Hfusion.at<23>() = 0;
|
||||
|
||||
|
||||
// Kalman gains - axis 1
|
||||
Kfusion(0) = HK59*HK66;
|
||||
Kfusion(1) = HK63*HK66;
|
||||
Kfusion(2) = HK61*HK66;
|
||||
Kfusion(3) = HK65*HK66;
|
||||
Kfusion(4) = HK62*HK66;
|
||||
Kfusion(5) = HK64*HK66;
|
||||
Kfusion(6) = HK60*HK66;
|
||||
Kfusion(7) = HK66*(HK52*P(2,7) + HK53*P(6,7) + HK54*P(0,7) - HK55*P(4,7) + HK56*P(1,7) - HK57*P(3,7) - HK58*P(5,7));
|
||||
Kfusion(8) = HK66*(HK52*P(2,8) + HK53*P(6,8) + HK54*P(0,8) - HK55*P(4,8) + HK56*P(1,8) - HK57*P(3,8) - HK58*P(5,8));
|
||||
Kfusion(9) = HK66*(HK52*P(2,9) + HK53*P(6,9) + HK54*P(0,9) - HK55*P(4,9) + HK56*P(1,9) - HK57*P(3,9) - HK58*P(5,9));
|
||||
Kfusion(10) = HK66*(HK52*P(2,10) + HK53*P(6,10) + HK54*P(0,10) - HK55*P(4,10) + HK56*P(1,10) - HK57*P(3,10) - HK58*P(5,10));
|
||||
Kfusion(11) = HK66*(HK52*P(2,11) + HK53*P(6,11) + HK54*P(0,11) - HK55*P(4,11) + HK56*P(1,11) - HK57*P(3,11) - HK58*P(5,11));
|
||||
Kfusion(12) = HK66*(HK52*P(2,12) + HK53*P(6,12) + HK54*P(0,12) - HK55*P(4,12) + HK56*P(1,12) - HK57*P(3,12) - HK58*P(5,12));
|
||||
Kfusion(13) = HK66*(HK52*P(2,13) + HK53*P(6,13) + HK54*P(0,13) - HK55*P(4,13) + HK56*P(1,13) - HK57*P(3,13) - HK58*P(5,13));
|
||||
Kfusion(14) = HK66*(HK52*P(2,14) + HK53*P(6,14) + HK54*P(0,14) - HK55*P(4,14) + HK56*P(1,14) - HK57*P(3,14) - HK58*P(5,14));
|
||||
Kfusion(15) = HK66*(HK52*P(2,15) + HK53*P(6,15) + HK54*P(0,15) - HK55*P(4,15) + HK56*P(1,15) - HK57*P(3,15) - HK58*P(5,15));
|
||||
Kfusion(16) = HK66*(HK52*P(2,16) + HK53*P(6,16) + HK54*P(0,16) - HK55*P(4,16) + HK56*P(1,16) - HK57*P(3,16) - HK58*P(5,16));
|
||||
Kfusion(17) = HK66*(HK52*P(2,17) + HK53*P(6,17) + HK54*P(0,17) - HK55*P(4,17) + HK56*P(1,17) - HK57*P(3,17) - HK58*P(5,17));
|
||||
Kfusion(18) = HK66*(HK52*P(2,18) + HK53*P(6,18) + HK54*P(0,18) - HK55*P(4,18) + HK56*P(1,18) - HK57*P(3,18) - HK58*P(5,18));
|
||||
Kfusion(19) = HK66*(HK52*P(2,19) + HK53*P(6,19) + HK54*P(0,19) - HK55*P(4,19) + HK56*P(1,19) - HK57*P(3,19) - HK58*P(5,19));
|
||||
Kfusion(20) = HK66*(HK52*P(2,20) + HK53*P(6,20) + HK54*P(0,20) - HK55*P(4,20) + HK56*P(1,20) - HK57*P(3,20) - HK58*P(5,20));
|
||||
Kfusion(21) = HK66*(HK52*P(2,21) + HK53*P(6,21) + HK54*P(0,21) - HK55*P(4,21) + HK56*P(1,21) - HK57*P(3,21) - HK58*P(5,21));
|
||||
Kfusion(22) = HK66*(HK52*P(2,22) + HK53*P(6,22) + HK54*P(0,22) - HK55*P(4,22) + HK56*P(1,22) - HK57*P(3,22) - HK58*P(5,22));
|
||||
Kfusion(23) = HK66*(HK52*P(2,23) + HK53*P(6,23) + HK54*P(0,23) - HK55*P(4,23) + HK56*P(1,23) - HK57*P(3,23) - HK58*P(5,23));
|
||||
|
||||
|
||||
// Observation Jacobians - axis 2
|
||||
Hfusion.at<0>() = 2*HK30 + 2*HK8;
|
||||
Hfusion.at<1>() = -2*HK10 + 2*HK12 - 2*HK67;
|
||||
Hfusion.at<2>() = 2*HK68;
|
||||
Hfusion.at<3>() = 2*HK69;
|
||||
Hfusion.at<4>() = 2*HK70;
|
||||
Hfusion.at<5>() = -2*HK49 + 2*HK50;
|
||||
Hfusion.at<6>() = HK15 + HK48 + 1;
|
||||
Hfusion.at<7>() = 0;
|
||||
Hfusion.at<8>() = 0;
|
||||
Hfusion.at<9>() = 0;
|
||||
Hfusion.at<10>() = 0;
|
||||
Hfusion.at<11>() = 0;
|
||||
Hfusion.at<12>() = 0;
|
||||
Hfusion.at<13>() = 0;
|
||||
Hfusion.at<14>() = 0;
|
||||
Hfusion.at<15>() = 0;
|
||||
Hfusion.at<16>() = 0;
|
||||
Hfusion.at<17>() = 0;
|
||||
Hfusion.at<18>() = 0;
|
||||
Hfusion.at<19>() = 0;
|
||||
Hfusion.at<20>() = 0;
|
||||
Hfusion.at<21>() = 0;
|
||||
Hfusion.at<22>() = 0;
|
||||
Hfusion.at<23>() = 0;
|
||||
|
||||
|
||||
// Kalman gains - axis 2
|
||||
Kfusion(0) = -HK78*HK85;
|
||||
Kfusion(1) = -HK84*HK85;
|
||||
Kfusion(2) = -HK83*HK85;
|
||||
Kfusion(3) = -HK80*HK85;
|
||||
Kfusion(4) = -HK79*HK85;
|
||||
Kfusion(5) = -HK81*HK85;
|
||||
Kfusion(6) = -HK82*HK85;
|
||||
Kfusion(7) = -HK85*(-HK71*P(3,7) - HK72*P(4,7) + HK73*P(0,7) + HK74*P(5,7) - HK75*P(2,7) + HK76*P(6,7) + HK77*P(1,7));
|
||||
Kfusion(8) = -HK85*(-HK71*P(3,8) - HK72*P(4,8) + HK73*P(0,8) + HK74*P(5,8) - HK75*P(2,8) + HK76*P(6,8) + HK77*P(1,8));
|
||||
Kfusion(9) = -HK85*(-HK71*P(3,9) - HK72*P(4,9) + HK73*P(0,9) + HK74*P(5,9) - HK75*P(2,9) + HK76*P(6,9) + HK77*P(1,9));
|
||||
Kfusion(10) = -HK85*(-HK71*P(3,10) - HK72*P(4,10) + HK73*P(0,10) + HK74*P(5,10) - HK75*P(2,10) + HK76*P(6,10) + HK77*P(1,10));
|
||||
Kfusion(11) = -HK85*(-HK71*P(3,11) - HK72*P(4,11) + HK73*P(0,11) + HK74*P(5,11) - HK75*P(2,11) + HK76*P(6,11) + HK77*P(1,11));
|
||||
Kfusion(12) = -HK85*(-HK71*P(3,12) - HK72*P(4,12) + HK73*P(0,12) + HK74*P(5,12) - HK75*P(2,12) + HK76*P(6,12) + HK77*P(1,12));
|
||||
Kfusion(13) = -HK85*(-HK71*P(3,13) - HK72*P(4,13) + HK73*P(0,13) + HK74*P(5,13) - HK75*P(2,13) + HK76*P(6,13) + HK77*P(1,13));
|
||||
Kfusion(14) = -HK85*(-HK71*P(3,14) - HK72*P(4,14) + HK73*P(0,14) + HK74*P(5,14) - HK75*P(2,14) + HK76*P(6,14) + HK77*P(1,14));
|
||||
Kfusion(15) = -HK85*(-HK71*P(3,15) - HK72*P(4,15) + HK73*P(0,15) + HK74*P(5,15) - HK75*P(2,15) + HK76*P(6,15) + HK77*P(1,15));
|
||||
Kfusion(16) = -HK85*(-HK71*P(3,16) - HK72*P(4,16) + HK73*P(0,16) + HK74*P(5,16) - HK75*P(2,16) + HK76*P(6,16) + HK77*P(1,16));
|
||||
Kfusion(17) = -HK85*(-HK71*P(3,17) - HK72*P(4,17) + HK73*P(0,17) + HK74*P(5,17) - HK75*P(2,17) + HK76*P(6,17) + HK77*P(1,17));
|
||||
Kfusion(18) = -HK85*(-HK71*P(3,18) - HK72*P(4,18) + HK73*P(0,18) + HK74*P(5,18) - HK75*P(2,18) + HK76*P(6,18) + HK77*P(1,18));
|
||||
Kfusion(19) = -HK85*(-HK71*P(3,19) - HK72*P(4,19) + HK73*P(0,19) + HK74*P(5,19) - HK75*P(2,19) + HK76*P(6,19) + HK77*P(1,19));
|
||||
Kfusion(20) = -HK85*(-HK71*P(3,20) - HK72*P(4,20) + HK73*P(0,20) + HK74*P(5,20) - HK75*P(2,20) + HK76*P(6,20) + HK77*P(1,20));
|
||||
Kfusion(21) = -HK85*(-HK71*P(3,21) - HK72*P(4,21) + HK73*P(0,21) + HK74*P(5,21) - HK75*P(2,21) + HK76*P(6,21) + HK77*P(1,21));
|
||||
Kfusion(22) = -HK85*(-HK71*P(3,22) - HK72*P(4,22) + HK73*P(0,22) + HK74*P(5,22) - HK75*P(2,22) + HK76*P(6,22) + HK77*P(1,22));
|
||||
Kfusion(23) = -HK85*(-HK71*P(3,23) - HK72*P(4,23) + HK73*P(0,23) + HK74*P(5,23) - HK75*P(2,23) + HK76*P(6,23) + HK77*P(1,23));
|
||||
|
||||
|
||||
+21
@@ -0,0 +1,21 @@
|
||||
// Equations for covariance matrix prediction
|
||||
const float S0 = cosf(psi);
|
||||
const float S1 = powf(S0, 2);
|
||||
const float S2 = sinf(psi);
|
||||
const float S3 = powf(S2, 2);
|
||||
const float S4 = S0*dvy + S2*dvx;
|
||||
const float S5 = P(0,2) - P(2,2)*S4;
|
||||
const float S6 = S0*dvx - S2*dvy;
|
||||
const float S7 = S0*S2;
|
||||
const float S8 = P(0,1) + S7*dvxVar - S7*dvyVar;
|
||||
const float S9 = P(1,2) + P(2,2)*S6;
|
||||
|
||||
|
||||
_ekf_gsf[model_index].P(0,0) = P(0,0) - P(0,2)*S4 + S1*dvxVar + S3*dvyVar - S4*S5;
|
||||
_ekf_gsf[model_index].P(0,1) = -P(1,2)*S4 + S5*S6 + S8;
|
||||
_ekf_gsf[model_index].P(1,1) = P(1,1) + P(1,2)*S6 + S1*dvyVar + S3*dvxVar + S6*S9;
|
||||
_ekf_gsf[model_index].P(0,2) = S5;
|
||||
_ekf_gsf[model_index].P(1,2) = S9;
|
||||
_ekf_gsf[model_index].P(2,2) = P(2,2) + dazVar;
|
||||
|
||||
|
||||
+68
@@ -0,0 +1,68 @@
|
||||
// Intermediate variables
|
||||
const float t0 = powf(P(0,1), 2);
|
||||
const float t1 = -t0;
|
||||
const float t2 = P(0,0)*P(1,1) + P(0,0)*velObsVar + P(1,1)*velObsVar + t1 + powf(velObsVar, 2);
|
||||
const float t3 = 1.0F/t2;
|
||||
const float t4 = P(1,1) + velObsVar;
|
||||
const float t5 = P(0,1)*t3;
|
||||
const float t6 = -t5;
|
||||
const float t7 = P(0,0) + velObsVar;
|
||||
const float t8 = P(0,0)*t4 + t1;
|
||||
const float t9 = t5*velObsVar;
|
||||
const float t10 = P(1,1)*t7;
|
||||
const float t11 = t1 + t10;
|
||||
const float t12 = P(0,1)*P(1,2);
|
||||
const float t13 = P(0,2)*t4;
|
||||
const float t14 = P(0,1)*P(0,2);
|
||||
const float t15 = P(1,2)*t7;
|
||||
const float t16 = t0*velObsVar;
|
||||
const float t17 = powf(t2, -2);
|
||||
const float t18 = t4*velObsVar + t8;
|
||||
const float t19 = t17*t18;
|
||||
const float t20 = t17*(t16 + t7*t8);
|
||||
const float t21 = t0 - t10;
|
||||
const float t22 = t17*t21;
|
||||
const float t23 = t14 - t15;
|
||||
const float t24 = P(0,1)*t23;
|
||||
const float t25 = t12 - t13;
|
||||
const float t26 = t16 - t21*t4;
|
||||
const float t27 = t17*t26;
|
||||
const float t28 = t11 + t7*velObsVar;
|
||||
const float t29 = t17*t8;
|
||||
const float t30 = t17*t28;
|
||||
const float t31 = P(0,1)*t25;
|
||||
const float t32 = t23*t4 + t31;
|
||||
const float t33 = t17*t32;
|
||||
const float t34 = P(0,1)*velObsVar;
|
||||
const float t35 = t24 + t25*t7;
|
||||
const float t36 = t17*t35;
|
||||
|
||||
|
||||
// Equations for NE velocity innovation variance's determinante inverse
|
||||
_ekf_gsf[model_index].S_det_inverse = t3;
|
||||
|
||||
|
||||
// Equations for NE velocity innovation variance inverse
|
||||
_ekf_gsf[model_index].S_inverse(0,0) = t3*t4;
|
||||
_ekf_gsf[model_index].S_inverse(0,1) = t6;
|
||||
_ekf_gsf[model_index].S_inverse(1,1) = t3*t7;
|
||||
|
||||
|
||||
// Equations for NE velocity Kalman gain
|
||||
K(0,0) = t3*t8;
|
||||
K(1,0) = t9;
|
||||
K(2,0) = t3*(-t12 + t13);
|
||||
K(0,1) = t9;
|
||||
K(1,1) = t11*t3;
|
||||
K(2,1) = t3*(-t14 + t15);
|
||||
|
||||
|
||||
// Equations for covariance matrix update
|
||||
_ekf_gsf[model_index].P(0,0) = P(0,0) - t16*t19 - t20*t8;
|
||||
_ekf_gsf[model_index].P(0,1) = P(0,1)*(t18*t22 - t20*velObsVar + 1);
|
||||
_ekf_gsf[model_index].P(1,1) = P(1,1) - t16*t30 + t22*t26;
|
||||
_ekf_gsf[model_index].P(0,2) = P(0,2) + t19*t24 + t20*t25;
|
||||
_ekf_gsf[model_index].P(1,2) = P(1,2) + t23*t27 + t30*t31;
|
||||
_ekf_gsf[model_index].P(2,2) = P(2,2) - t23*t33 - t25*t36;
|
||||
|
||||
|
||||
+315
@@ -0,0 +1,315 @@
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <cstdlib>
|
||||
#include "../../../../../matrix/matrix/math.hpp"
|
||||
|
||||
typedef matrix::Vector<float, 24> Vector24f;
|
||||
typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
|
||||
|
||||
float sq(float in) {
|
||||
return in * in;
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
// Compare calculation of observation Jacobian for sympy and matlab generated equations
|
||||
|
||||
// observation Jacobians
|
||||
float H_YAW[4];
|
||||
|
||||
// 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;
|
||||
|
||||
|
||||
// calculate 321 yaw observation matrix using two computational paths to work around singularities
|
||||
// in calculation of the Jacobians.
|
||||
|
||||
{
|
||||
// This first comparison is for the 321 sequence option A equations that have a singularity when
|
||||
// yaw is at +- 90 deg
|
||||
|
||||
// perform calculation using sympy generated equations
|
||||
const float SA0 = 2*q3;
|
||||
const float SA1 = 2*q2;
|
||||
const float SA2 = SA0*q0 + SA1*q1;
|
||||
const float SA3 = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2);
|
||||
const float SA4 = powf(SA3, -2);
|
||||
const float SA5 = 1.0F/(powf(SA2, 2)*SA4 + 1);
|
||||
const float SA6 = 1.0F/SA3;
|
||||
const float SA7 = SA2*SA4;
|
||||
const float SA8 = 2*SA7;
|
||||
const float SA9 = 2*SA6;
|
||||
|
||||
H_YAW[0] = SA5*(SA0*SA6 - SA8*q0);
|
||||
H_YAW[1] = SA5*(SA1*SA6 - SA8*q1);
|
||||
H_YAW[2] = SA5*(SA1*SA7 + SA9*q1);
|
||||
H_YAW[3] = SA5*(SA0*SA7 + SA9*q0);
|
||||
|
||||
// save output and repeat calculation using legacy matlab generated code
|
||||
float H_YAW_sympy[4];
|
||||
for (int row=0; row<4; row++) {
|
||||
H_YAW_sympy[row] = H_YAW[row];
|
||||
}
|
||||
|
||||
// repeat calculation using matlab generated equations
|
||||
float t9 = q0*q3;
|
||||
float t10 = q1*q2;
|
||||
float t2 = t9+t10;
|
||||
float t3 = q0*q0;
|
||||
float t4 = q1*q1;
|
||||
float t5 = q2*q2;
|
||||
float t6 = q3*q3;
|
||||
float t7 = t3+t4-t5-t6;
|
||||
float t16 = q3*t3;
|
||||
float t17 = q3*t5;
|
||||
float t18 = q0*q1*q2*2.0f;
|
||||
float t19 = t16+t17+t18-q3*t4+q3*t6;
|
||||
float t24 = q2*t4;
|
||||
float t25 = q2*t6;
|
||||
float t26 = q0*q1*q3*2.0f;
|
||||
float t27 = t24+t25+t26-q2*t3+q2*t5;
|
||||
float t28 = q1*t3;
|
||||
float t29 = q1*t5;
|
||||
float t30 = q0*q2*q3*2.0f;
|
||||
float t31 = t28+t29+t30+q1*t4-q1*t6;
|
||||
float t32 = q0*t4;
|
||||
float t33 = q0*t6;
|
||||
float t34 = q1*q2*q3*2.0f;
|
||||
float t35 = t32+t33+t34+q0*t3-q0*t5;
|
||||
float t8 = t7*t7;
|
||||
t8 = 1.0f/t8;
|
||||
float t11 = t2*t2;
|
||||
float t12 = t8*t11*4.0f;
|
||||
float t13 = t12+1.0f;
|
||||
float t14 = 1.0f/t13;
|
||||
|
||||
H_YAW[0] = t8*t14*t19*(-2.0f);
|
||||
H_YAW[1] = t8*t14*t27*(-2.0f);
|
||||
H_YAW[2] = t8*t14*t31*2.0f;
|
||||
H_YAW[3] = t8*t14*t35*2.0f;
|
||||
|
||||
// find largest 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<4; row++) {
|
||||
float diff_fraction = fabsf(H_YAW_sympy[row] - H_YAW[row]) / fabsf(H_YAW[row]);
|
||||
if (diff_fraction > max_diff_fraction) {
|
||||
max_diff_fraction = diff_fraction;
|
||||
max_row = row;
|
||||
max_old = H_YAW[row];
|
||||
max_new = H_YAW_sympy[row];
|
||||
}
|
||||
}
|
||||
|
||||
if (max_diff_fraction > 1e-5f) {
|
||||
printf("Fail: 321 yaw option A 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: 321 yaw option A Hfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
|
||||
// This second comparison for the 321 sequence option B equations that have a singularity when
|
||||
// yaw is at 0 and +-190 deg
|
||||
|
||||
// perform calculation using sympy generated equations
|
||||
const float SB0 = 2*q0;
|
||||
const float SB1 = 2*q1;
|
||||
const float SB2 = SB0*q3 + SB1*q2;
|
||||
const float SB3 = powf(SB2, -2);
|
||||
const float SB4 = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2);
|
||||
const float SB5 = 1.0F/(SB3*powf(SB4, 2) + 1);
|
||||
const float SB6 = 1.0F/SB2;
|
||||
const float SB7 = SB3*SB4;
|
||||
const float SB8 = 2*SB7;
|
||||
const float SB9 = 2*SB6;
|
||||
|
||||
H_YAW[0] = -SB5*(SB0*SB6 - SB8*q3);
|
||||
H_YAW[1] = -SB5*(SB1*SB6 - SB8*q2);
|
||||
H_YAW[2] = -SB5*(-SB1*SB7 - SB9*q2);
|
||||
H_YAW[3] = -SB5*(-SB0*SB7 - SB9*q3);
|
||||
|
||||
// save output and repeat calculation using legacy matlab generated code
|
||||
for (int row=0; row<4; row++) {
|
||||
H_YAW_sympy[row] = H_YAW[row];
|
||||
}
|
||||
|
||||
float t15 = t2*t2;
|
||||
t15 = 1.0f/t15;
|
||||
float t20 = t7*t7;
|
||||
float t21 = t15*t20*0.25f;
|
||||
float t22 = t21+1.0f;
|
||||
float t23 = 1.0f/t22;
|
||||
|
||||
H_YAW[0] = t15*t19*t23*(-0.5f);
|
||||
H_YAW[1] = t15*t23*t27*(-0.5f);
|
||||
H_YAW[2] = t15*t23*t31*0.5f;
|
||||
H_YAW[3] = t15*t23*t35*0.5f;
|
||||
|
||||
// find largest difference as a fraction of the matlab value
|
||||
max_diff_fraction = 0.0f;
|
||||
for (int row=0; row<4; row++) {
|
||||
float diff_fraction = fabsf(H_YAW_sympy[row] - H_YAW[row]) / fabsf(H_YAW[row]);
|
||||
if (diff_fraction > max_diff_fraction) {
|
||||
max_diff_fraction = diff_fraction;
|
||||
max_row = row;
|
||||
max_old = H_YAW[row];
|
||||
max_new = H_YAW_sympy[row];
|
||||
}
|
||||
}
|
||||
|
||||
if (max_diff_fraction > 1e-5f) {
|
||||
printf("Fail: 321 yaw option B 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: 321 yaw option B Hfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
}
|
||||
|
||||
// calculate 312 yaw observation matrix using two computational paths to work around singularities
|
||||
// in calculation of the Jacobians.
|
||||
|
||||
{
|
||||
// This first comparison is for the 312 sequence option A equations that have a singularity when
|
||||
// yaw is at +- 90 deg
|
||||
|
||||
// perform calculation using sympy generated equations
|
||||
const float SA0 = 2*q3;
|
||||
const float SA1 = 2*q2;
|
||||
const float SA2 = SA0*q0 - SA1*q1;
|
||||
const float SA3 = powf(q0, 2) - powf(q1, 2) + powf(q2, 2) - powf(q3, 2);
|
||||
const float SA4 = powf(SA3, -2);
|
||||
const float SA5 = 1.0F/(powf(SA2, 2)*SA4 + 1);
|
||||
const float SA6 = 1.0F/SA3;
|
||||
const float SA7 = SA2*SA4;
|
||||
const float SA8 = 2*SA7;
|
||||
const float SA9 = 2*SA6;
|
||||
|
||||
H_YAW[0] = SA5*(SA0*SA6 - SA8*q0);
|
||||
H_YAW[1] = SA5*(-SA1*SA6 + SA8*q1);
|
||||
H_YAW[2] = SA5*(-SA1*SA7 - SA9*q1);
|
||||
H_YAW[3] = SA5*(SA0*SA7 + SA9*q0);
|
||||
|
||||
// save output and repeat calculation using legacy matlab generated code
|
||||
float H_YAW_sympy[4];
|
||||
for (int row=0; row<4; row++) {
|
||||
H_YAW_sympy[row] = H_YAW[row];
|
||||
}
|
||||
|
||||
// repeat calculation using matlab generated equations
|
||||
float t9 = q0*q3;
|
||||
float t10 = q1*q2;
|
||||
float t2 = t9-t10;
|
||||
float t3 = q0*q0;
|
||||
float t4 = q1*q1;
|
||||
float t5 = q2*q2;
|
||||
float t6 = q3*q3;
|
||||
float t7 = t3-t4+t5-t6;
|
||||
float t16 = q3*t3;
|
||||
float t17 = q3*t4;
|
||||
float t18 = t16+t17-q3*t5+q3*t6-q0*q1*q2*2.0f;
|
||||
float t23 = q2*t3;
|
||||
float t24 = q2*t4;
|
||||
float t25 = t23+t24+q2*t5-q2*t6-q0*q1*q3*2.0f;
|
||||
float t26 = q1*t5;
|
||||
float t27 = q1*t6;
|
||||
float t28 = t26+t27-q1*t3+q1*t4-q0*q2*q3*2.0f;
|
||||
float t29 = q0*t5;
|
||||
float t30 = q0*t6;
|
||||
float t31 = t29+t30+q0*t3-q0*t4-q1*q2*q3*2.0f;
|
||||
float t8 = t7*t7;
|
||||
float t15 = t2*t2;
|
||||
t8 = 1.0f/t8;
|
||||
float t11 = t2*t2;
|
||||
float t12 = t8*t11*4.0f;
|
||||
float t13 = t12+1.0f;
|
||||
float t14 = 1.0f/t13;
|
||||
|
||||
H_YAW[0] = t8*t14*t18*(-2.0f);
|
||||
H_YAW[1] = t8*t14*t25*(-2.0f);
|
||||
H_YAW[2] = t8*t14*t28*2.0f;
|
||||
H_YAW[3] = t8*t14*t31*2.0f;
|
||||
|
||||
// find largest 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<4; row++) {
|
||||
float diff_fraction = fabsf(H_YAW_sympy[row] - H_YAW[row]) / fabsf(H_YAW[row]);
|
||||
if (diff_fraction > max_diff_fraction) {
|
||||
max_diff_fraction = diff_fraction;
|
||||
max_row = row;
|
||||
max_old = H_YAW[row];
|
||||
max_new = H_YAW_sympy[row];
|
||||
}
|
||||
}
|
||||
|
||||
if (max_diff_fraction > 1e-5f) {
|
||||
printf("Fail: 312 yaw option A 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: 312 yaw option A Hfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
|
||||
// This second comparison for the 321 sequence option B equations that have a singularity when
|
||||
// yaw is at 0 and +-190 deg
|
||||
|
||||
// perform calculation using sympy generated equations
|
||||
const float SB0 = 2*q0;
|
||||
const float SB1 = 2*q1;
|
||||
const float SB2 = -SB0*q3 + SB1*q2;
|
||||
const float SB3 = powf(SB2, -2);
|
||||
const float SB4 = -powf(q0, 2) + powf(q1, 2) - powf(q2, 2) + powf(q3, 2);
|
||||
const float SB5 = 1.0F/(SB3*powf(SB4, 2) + 1);
|
||||
const float SB6 = 1.0F/SB2;
|
||||
const float SB7 = SB3*SB4;
|
||||
const float SB8 = 2*SB7;
|
||||
const float SB9 = 2*SB6;
|
||||
|
||||
H_YAW[0] = -SB5*(-SB0*SB6 + SB8*q3);
|
||||
H_YAW[1] = -SB5*(SB1*SB6 - SB8*q2);
|
||||
H_YAW[2] = -SB5*(-SB1*SB7 - SB9*q2);
|
||||
H_YAW[3] = -SB5*(SB0*SB7 + SB9*q3);
|
||||
|
||||
// save output and repeat calculation using legacy matlab generated code
|
||||
for (int row=0; row<4; row++) {
|
||||
H_YAW_sympy[row] = H_YAW[row];
|
||||
}
|
||||
|
||||
t15 = 1.0f/t15;
|
||||
float t19 = t7*t7;
|
||||
float t20 = t15*t19*0.25f;
|
||||
float t21 = t20+1.0f;
|
||||
float t22 = 1.0f/t21;
|
||||
|
||||
H_YAW[0] = t15*t18*t22*(-0.5f);
|
||||
H_YAW[1] = t15*t22*t25*(-0.5f);
|
||||
H_YAW[2] = t15*t22*t28*0.5f;
|
||||
H_YAW[3] = t15*t22*t31*0.5f;
|
||||
|
||||
// find largest difference as a fraction of the matlab value
|
||||
max_diff_fraction = 0.0f;
|
||||
for (int row=0; row<4; row++) {
|
||||
float diff_fraction = fabsf(H_YAW_sympy[row] - H_YAW[row]) / fabsf(H_YAW[row]);
|
||||
if (diff_fraction > max_diff_fraction) {
|
||||
max_diff_fraction = diff_fraction;
|
||||
max_row = row;
|
||||
max_old = H_YAW[row];
|
||||
max_new = H_YAW_sympy[row];
|
||||
}
|
||||
}
|
||||
|
||||
if (max_diff_fraction > 1e-5f) {
|
||||
printf("Fail: 312 yaw option B 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: 312 yaw option B Hfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,152 @@
|
||||
// calculate 321 yaw observation matrix - option A
|
||||
const float SA0 = 2*q0;
|
||||
const float SA1 = 2*q1;
|
||||
const float SA2 = SA0*q3 + SA1*q2;
|
||||
const float SA3 = -2*powf(q2, 2) - 2*powf(q3, 2) + 1;
|
||||
const float SA4 = powf(SA3, -2);
|
||||
const float SA5 = 1.0F/(powf(SA2, 2)*SA4 + 1);
|
||||
const float SA6 = 1.0F/SA3;
|
||||
const float SA7 = 2*SA5*SA6;
|
||||
const float SA8 = 4*SA2*SA4;
|
||||
|
||||
|
||||
H_YAW.at<0>() = SA7*q3;
|
||||
H_YAW.at<1>() = SA7*q2;
|
||||
H_YAW.at<2>() = SA5*(SA1*SA6 + SA8*q2);
|
||||
H_YAW.at<3>() = SA5*(SA0*SA6 + SA8*q3);
|
||||
H_YAW.at<4>() = 0;
|
||||
H_YAW.at<5>() = 0;
|
||||
H_YAW.at<6>() = 0;
|
||||
H_YAW.at<7>() = 0;
|
||||
H_YAW.at<8>() = 0;
|
||||
H_YAW.at<9>() = 0;
|
||||
H_YAW.at<10>() = 0;
|
||||
H_YAW.at<11>() = 0;
|
||||
H_YAW.at<12>() = 0;
|
||||
H_YAW.at<13>() = 0;
|
||||
H_YAW.at<14>() = 0;
|
||||
H_YAW.at<15>() = 0;
|
||||
H_YAW.at<16>() = 0;
|
||||
H_YAW.at<17>() = 0;
|
||||
H_YAW.at<18>() = 0;
|
||||
H_YAW.at<19>() = 0;
|
||||
H_YAW.at<20>() = 0;
|
||||
H_YAW.at<21>() = 0;
|
||||
H_YAW.at<22>() = 0;
|
||||
H_YAW.at<23>() = 0;
|
||||
|
||||
|
||||
// calculate 321 yaw observation matrix - option B
|
||||
const float SB0 = 2*q0;
|
||||
const float SB1 = 2*q1;
|
||||
const float SB2 = SB0*q3 + SB1*q2;
|
||||
const float SB3 = powf(SB2, -2);
|
||||
const float SB4 = -2*powf(q2, 2) - 2*powf(q3, 2) + 1;
|
||||
const float SB5 = 1.0F/(SB3*powf(SB4, 2) + 1);
|
||||
const float SB6 = SB3*SB4;
|
||||
const float SB7 = 2*SB5*SB6;
|
||||
const float SB8 = 4/SB2;
|
||||
|
||||
|
||||
H_YAW.at<0>() = SB7*q3;
|
||||
H_YAW.at<1>() = SB7*q2;
|
||||
H_YAW.at<2>() = -SB5*(-SB1*SB6 - SB8*q2);
|
||||
H_YAW.at<3>() = -SB5*(-SB0*SB6 - SB8*q3);
|
||||
H_YAW.at<4>() = 0;
|
||||
H_YAW.at<5>() = 0;
|
||||
H_YAW.at<6>() = 0;
|
||||
H_YAW.at<7>() = 0;
|
||||
H_YAW.at<8>() = 0;
|
||||
H_YAW.at<9>() = 0;
|
||||
H_YAW.at<10>() = 0;
|
||||
H_YAW.at<11>() = 0;
|
||||
H_YAW.at<12>() = 0;
|
||||
H_YAW.at<13>() = 0;
|
||||
H_YAW.at<14>() = 0;
|
||||
H_YAW.at<15>() = 0;
|
||||
H_YAW.at<16>() = 0;
|
||||
H_YAW.at<17>() = 0;
|
||||
H_YAW.at<18>() = 0;
|
||||
H_YAW.at<19>() = 0;
|
||||
H_YAW.at<20>() = 0;
|
||||
H_YAW.at<21>() = 0;
|
||||
H_YAW.at<22>() = 0;
|
||||
H_YAW.at<23>() = 0;
|
||||
|
||||
|
||||
// calculate 312 yaw observation matrix - option A
|
||||
const float SA0 = 2*q0;
|
||||
const float SA1 = 2*q2;
|
||||
const float SA2 = SA0*q3 - SA1*q1;
|
||||
const float SA3 = -2*powf(q1, 2) - 2*powf(q3, 2) + 1;
|
||||
const float SA4 = powf(SA3, -2);
|
||||
const float SA5 = 1.0F/(powf(SA2, 2)*SA4 + 1);
|
||||
const float SA6 = 1.0F/SA3;
|
||||
const float SA7 = 2*SA5*SA6;
|
||||
const float SA8 = 4*SA2*SA4;
|
||||
|
||||
|
||||
H_YAW.at<0>() = SA7*q3;
|
||||
H_YAW.at<1>() = SA5*(-SA1*SA6 + SA8*q1);
|
||||
H_YAW.at<2>() = -SA7*q1;
|
||||
H_YAW.at<3>() = SA5*(SA0*SA6 + SA8*q3);
|
||||
H_YAW.at<4>() = 0;
|
||||
H_YAW.at<5>() = 0;
|
||||
H_YAW.at<6>() = 0;
|
||||
H_YAW.at<7>() = 0;
|
||||
H_YAW.at<8>() = 0;
|
||||
H_YAW.at<9>() = 0;
|
||||
H_YAW.at<10>() = 0;
|
||||
H_YAW.at<11>() = 0;
|
||||
H_YAW.at<12>() = 0;
|
||||
H_YAW.at<13>() = 0;
|
||||
H_YAW.at<14>() = 0;
|
||||
H_YAW.at<15>() = 0;
|
||||
H_YAW.at<16>() = 0;
|
||||
H_YAW.at<17>() = 0;
|
||||
H_YAW.at<18>() = 0;
|
||||
H_YAW.at<19>() = 0;
|
||||
H_YAW.at<20>() = 0;
|
||||
H_YAW.at<21>() = 0;
|
||||
H_YAW.at<22>() = 0;
|
||||
H_YAW.at<23>() = 0;
|
||||
|
||||
|
||||
// calculate 312 yaw observation matrix - option B
|
||||
const float SB0 = 2*q0;
|
||||
const float SB1 = 2*q2;
|
||||
const float SB2 = -SB0*q3 + SB1*q1;
|
||||
const float SB3 = powf(SB2, -2);
|
||||
const float SB4 = 2*powf(q1, 2) + 2*powf(q3, 2) - 1;
|
||||
const float SB5 = 1.0F/(SB3*powf(SB4, 2) + 1);
|
||||
const float SB6 = SB3*SB4;
|
||||
const float SB7 = 2*SB5*SB6;
|
||||
const float SB8 = 4/SB2;
|
||||
|
||||
|
||||
H_YAW.at<0>() = -SB7*q3;
|
||||
H_YAW.at<1>() = -SB5*(-SB1*SB6 + SB8*q1);
|
||||
H_YAW.at<2>() = SB7*q1;
|
||||
H_YAW.at<3>() = -SB5*(SB0*SB6 + SB8*q3);
|
||||
H_YAW.at<4>() = 0;
|
||||
H_YAW.at<5>() = 0;
|
||||
H_YAW.at<6>() = 0;
|
||||
H_YAW.at<7>() = 0;
|
||||
H_YAW.at<8>() = 0;
|
||||
H_YAW.at<9>() = 0;
|
||||
H_YAW.at<10>() = 0;
|
||||
H_YAW.at<11>() = 0;
|
||||
H_YAW.at<12>() = 0;
|
||||
H_YAW.at<13>() = 0;
|
||||
H_YAW.at<14>() = 0;
|
||||
H_YAW.at<15>() = 0;
|
||||
H_YAW.at<16>() = 0;
|
||||
H_YAW.at<17>() = 0;
|
||||
H_YAW.at<18>() = 0;
|
||||
H_YAW.at<19>() = 0;
|
||||
H_YAW.at<20>() = 0;
|
||||
H_YAW.at<21>() = 0;
|
||||
H_YAW.at<22>() = 0;
|
||||
H_YAW.at<23>() = 0;
|
||||
|
||||
|
||||
+640
@@ -0,0 +1,640 @@
|
||||
#!/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 quat2Rot(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]])
|
||||
|
||||
# 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 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
|
||||
|
||||
# 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:]), "Kfusion", 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()
|
||||
|
||||
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 = quat2Rot(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()
|
||||
|
||||
# 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()
|
||||
@@ -0,0 +1,98 @@
|
||||
"""
|
||||
|
||||
This script calculates the observation scalars (H matrix) for fusing optical flow
|
||||
measurements for terrain estimation.
|
||||
|
||||
@author: roman
|
||||
"""
|
||||
|
||||
from sympy import *
|
||||
|
||||
# q: quaternion describing rotation from frame 1 to frame 2
|
||||
# returns a rotation matrix derived form q which describes the same
|
||||
# rotation
|
||||
def quat2Rot(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
|
||||
|
||||
# take an expression calculated by the cse() method and write the expression
|
||||
# into a text file in C format
|
||||
def write_simplified(P_touple, filename, out_name):
|
||||
subs = P_touple[0]
|
||||
P = Matrix(P_touple[1])
|
||||
fd = open(filename, 'a')
|
||||
|
||||
is_vector = P.shape[0] == 1 or P.shape[1] == 1
|
||||
|
||||
# write sub expressions
|
||||
for index, item in enumerate(subs):
|
||||
fd.write('float ' + str(item[0]) + ' = ' + str(item[1]) + ';\n')
|
||||
|
||||
# write actual matrix values
|
||||
fd.write('\n')
|
||||
|
||||
if not is_vector:
|
||||
iterator = range(0,sqrt(len(P)), 1)
|
||||
for row in iterator:
|
||||
for column in iterator:
|
||||
fd.write(out_name + '(' + str(row) + ',' + str(column) + ') = ' + str(P[row, column]) + ';\n')
|
||||
else:
|
||||
iterator = range(0, len(P), 1)
|
||||
|
||||
for item in iterator:
|
||||
fd.write(out_name + '(' + str(item) + ') = ' + str(P[item]) + ';\n')
|
||||
|
||||
fd.write('\n\n')
|
||||
fd.close()
|
||||
|
||||
########## Symbolic variable definition #######################################
|
||||
|
||||
|
||||
# vehicle velocity
|
||||
v_x = Symbol("v_x", real=True) # vehicle body x velocity
|
||||
v_y = Symbol("v_y", real=True) # vehicle body y velocity
|
||||
|
||||
# unit quaternion describing vehicle attitude, qw is real part
|
||||
qw = Symbol("q0", real=True)
|
||||
qx = Symbol("q1", real=True)
|
||||
qy = Symbol("q2", real=True)
|
||||
qz = Symbol("q3", real=True)
|
||||
q_att = Matrix([qw, qx, qy, qz])
|
||||
|
||||
# terrain vertial position in local NED frame
|
||||
_terrain_vpos = Symbol("_terrain_vpos", real=True)
|
||||
|
||||
_terrain_var = Symbol("_terrain_var", real=True)
|
||||
|
||||
# vehicle vertical position in local NED frame
|
||||
pos_z = Symbol("z", real=True)
|
||||
|
||||
R_body_to_earth = quat2Rot(q_att)
|
||||
|
||||
# Optical flow around x axis
|
||||
flow_x = -v_y / (_terrain_vpos - pos_z) * R_body_to_earth[2,2]
|
||||
|
||||
# Calculate observation scalar
|
||||
H_x = Matrix([flow_x]).jacobian(Matrix([_terrain_vpos]))
|
||||
|
||||
H_x_simple = cse(H_x, symbols('t0:30'))
|
||||
|
||||
# Optical flow around y axis
|
||||
flow_y = v_x / (_terrain_vpos - pos_z) * R_body_to_earth[2,2]
|
||||
|
||||
# Calculate observation scalar
|
||||
H_y = Matrix([flow_y]).jacobian(Matrix([_terrain_vpos]))
|
||||
|
||||
H_y_simple = cse(H_y, symbols('t0:30'))
|
||||
|
||||
write_simplified(H_x_simple, "flow_x_observation.txt", "Hx")
|
||||
write_simplified(H_y_simple, "flow_y_observation.txt", "Hy")
|
||||
|
||||
@@ -0,0 +1,67 @@
|
||||
#!/usr/bin/env python2
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
Created on Tue Oct 29 14:11:58 2019
|
||||
|
||||
@author: roman
|
||||
"""
|
||||
|
||||
from sympy import *
|
||||
|
||||
################## Here are the variables you can change to see the effects on the cov matrix ###########################
|
||||
yaw_init = 0.5
|
||||
|
||||
# ground speed in body frame (comes from ekf2)
|
||||
groundspeed_body_x_init = 5
|
||||
groundspeed_body_y_init = 5
|
||||
|
||||
# true airspeed measured by pitot tube
|
||||
V_init = 7
|
||||
|
||||
# heading variance
|
||||
R_yaw_init = rad(15.0)**2
|
||||
|
||||
# sideslip variance
|
||||
R_beta_init = rad(15.0)**2
|
||||
|
||||
# True airspeed measurement variance
|
||||
R_tas_init = 1.4**2
|
||||
|
||||
#########################################################################################################################
|
||||
|
||||
# define symbols: true airspeed, sidslip angle,
|
||||
V, beta, yaw, groundspeed_body_x, groundspeed_body_y = symbols('V beta yaw vx_body vy_body')
|
||||
R_tas, R_beta, R_yaw = symbols('R_tas R_beta R_yaw')
|
||||
|
||||
|
||||
# body x/y component of relative wind vector ( V is what the airspeed sensor measures)
|
||||
Vx = V * cos(beta)
|
||||
Vy = V * sin(beta)
|
||||
|
||||
|
||||
# wind in body frame
|
||||
wind_body_x = groundspeed_body_x - Vx
|
||||
wind_body_y = groundspeed_body_y - Vy
|
||||
|
||||
# wind in earth frame
|
||||
wind_n = cos(yaw) * wind_body_x - sin(yaw) * wind_body_y
|
||||
wind_e = sin(yaw) * wind_body_x + cos(yaw) * wind_body_y
|
||||
wind_earth = Matrix([wind_n, wind_e])
|
||||
|
||||
# jacobian of earth wind vector with respect to states with known uncertainties
|
||||
G = wind_earth.jacobian([V, beta, yaw])
|
||||
|
||||
# initial covariance matrix
|
||||
P = Matrix([[R_tas, 0, 0], [0, R_beta,0], [0,0,R_yaw]])
|
||||
|
||||
# earth wind covariance matrix, assume 0 sideslip angle
|
||||
P_wind_earth = (G*P*G.T).subs([(beta, 0)])
|
||||
|
||||
P_wind_earth_numeric = P_wind_earth.subs([(V, V_init),(yaw, yaw_init), (R_tas, R_tas_init), (R_yaw, R_yaw_init), (R_beta, R_beta_init)])
|
||||
P_wind_earth_numeric = P_wind_earth_numeric.subs([(groundspeed_body_x, groundspeed_body_x_init), (groundspeed_body_y, groundspeed_body_y_init) ])
|
||||
|
||||
|
||||
print('P[22][22] = ' + str(P_wind_earth_numeric[0,0]))
|
||||
print('P[22][23] = ' + str(P_wind_earth_numeric[0,1]))
|
||||
print('P[23][22] = ' + str(P_wind_earth_numeric[1,0]))
|
||||
print('P[23][23] = ' + str(P_wind_earth_numeric[1,1]))
|
||||
Reference in New Issue
Block a user