mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-23 06:36:45 +08:00
EKF: replacement of covariance prediction autocode with sympy generated output (#870)
* added python script with ekf derivation (WIP) Signed-off-by: RomanBapst <bapstroman@gmail.com> * worked on c code auto-generation Signed-off-by: RomanBapst <bapstroman@gmail.com> * save before variable name change Signed-off-by: RomanBapst <bapstroman@gmail.com> * changed symbol names Signed-off-by: RomanBapst <bapstroman@gmail.com> * added codegeneration class Signed-off-by: RomanBapst <bapstroman@gmail.com> * improve 3D mag fusion derivation Signed-off-by: RomanBapst <bapstroman@gmail.com> * EKF: Extend ekf sympy derivation to include all observation types * EKF: Add custom ecl::powf function for integer powers * EKF: Convert ekf covariance prediction to use sympy output * EKF: Add test program to compare sympy and matlab covariance prediction Also tests ecl::powf(x,exp) function * EKF: simplify ecl::powf function * Generate code to subfolder generated/ * Add printouts for showing code generation progress * Move generated covariance code to generated folder * Upgrade code generation to python3 * main.py: Remove unused create_symbols function & making code more compact * main.py: move main part into function * Code generation: fix passing wrong rotation matrix to yaw_observation () * EKF: Amend generated code filename for consistency * Move ecl::powf function test to unit tests * EKF: Use updated ecl:powf functionality in test program * Move ecl::powf to utils.hpp * Update ecl::powf test * Update output change indication * test: update expected output for change indicator * test: update expected output for change indicator again Co-authored-by: RomanBapst <bapstroman@gmail.com> Co-authored-by: kamilritz <kritz@ethz.ch>
This commit is contained in:
Paul Riseborough
GitHub
+453
-271
File diff suppressed because it is too large
Load Diff
@@ -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,55 @@
|
||||
#!/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, identifier, is_symmetric=False):
|
||||
write_string = ""
|
||||
|
||||
if matrix.shape[0] * matrix.shape[1] == 1:
|
||||
write_string = write_string + identifier + " = " + self.get_ccode(matrix[0]) + ";\n"
|
||||
elif matrix.shape[0] == 1 or matrix.shape[1] == 1:
|
||||
for i in range(0,len(matrix)):
|
||||
if (identifier == "Kfusion"):
|
||||
# Vector f format used by Kfusion
|
||||
write_string = write_string + identifier + "(" + str(i) + ") = " + self.get_ccode(matrix[i]) + ";\n"
|
||||
else:
|
||||
# legacy array format used by Hfusion
|
||||
write_string = write_string + identifier + "[" + str(i) + "] = " + 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 + identifier + "(" + str(i) + "," + str(j) + ") = " + self.get_ccode(matrix[i,j]) + ";\n"
|
||||
# legacy array format
|
||||
# write_string = write_string + identifier + "[" + str(i) + "][" + str(j) + "] = " + self.get_ccode(matrix[i,j]) + ";\n"
|
||||
|
||||
write_string = write_string + "\n\n"
|
||||
self.file.write(write_string)
|
||||
|
||||
def close(self):
|
||||
self.file.close()
|
||||
@@ -0,0 +1,492 @@
|
||||
// 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 = -PS2;
|
||||
const float PS42 = 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);
|
||||
|
||||
|
||||
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*powf(PS54, 2) + PS157*powf(PS46, 2) + PS158 + 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*powf(PS68, 2) + PS160*PS173 - PS162*PS170 - PS169*PS65 - PS172*PS60 + PS175*PS72 + PS177*PS74 + PS179*PS62 + PS180*powf(PS66, 2) + PS181 + 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*powf(PS71, 2) - PS165*PS184 + PS166*PS185 + PS180*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) + 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);
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,489 @@
|
||||
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]])
|
||||
|
||||
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_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 Jacobian and Kalman gain
|
||||
def generate_observation_equations(P,state,observation,variance):
|
||||
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]
|
||||
HK_simple = cse(Matrix([H.transpose(), K]), symbols("HK0:1000"), 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")
|
||||
code_generator_id.print_string("Kalman gains")
|
||||
code_generator_id.write_matrix(Matrix(equations[1][0][24:]), "Kfusion")
|
||||
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")
|
||||
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")
|
||||
|
||||
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")
|
||||
vel_bf_code_generator.write_matrix(Matrix(equations[1][0][24:]), "Kfusion")
|
||||
|
||||
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
|
||||
# accXpred = -0.5*rho*vrel[0]*vrel[0]*BCXinv # predicted acceleration measured along X body axis
|
||||
# accYpred = -0.5*rho*vrel[1]*vrel[1]*BCYinv # predicted acceleration measured along Y body axis
|
||||
|
||||
# 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")
|
||||
|
||||
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")
|
||||
|
||||
# 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")
|
||||
|
||||
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")
|
||||
|
||||
yaw_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]
|
||||
for index in axes:
|
||||
equations = generate_observation_equations(P,state,m_mag[index],obs_var)
|
||||
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
|
||||
|
||||
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(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('Code generation finished!')
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
generate_code()
|
||||
@@ -1,5 +1,6 @@
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
#pragma once
|
||||
|
||||
// converts Tait-Bryan 312 sequence of rotations from frame 1 to frame 2
|
||||
// to the corresponding rotation matrix that rotates from frame 2 to frame 1
|
||||
@@ -18,3 +19,20 @@ float kahanSummation(float sum_previous, float input, float &accumulator);
|
||||
// calculate the inverse rotation matrix from a quaternion rotation
|
||||
// this produces the inverse rotation to that produced by the math library quaternion to Dcmf operator
|
||||
matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat);
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -55,6 +55,7 @@ set(SRCS
|
||||
test_SensorRangeFinder.cpp
|
||||
test_geo.cpp
|
||||
test_geo_lookup.cpp
|
||||
test_EKF_utils.cpp
|
||||
)
|
||||
add_executable(ECL_GTESTS ${SRCS})
|
||||
|
||||
|
||||
+346
-346
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,57 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 ECL Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file test_EKF_utils.cpp
|
||||
*
|
||||
* @brief Unit tests for the miscellaneous EKF utilities
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
|
||||
#include "EKF/utils.hpp"
|
||||
|
||||
TEST(eclPowfTest, compareToStandardImplementation)
|
||||
{
|
||||
std::vector<int> exponents = {-3,-2,-1,-0,0,1,2,3};
|
||||
std::vector<float> bases = {-INFINITY, -11.1f,-0.5f, -0.f, 0.f, 0.5f, 11.1f, INFINITY};
|
||||
|
||||
for (auto const exponent : exponents) {
|
||||
for (auto const basis : bases) {
|
||||
EXPECT_EQ(ecl::powf(basis, exponent),
|
||||
std::pow(basis, static_cast<float>(exponent)));
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user