From 3d01b5aa1148b0f4e7b5ffc0f95290d23ff45101 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 1 Jul 2022 16:12:45 +0200 Subject: [PATCH] ekf2_derivation: use unsimplified q->rot for observation equations --- .../generated/3Dmag_generated.cpp | 315 ++++++++-------- .../generated/3Dmag_generated_alt.cpp | 349 +++++++++--------- .../generated/3Dmag_innov_var_generated.cpp | 42 +-- .../generated/acc_bf_generated.cpp | 277 +++++++------- .../generated/acc_bf_generated_alt.cpp | 277 +++++++------- .../generated/beta_generated.cpp | 166 +++++---- .../generated/flow_generated.cpp | 319 ++++++++-------- .../generated/flow_generated_alt.cpp | 297 +++++++-------- .../generated/gps_yaw_generated.cpp | 112 +++--- .../generated/vel_bf_generated.cpp | 323 ++++++++-------- .../generated/vel_bf_generated_alt.cpp | 341 +++++++++-------- .../generated/yaw_generated.cpp | 72 ++-- .../ekf2/EKF/python/ekf_derivation/main.py | 25 +- 13 files changed, 1431 insertions(+), 1484 deletions(-) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated.cpp index eae5017664..b427134726 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated.cpp @@ -1,195 +1,192 @@ // Axis 0 equations // Sub Expressions -const float HKX0 = magD*q2 - magE*q3; -const float HKX1 = magD*q3 + magE*q2; -const float HKX2 = 2*magN; -const float HKX3 = HKX2*q2 + magD*q0 - magE*q1; -const float HKX4 = -HKX2*q3 + magD*q1 + magE*q0; -const float HKX5 = 2*(q2)*(q2) + 2*(q3)*(q3) - 1; -const float HKX6 = q0*q3 + q1*q2; -const float HKX7 = q0*q2 - q1*q3; -const float HKX8 = 2*HKX0; -const float HKX9 = 2*HKX7; -const float HKX10 = 2*HKX3; -const float HKX11 = -2*HKX1*P(0,1) + HKX10*P(0,2) - 2*HKX4*P(0,3) + HKX5*P(0,16) - 2*HKX6*P(0,17) + HKX8*P(0,0) + HKX9*P(0,18) - P(0,19); -const float HKX12 = -2*HKX1*P(1,16) + HKX10*P(2,16) - 2*HKX4*P(3,16) + HKX5*P(16,16) - 2*HKX6*P(16,17) + HKX8*P(0,16) + HKX9*P(16,18) - P(16,19); -const float HKX13 = -2*HKX1*P(1,1) + HKX10*P(1,2) - 2*HKX4*P(1,3) + HKX5*P(1,16) - 2*HKX6*P(1,17) + HKX8*P(0,1) + HKX9*P(1,18) - P(1,19); -const float HKX14 = 2*HKX1; -const float HKX15 = -2*HKX1*P(1,17) + HKX10*P(2,17) - 2*HKX4*P(3,17) + HKX5*P(16,17) - 2*HKX6*P(17,17) + HKX8*P(0,17) + HKX9*P(17,18) - P(17,19); -const float HKX16 = 2*HKX6; -const float HKX17 = -2*HKX1*P(1,3) + HKX10*P(2,3) - 2*HKX4*P(3,3) + HKX5*P(3,16) - 2*HKX6*P(3,17) + HKX8*P(0,3) + HKX9*P(3,18) - P(3,19); -const float HKX18 = 2*HKX4; -const float HKX19 = -2*HKX1*P(1,18) + HKX10*P(2,18) - 2*HKX4*P(3,18) + HKX5*P(16,18) - 2*HKX6*P(17,18) + HKX8*P(0,18) + HKX9*P(18,18) - P(18,19); -const float HKX20 = -2*HKX1*P(1,2) + HKX10*P(2,2) - 2*HKX4*P(2,3) + HKX5*P(2,16) - 2*HKX6*P(2,17) + HKX8*P(0,2) + HKX9*P(2,18) - P(2,19); -const float HKX21 = HKX10*P(2,19) - HKX14*P(1,19) - HKX16*P(17,19) - HKX18*P(3,19) + HKX5*P(16,19) + HKX8*P(0,19) + HKX9*P(18,19) - P(19,19); -const float HKX22 = 1.0F/(-HKX10*HKX20 - HKX11*HKX8 - HKX12*HKX5 + HKX13*HKX14 + HKX15*HKX16 + HKX17*HKX18 - HKX19*HKX9 + HKX21 - R_MAG); +const float HKX0 = -magD*q2 + magE*q3 + magN*q0; +const float HKX1 = magD*q3 + magE*q2 + magN*q1; +const float HKX2 = magD*q0 - magE*q1 + magN*q2; +const float HKX3 = magD*q1 + magE*q0 - magN*q3; +const float HKX4 = (q0)*(q0) + (q1)*(q1) - (q2)*(q2) - (q3)*(q3); +const float HKX5 = q0*q3 + q1*q2; +const float HKX6 = q0*q2 - q1*q3; +const float HKX7 = 2*HKX5; +const float HKX8 = 2*HKX6; +const float HKX9 = 2*HKX1; +const float HKX10 = 2*HKX0; +const float HKX11 = 2*HKX2; +const float HKX12 = 2*HKX3; +const float HKX13 = HKX10*P(0,0) - HKX11*P(0,2) + HKX12*P(0,3) + HKX4*P(0,16) + HKX7*P(0,17) - HKX8*P(0,18) + HKX9*P(0,1) + P(0,19); +const float HKX14 = HKX10*P(0,16) - HKX11*P(2,16) + HKX12*P(3,16) + HKX4*P(16,16) + HKX7*P(16,17) - HKX8*P(16,18) + HKX9*P(1,16) + P(16,19); +const float HKX15 = HKX10*P(0,18) - HKX11*P(2,18) + HKX12*P(3,18) + HKX4*P(16,18) + HKX7*P(17,18) - HKX8*P(18,18) + HKX9*P(1,18) + P(18,19); +const float HKX16 = HKX10*P(0,2) - HKX11*P(2,2) + HKX12*P(2,3) + HKX4*P(2,16) + HKX7*P(2,17) - HKX8*P(2,18) + HKX9*P(1,2) + P(2,19); +const float HKX17 = HKX10*P(0,17) - HKX11*P(2,17) + HKX12*P(3,17) + HKX4*P(16,17) + HKX7*P(17,17) - HKX8*P(17,18) + HKX9*P(1,17) + P(17,19); +const float HKX18 = HKX10*P(0,3) - HKX11*P(2,3) + HKX12*P(3,3) + HKX4*P(3,16) + HKX7*P(3,17) - HKX8*P(3,18) + HKX9*P(1,3) + P(3,19); +const float HKX19 = HKX10*P(0,1) - HKX11*P(1,2) + HKX12*P(1,3) + HKX4*P(1,16) + HKX7*P(1,17) - HKX8*P(1,18) + HKX9*P(1,1) + P(1,19); +const float HKX20 = HKX10*P(0,19) - HKX11*P(2,19) + HKX12*P(3,19) + HKX4*P(16,19) + HKX7*P(17,19) - HKX8*P(18,19) + HKX9*P(1,19) + P(19,19); +const float HKX21 = 1.0F/(HKX10*HKX13 - HKX11*HKX16 + HKX12*HKX18 + HKX14*HKX4 - HKX15*HKX8 + HKX17*HKX7 + HKX19*HKX9 + HKX20 + R_MAG); // Observation Jacobians -Hfusion.at<0>() = -2*HKX0; +Hfusion.at<0>() = 2*HKX0; Hfusion.at<1>() = 2*HKX1; -Hfusion.at<2>() = -2*HKX3; -Hfusion.at<3>() = 2*HKX4; -Hfusion.at<16>() = -HKX5; -Hfusion.at<17>() = 2*HKX6; -Hfusion.at<18>() = -2*HKX7; +Hfusion.at<2>() = -2*HKX2; +Hfusion.at<3>() = 2*HKX3; +Hfusion.at<16>() = HKX4; +Hfusion.at<17>() = 2*HKX5; +Hfusion.at<18>() = -2*HKX6; Hfusion.at<19>() = 1; // Kalman gains -Kfusion(0) = HKX11*HKX22; -Kfusion(1) = HKX13*HKX22; -Kfusion(2) = HKX20*HKX22; -Kfusion(3) = HKX17*HKX22; -Kfusion(4) = HKX22*(HKX10*P(2,4) - HKX14*P(1,4) - HKX16*P(4,17) - HKX18*P(3,4) + HKX5*P(4,16) + HKX8*P(0,4) + HKX9*P(4,18) - P(4,19)); -Kfusion(5) = HKX22*(HKX10*P(2,5) - HKX14*P(1,5) - HKX16*P(5,17) - HKX18*P(3,5) + HKX5*P(5,16) + HKX8*P(0,5) + HKX9*P(5,18) - P(5,19)); -Kfusion(6) = HKX22*(HKX10*P(2,6) - HKX14*P(1,6) - HKX16*P(6,17) - HKX18*P(3,6) + HKX5*P(6,16) + HKX8*P(0,6) + HKX9*P(6,18) - P(6,19)); -Kfusion(7) = HKX22*(HKX10*P(2,7) - HKX14*P(1,7) - HKX16*P(7,17) - HKX18*P(3,7) + HKX5*P(7,16) + HKX8*P(0,7) + HKX9*P(7,18) - P(7,19)); -Kfusion(8) = HKX22*(HKX10*P(2,8) - HKX14*P(1,8) - HKX16*P(8,17) - HKX18*P(3,8) + HKX5*P(8,16) + HKX8*P(0,8) + HKX9*P(8,18) - P(8,19)); -Kfusion(9) = HKX22*(HKX10*P(2,9) - HKX14*P(1,9) - HKX16*P(9,17) - HKX18*P(3,9) + HKX5*P(9,16) + HKX8*P(0,9) + HKX9*P(9,18) - P(9,19)); -Kfusion(10) = HKX22*(HKX10*P(2,10) - HKX14*P(1,10) - HKX16*P(10,17) - HKX18*P(3,10) + HKX5*P(10,16) + HKX8*P(0,10) + HKX9*P(10,18) - P(10,19)); -Kfusion(11) = HKX22*(HKX10*P(2,11) - HKX14*P(1,11) - HKX16*P(11,17) - HKX18*P(3,11) + HKX5*P(11,16) + HKX8*P(0,11) + HKX9*P(11,18) - P(11,19)); -Kfusion(12) = HKX22*(HKX10*P(2,12) - HKX14*P(1,12) - HKX16*P(12,17) - HKX18*P(3,12) + HKX5*P(12,16) + HKX8*P(0,12) + HKX9*P(12,18) - P(12,19)); -Kfusion(13) = HKX22*(HKX10*P(2,13) - HKX14*P(1,13) - HKX16*P(13,17) - HKX18*P(3,13) + HKX5*P(13,16) + HKX8*P(0,13) + HKX9*P(13,18) - P(13,19)); -Kfusion(14) = HKX22*(HKX10*P(2,14) - HKX14*P(1,14) - HKX16*P(14,17) - HKX18*P(3,14) + HKX5*P(14,16) + HKX8*P(0,14) + HKX9*P(14,18) - P(14,19)); -Kfusion(15) = HKX22*(HKX10*P(2,15) - HKX14*P(1,15) - HKX16*P(15,17) - HKX18*P(3,15) + HKX5*P(15,16) + HKX8*P(0,15) + HKX9*P(15,18) - P(15,19)); -Kfusion(16) = HKX12*HKX22; -Kfusion(17) = HKX15*HKX22; -Kfusion(18) = HKX19*HKX22; -Kfusion(19) = HKX21*HKX22; -Kfusion(20) = HKX22*(HKX10*P(2,20) - HKX14*P(1,20) - HKX16*P(17,20) - HKX18*P(3,20) + HKX5*P(16,20) + HKX8*P(0,20) + HKX9*P(18,20) - P(19,20)); -Kfusion(21) = HKX22*(HKX10*P(2,21) - HKX14*P(1,21) - HKX16*P(17,21) - HKX18*P(3,21) + HKX5*P(16,21) + HKX8*P(0,21) + HKX9*P(18,21) - P(19,21)); -Kfusion(22) = HKX22*(HKX10*P(2,22) - HKX14*P(1,22) - HKX16*P(17,22) - HKX18*P(3,22) + HKX5*P(16,22) + HKX8*P(0,22) + HKX9*P(18,22) - P(19,22)); -Kfusion(23) = HKX22*(HKX10*P(2,23) - HKX14*P(1,23) - HKX16*P(17,23) - HKX18*P(3,23) + HKX5*P(16,23) + HKX8*P(0,23) + HKX9*P(18,23) - P(19,23)); +Kfusion(0) = HKX13*HKX21; +Kfusion(1) = HKX19*HKX21; +Kfusion(2) = HKX16*HKX21; +Kfusion(3) = HKX18*HKX21; +Kfusion(4) = HKX21*(HKX10*P(0,4) - HKX11*P(2,4) + HKX12*P(3,4) + HKX4*P(4,16) + HKX7*P(4,17) - HKX8*P(4,18) + HKX9*P(1,4) + P(4,19)); +Kfusion(5) = HKX21*(HKX10*P(0,5) - HKX11*P(2,5) + HKX12*P(3,5) + HKX4*P(5,16) + HKX7*P(5,17) - HKX8*P(5,18) + HKX9*P(1,5) + P(5,19)); +Kfusion(6) = HKX21*(HKX10*P(0,6) - HKX11*P(2,6) + HKX12*P(3,6) + HKX4*P(6,16) + HKX7*P(6,17) - HKX8*P(6,18) + HKX9*P(1,6) + P(6,19)); +Kfusion(7) = HKX21*(HKX10*P(0,7) - HKX11*P(2,7) + HKX12*P(3,7) + HKX4*P(7,16) + HKX7*P(7,17) - HKX8*P(7,18) + HKX9*P(1,7) + P(7,19)); +Kfusion(8) = HKX21*(HKX10*P(0,8) - HKX11*P(2,8) + HKX12*P(3,8) + HKX4*P(8,16) + HKX7*P(8,17) - HKX8*P(8,18) + HKX9*P(1,8) + P(8,19)); +Kfusion(9) = HKX21*(HKX10*P(0,9) - HKX11*P(2,9) + HKX12*P(3,9) + HKX4*P(9,16) + HKX7*P(9,17) - HKX8*P(9,18) + HKX9*P(1,9) + P(9,19)); +Kfusion(10) = HKX21*(HKX10*P(0,10) - HKX11*P(2,10) + HKX12*P(3,10) + HKX4*P(10,16) + HKX7*P(10,17) - HKX8*P(10,18) + HKX9*P(1,10) + P(10,19)); +Kfusion(11) = HKX21*(HKX10*P(0,11) - HKX11*P(2,11) + HKX12*P(3,11) + HKX4*P(11,16) + HKX7*P(11,17) - HKX8*P(11,18) + HKX9*P(1,11) + P(11,19)); +Kfusion(12) = HKX21*(HKX10*P(0,12) - HKX11*P(2,12) + HKX12*P(3,12) + HKX4*P(12,16) + HKX7*P(12,17) - HKX8*P(12,18) + HKX9*P(1,12) + P(12,19)); +Kfusion(13) = HKX21*(HKX10*P(0,13) - HKX11*P(2,13) + HKX12*P(3,13) + HKX4*P(13,16) + HKX7*P(13,17) - HKX8*P(13,18) + HKX9*P(1,13) + P(13,19)); +Kfusion(14) = HKX21*(HKX10*P(0,14) - HKX11*P(2,14) + HKX12*P(3,14) + HKX4*P(14,16) + HKX7*P(14,17) - HKX8*P(14,18) + HKX9*P(1,14) + P(14,19)); +Kfusion(15) = HKX21*(HKX10*P(0,15) - HKX11*P(2,15) + HKX12*P(3,15) + HKX4*P(15,16) + HKX7*P(15,17) - HKX8*P(15,18) + HKX9*P(1,15) + P(15,19)); +Kfusion(16) = HKX14*HKX21; +Kfusion(17) = HKX17*HKX21; +Kfusion(18) = HKX15*HKX21; +Kfusion(19) = HKX20*HKX21; +Kfusion(20) = HKX21*(HKX10*P(0,20) - HKX11*P(2,20) + HKX12*P(3,20) + HKX4*P(16,20) + HKX7*P(17,20) - HKX8*P(18,20) + HKX9*P(1,20) + P(19,20)); +Kfusion(21) = HKX21*(HKX10*P(0,21) - HKX11*P(2,21) + HKX12*P(3,21) + HKX4*P(16,21) + HKX7*P(17,21) - HKX8*P(18,21) + HKX9*P(1,21) + P(19,21)); +Kfusion(22) = HKX21*(HKX10*P(0,22) - HKX11*P(2,22) + HKX12*P(3,22) + HKX4*P(16,22) + HKX7*P(17,22) - HKX8*P(18,22) + HKX9*P(1,22) + P(19,22)); +Kfusion(23) = HKX21*(HKX10*P(0,23) - HKX11*P(2,23) + HKX12*P(3,23) + HKX4*P(16,23) + HKX7*P(17,23) - HKX8*P(18,23) + HKX9*P(1,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 = HKY1*q3 - magD*q2 + magN*q0; -const float HKY5 = q0*q3 - q1*q2; -const float HKY6 = 2*(q1)*(q1) + 2*(q3)*(q3) - 1; -const float HKY7 = q0*q1 + q2*q3; -const float HKY8 = 2*HKY7; -const float HKY9 = 2*HKY3; +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 + magE*q3 + magN*q0; +const float HKY4 = q0*q3 - q1*q2; +const float HKY5 = (q0)*(q0) - (q1)*(q1) + (q2)*(q2) - (q3)*(q3); +const float HKY6 = q0*q1 + q2*q3; +const float HKY7 = 2*HKY6; +const float HKY8 = 2*HKY4; +const float HKY9 = 2*HKY2; const float HKY10 = 2*HKY0; -const float HKY11 = 2*HKY5; -const float HKY12 = 2*HKY2; -const float HKY13 = 2*HKY4; -const float HKY14 = HKY10*P(0,0) - HKY11*P(0,16) + HKY12*P(0,1) - HKY13*P(0,3) - HKY6*P(0,17) + HKY8*P(0,18) + HKY9*P(0,2) + P(0,20); -const float HKY15 = HKY10*P(0,17) - HKY11*P(16,17) + HKY12*P(1,17) - HKY13*P(3,17) - HKY6*P(17,17) + HKY8*P(17,18) + HKY9*P(2,17) + P(17,20); -const float HKY16 = HKY10*P(0,16) - HKY11*P(16,16) + HKY12*P(1,16) - HKY13*P(3,16) - HKY6*P(16,17) + HKY8*P(16,18) + HKY9*P(2,16) + P(16,20); -const float HKY17 = HKY10*P(0,3) - HKY11*P(3,16) + HKY12*P(1,3) - HKY13*P(3,3) - HKY6*P(3,17) + HKY8*P(3,18) + HKY9*P(2,3) + P(3,20); -const float HKY18 = HKY10*P(0,2) - HKY11*P(2,16) + HKY12*P(1,2) - HKY13*P(2,3) - HKY6*P(2,17) + HKY8*P(2,18) + HKY9*P(2,2) + P(2,20); -const float HKY19 = HKY10*P(0,18) - HKY11*P(16,18) + HKY12*P(1,18) - HKY13*P(3,18) - HKY6*P(17,18) + HKY8*P(18,18) + HKY9*P(2,18) + P(18,20); -const float HKY20 = HKY10*P(0,1) - HKY11*P(1,16) + HKY12*P(1,1) - HKY13*P(1,3) - HKY6*P(1,17) + HKY8*P(1,18) + HKY9*P(1,2) + P(1,20); -const float HKY21 = HKY10*P(0,20) - HKY11*P(16,20) + HKY12*P(1,20) - HKY13*P(3,20) - HKY6*P(17,20) + HKY8*P(18,20) + HKY9*P(2,20) + P(20,20); -const float HKY22 = 1.0F/(HKY10*HKY14 - HKY11*HKY16 + HKY12*HKY20 - HKY13*HKY17 - HKY15*HKY6 + HKY18*HKY9 + HKY19*HKY8 + HKY21 + R_MAG); +const float HKY11 = 2*HKY1; +const float HKY12 = 2*HKY3; +const float HKY13 = HKY10*P(0,0) + HKY11*P(0,1) - HKY12*P(0,3) + HKY5*P(0,17) + HKY7*P(0,18) - HKY8*P(0,16) + HKY9*P(0,2) + P(0,20); +const float HKY14 = HKY10*P(0,17) + HKY11*P(1,17) - HKY12*P(3,17) + HKY5*P(17,17) + HKY7*P(17,18) - HKY8*P(16,17) + HKY9*P(2,17) + P(17,20); +const float HKY15 = HKY10*P(0,16) + HKY11*P(1,16) - HKY12*P(3,16) + HKY5*P(16,17) + HKY7*P(16,18) - HKY8*P(16,16) + HKY9*P(2,16) + P(16,20); +const float HKY16 = HKY10*P(0,3) + HKY11*P(1,3) - HKY12*P(3,3) + HKY5*P(3,17) + HKY7*P(3,18) - HKY8*P(3,16) + HKY9*P(2,3) + P(3,20); +const float HKY17 = HKY10*P(0,18) + HKY11*P(1,18) - HKY12*P(3,18) + HKY5*P(17,18) + HKY7*P(18,18) - HKY8*P(16,18) + HKY9*P(2,18) + P(18,20); +const float HKY18 = HKY10*P(0,1) + HKY11*P(1,1) - HKY12*P(1,3) + HKY5*P(1,17) + HKY7*P(1,18) - HKY8*P(1,16) + HKY9*P(1,2) + P(1,20); +const float HKY19 = HKY10*P(0,2) + HKY11*P(1,2) - HKY12*P(2,3) + HKY5*P(2,17) + HKY7*P(2,18) - HKY8*P(2,16) + HKY9*P(2,2) + P(2,20); +const float HKY20 = HKY10*P(0,20) + HKY11*P(1,20) - HKY12*P(3,20) + HKY5*P(17,20) + HKY7*P(18,20) - HKY8*P(16,20) + HKY9*P(2,20) + P(20,20); +const float HKY21 = 1.0F/(HKY10*HKY13 + HKY11*HKY18 - HKY12*HKY16 + HKY14*HKY5 - HKY15*HKY8 + HKY17*HKY7 + HKY19*HKY9 + HKY20 + R_MAG); // Observation Jacobians Hfusion.at<0>() = 2*HKY0; -Hfusion.at<1>() = 2*HKY2; -Hfusion.at<2>() = 2*HKY3; -Hfusion.at<3>() = -2*HKY4; -Hfusion.at<16>() = -2*HKY5; -Hfusion.at<17>() = -HKY6; -Hfusion.at<18>() = 2*HKY7; +Hfusion.at<1>() = 2*HKY1; +Hfusion.at<2>() = 2*HKY2; +Hfusion.at<3>() = -2*HKY3; +Hfusion.at<16>() = -2*HKY4; +Hfusion.at<17>() = HKY5; +Hfusion.at<18>() = 2*HKY6; Hfusion.at<20>() = 1; // Kalman gains -Kfusion(0) = HKY14*HKY22; -Kfusion(1) = HKY20*HKY22; -Kfusion(2) = HKY18*HKY22; -Kfusion(3) = HKY17*HKY22; -Kfusion(4) = HKY22*(HKY10*P(0,4) - HKY11*P(4,16) + HKY12*P(1,4) - HKY13*P(3,4) - HKY6*P(4,17) + HKY8*P(4,18) + HKY9*P(2,4) + P(4,20)); -Kfusion(5) = HKY22*(HKY10*P(0,5) - HKY11*P(5,16) + HKY12*P(1,5) - HKY13*P(3,5) - HKY6*P(5,17) + HKY8*P(5,18) + HKY9*P(2,5) + P(5,20)); -Kfusion(6) = HKY22*(HKY10*P(0,6) - HKY11*P(6,16) + HKY12*P(1,6) - HKY13*P(3,6) - HKY6*P(6,17) + HKY8*P(6,18) + HKY9*P(2,6) + P(6,20)); -Kfusion(7) = HKY22*(HKY10*P(0,7) - HKY11*P(7,16) + HKY12*P(1,7) - HKY13*P(3,7) - HKY6*P(7,17) + HKY8*P(7,18) + HKY9*P(2,7) + P(7,20)); -Kfusion(8) = HKY22*(HKY10*P(0,8) - HKY11*P(8,16) + HKY12*P(1,8) - HKY13*P(3,8) - HKY6*P(8,17) + HKY8*P(8,18) + HKY9*P(2,8) + P(8,20)); -Kfusion(9) = HKY22*(HKY10*P(0,9) - HKY11*P(9,16) + HKY12*P(1,9) - HKY13*P(3,9) - HKY6*P(9,17) + HKY8*P(9,18) + HKY9*P(2,9) + P(9,20)); -Kfusion(10) = HKY22*(HKY10*P(0,10) - HKY11*P(10,16) + HKY12*P(1,10) - HKY13*P(3,10) - HKY6*P(10,17) + HKY8*P(10,18) + HKY9*P(2,10) + P(10,20)); -Kfusion(11) = HKY22*(HKY10*P(0,11) - HKY11*P(11,16) + HKY12*P(1,11) - HKY13*P(3,11) - HKY6*P(11,17) + HKY8*P(11,18) + HKY9*P(2,11) + P(11,20)); -Kfusion(12) = HKY22*(HKY10*P(0,12) - HKY11*P(12,16) + HKY12*P(1,12) - HKY13*P(3,12) - HKY6*P(12,17) + HKY8*P(12,18) + HKY9*P(2,12) + P(12,20)); -Kfusion(13) = HKY22*(HKY10*P(0,13) - HKY11*P(13,16) + HKY12*P(1,13) - HKY13*P(3,13) - HKY6*P(13,17) + HKY8*P(13,18) + HKY9*P(2,13) + P(13,20)); -Kfusion(14) = HKY22*(HKY10*P(0,14) - HKY11*P(14,16) + HKY12*P(1,14) - HKY13*P(3,14) - HKY6*P(14,17) + HKY8*P(14,18) + HKY9*P(2,14) + P(14,20)); -Kfusion(15) = HKY22*(HKY10*P(0,15) - HKY11*P(15,16) + HKY12*P(1,15) - HKY13*P(3,15) - HKY6*P(15,17) + HKY8*P(15,18) + HKY9*P(2,15) + P(15,20)); -Kfusion(16) = HKY16*HKY22; -Kfusion(17) = HKY15*HKY22; -Kfusion(18) = HKY19*HKY22; -Kfusion(19) = HKY22*(HKY10*P(0,19) - HKY11*P(16,19) + HKY12*P(1,19) - HKY13*P(3,19) - HKY6*P(17,19) + HKY8*P(18,19) + HKY9*P(2,19) + P(19,20)); -Kfusion(20) = HKY21*HKY22; -Kfusion(21) = HKY22*(HKY10*P(0,21) - HKY11*P(16,21) + HKY12*P(1,21) - HKY13*P(3,21) - HKY6*P(17,21) + HKY8*P(18,21) + HKY9*P(2,21) + P(20,21)); -Kfusion(22) = HKY22*(HKY10*P(0,22) - HKY11*P(16,22) + HKY12*P(1,22) - HKY13*P(3,22) - HKY6*P(17,22) + HKY8*P(18,22) + HKY9*P(2,22) + P(20,22)); -Kfusion(23) = HKY22*(HKY10*P(0,23) - HKY11*P(16,23) + HKY12*P(1,23) - HKY13*P(3,23) - HKY6*P(17,23) + HKY8*P(18,23) + HKY9*P(2,23) + P(20,23)); +Kfusion(0) = HKY13*HKY21; +Kfusion(1) = HKY18*HKY21; +Kfusion(2) = HKY19*HKY21; +Kfusion(3) = HKY16*HKY21; +Kfusion(4) = HKY21*(HKY10*P(0,4) + HKY11*P(1,4) - HKY12*P(3,4) + HKY5*P(4,17) + HKY7*P(4,18) - HKY8*P(4,16) + HKY9*P(2,4) + P(4,20)); +Kfusion(5) = HKY21*(HKY10*P(0,5) + HKY11*P(1,5) - HKY12*P(3,5) + HKY5*P(5,17) + HKY7*P(5,18) - HKY8*P(5,16) + HKY9*P(2,5) + P(5,20)); +Kfusion(6) = HKY21*(HKY10*P(0,6) + HKY11*P(1,6) - HKY12*P(3,6) + HKY5*P(6,17) + HKY7*P(6,18) - HKY8*P(6,16) + HKY9*P(2,6) + P(6,20)); +Kfusion(7) = HKY21*(HKY10*P(0,7) + HKY11*P(1,7) - HKY12*P(3,7) + HKY5*P(7,17) + HKY7*P(7,18) - HKY8*P(7,16) + HKY9*P(2,7) + P(7,20)); +Kfusion(8) = HKY21*(HKY10*P(0,8) + HKY11*P(1,8) - HKY12*P(3,8) + HKY5*P(8,17) + HKY7*P(8,18) - HKY8*P(8,16) + HKY9*P(2,8) + P(8,20)); +Kfusion(9) = HKY21*(HKY10*P(0,9) + HKY11*P(1,9) - HKY12*P(3,9) + HKY5*P(9,17) + HKY7*P(9,18) - HKY8*P(9,16) + HKY9*P(2,9) + P(9,20)); +Kfusion(10) = HKY21*(HKY10*P(0,10) + HKY11*P(1,10) - HKY12*P(3,10) + HKY5*P(10,17) + HKY7*P(10,18) - HKY8*P(10,16) + HKY9*P(2,10) + P(10,20)); +Kfusion(11) = HKY21*(HKY10*P(0,11) + HKY11*P(1,11) - HKY12*P(3,11) + HKY5*P(11,17) + HKY7*P(11,18) - HKY8*P(11,16) + HKY9*P(2,11) + P(11,20)); +Kfusion(12) = HKY21*(HKY10*P(0,12) + HKY11*P(1,12) - HKY12*P(3,12) + HKY5*P(12,17) + HKY7*P(12,18) - HKY8*P(12,16) + HKY9*P(2,12) + P(12,20)); +Kfusion(13) = HKY21*(HKY10*P(0,13) + HKY11*P(1,13) - HKY12*P(3,13) + HKY5*P(13,17) + HKY7*P(13,18) - HKY8*P(13,16) + HKY9*P(2,13) + P(13,20)); +Kfusion(14) = HKY21*(HKY10*P(0,14) + HKY11*P(1,14) - HKY12*P(3,14) + HKY5*P(14,17) + HKY7*P(14,18) - HKY8*P(14,16) + HKY9*P(2,14) + P(14,20)); +Kfusion(15) = HKY21*(HKY10*P(0,15) + HKY11*P(1,15) - HKY12*P(3,15) + HKY5*P(15,17) + HKY7*P(15,18) - HKY8*P(15,16) + HKY9*P(2,15) + P(15,20)); +Kfusion(16) = HKY15*HKY21; +Kfusion(17) = HKY14*HKY21; +Kfusion(18) = HKY17*HKY21; +Kfusion(19) = HKY21*(HKY10*P(0,19) + HKY11*P(1,19) - HKY12*P(3,19) + HKY5*P(17,19) + HKY7*P(18,19) - HKY8*P(16,19) + HKY9*P(2,19) + P(19,20)); +Kfusion(20) = HKY20*HKY21; +Kfusion(21) = HKY21*(HKY10*P(0,21) + HKY11*P(1,21) - HKY12*P(3,21) + HKY5*P(17,21) + HKY7*P(18,21) - HKY8*P(16,21) + HKY9*P(2,21) + P(20,21)); +Kfusion(22) = HKY21*(HKY10*P(0,22) + HKY11*P(1,22) - HKY12*P(3,22) + HKY5*P(17,22) + HKY7*P(18,22) - HKY8*P(16,22) + HKY9*P(2,22) + P(20,22)); +Kfusion(23) = HKY21*(HKY10*P(0,23) + HKY11*P(1,23) - HKY12*P(3,23) + HKY5*P(17,23) + HKY7*P(18,23) - HKY8*P(16,23) + HKY9*P(2,23) + P(20,23)); // Axis 2 equations // Sub Expressions -const float HKZ0 = magE*q1 - magN*q2; -const float HKZ1 = 2*magD; -const float HKZ2 = HKZ1*q1 + magE*q0 - magN*q3; -const float HKZ3 = -HKZ1*q2 + magE*q3 + magN*q0; -const float HKZ4 = magE*q2 + magN*q1; -const float HKZ5 = q0*q2 + q1*q3; -const float HKZ6 = q0*q1 - q2*q3; -const float HKZ7 = 2*(q1)*(q1) + 2*(q2)*(q2) - 1; -const float HKZ8 = 2*HKZ0; -const float HKZ9 = 2*HKZ6; -const float HKZ10 = 2*HKZ2; -const float HKZ11 = HKZ10*P(0,1) - 2*HKZ3*P(0,2) - 2*HKZ4*P(0,3) - 2*HKZ5*P(0,16) + HKZ7*P(0,18) + HKZ8*P(0,0) + HKZ9*P(0,17) - P(0,21); -const float HKZ12 = HKZ10*P(1,18) - 2*HKZ3*P(2,18) - 2*HKZ4*P(3,18) - 2*HKZ5*P(16,18) + HKZ7*P(18,18) + HKZ8*P(0,18) + HKZ9*P(17,18) - P(18,21); -const float HKZ13 = HKZ10*P(1,3) - 2*HKZ3*P(2,3) - 2*HKZ4*P(3,3) - 2*HKZ5*P(3,16) + HKZ7*P(3,18) + HKZ8*P(0,3) + HKZ9*P(3,17) - P(3,21); -const float HKZ14 = 2*HKZ4; -const float HKZ15 = HKZ10*P(1,16) - 2*HKZ3*P(2,16) - 2*HKZ4*P(3,16) - 2*HKZ5*P(16,16) + HKZ7*P(16,18) + HKZ8*P(0,16) + HKZ9*P(16,17) - P(16,21); -const float HKZ16 = 2*HKZ5; -const float HKZ17 = HKZ10*P(1,2) - 2*HKZ3*P(2,2) - 2*HKZ4*P(2,3) - 2*HKZ5*P(2,16) + HKZ7*P(2,18) + HKZ8*P(0,2) + HKZ9*P(2,17) - P(2,21); -const float HKZ18 = 2*HKZ3; -const float HKZ19 = HKZ10*P(1,17) - 2*HKZ3*P(2,17) - 2*HKZ4*P(3,17) - 2*HKZ5*P(16,17) + HKZ7*P(17,18) + HKZ8*P(0,17) + HKZ9*P(17,17) - P(17,21); -const float HKZ20 = HKZ10*P(1,1) - 2*HKZ3*P(1,2) - 2*HKZ4*P(1,3) - 2*HKZ5*P(1,16) + HKZ7*P(1,18) + HKZ8*P(0,1) + HKZ9*P(1,17) - P(1,21); -const float HKZ21 = HKZ10*P(1,21) - HKZ14*P(3,21) - HKZ16*P(16,21) - HKZ18*P(2,21) + HKZ7*P(18,21) + HKZ8*P(0,21) + HKZ9*P(17,21) - P(21,21); -const float HKZ22 = 1.0F/(-HKZ10*HKZ20 - HKZ11*HKZ8 - HKZ12*HKZ7 + HKZ13*HKZ14 + HKZ15*HKZ16 + HKZ17*HKZ18 - HKZ19*HKZ9 + HKZ21 - R_MAG); +const float HKZ0 = magD*q0 - magE*q1 + magN*q2; +const float HKZ1 = magD*q1 + magE*q0 - magN*q3; +const float HKZ2 = -magD*q2 + magE*q3 + magN*q0; +const float HKZ3 = magD*q3 + magE*q2 + magN*q1; +const float HKZ4 = q0*q2 + q1*q3; +const float HKZ5 = q0*q1 - q2*q3; +const float HKZ6 = (q0)*(q0) - (q1)*(q1) - (q2)*(q2) + (q3)*(q3); +const float HKZ7 = 2*HKZ4; +const float HKZ8 = 2*HKZ5; +const float HKZ9 = 2*HKZ3; +const float HKZ10 = 2*HKZ0; +const float HKZ11 = 2*HKZ1; +const float HKZ12 = 2*HKZ2; +const float HKZ13 = HKZ10*P(0,0) - HKZ11*P(0,1) + HKZ12*P(0,2) + HKZ6*P(0,18) + HKZ7*P(0,16) - HKZ8*P(0,17) + HKZ9*P(0,3) + P(0,21); +const float HKZ14 = HKZ10*P(0,18) - HKZ11*P(1,18) + HKZ12*P(2,18) + HKZ6*P(18,18) + HKZ7*P(16,18) - HKZ8*P(17,18) + HKZ9*P(3,18) + P(18,21); +const float HKZ15 = HKZ10*P(0,17) - HKZ11*P(1,17) + HKZ12*P(2,17) + HKZ6*P(17,18) + HKZ7*P(16,17) - HKZ8*P(17,17) + HKZ9*P(3,17) + P(17,21); +const float HKZ16 = HKZ10*P(0,1) - HKZ11*P(1,1) + HKZ12*P(1,2) + HKZ6*P(1,18) + HKZ7*P(1,16) - HKZ8*P(1,17) + HKZ9*P(1,3) + P(1,21); +const float HKZ17 = HKZ10*P(0,16) - HKZ11*P(1,16) + HKZ12*P(2,16) + HKZ6*P(16,18) + HKZ7*P(16,16) - HKZ8*P(16,17) + HKZ9*P(3,16) + P(16,21); +const float HKZ18 = HKZ10*P(0,3) - HKZ11*P(1,3) + HKZ12*P(2,3) + HKZ6*P(3,18) + HKZ7*P(3,16) - HKZ8*P(3,17) + HKZ9*P(3,3) + P(3,21); +const float HKZ19 = HKZ10*P(0,2) - HKZ11*P(1,2) + HKZ12*P(2,2) + HKZ6*P(2,18) + HKZ7*P(2,16) - HKZ8*P(2,17) + HKZ9*P(2,3) + P(2,21); +const float HKZ20 = HKZ10*P(0,21) - HKZ11*P(1,21) + HKZ12*P(2,21) + HKZ6*P(18,21) + HKZ7*P(16,21) - HKZ8*P(17,21) + HKZ9*P(3,21) + P(21,21); +const float HKZ21 = 1.0F/(HKZ10*HKZ13 - HKZ11*HKZ16 + HKZ12*HKZ19 + HKZ14*HKZ6 - HKZ15*HKZ8 + HKZ17*HKZ7 + HKZ18*HKZ9 + HKZ20 + R_MAG); // Observation Jacobians -Hfusion.at<0>() = -2*HKZ0; -Hfusion.at<1>() = -2*HKZ2; -Hfusion.at<2>() = 2*HKZ3; -Hfusion.at<3>() = 2*HKZ4; -Hfusion.at<16>() = 2*HKZ5; -Hfusion.at<17>() = -2*HKZ6; -Hfusion.at<18>() = -HKZ7; +Hfusion.at<0>() = 2*HKZ0; +Hfusion.at<1>() = -2*HKZ1; +Hfusion.at<2>() = 2*HKZ2; +Hfusion.at<3>() = 2*HKZ3; +Hfusion.at<16>() = 2*HKZ4; +Hfusion.at<17>() = -2*HKZ5; +Hfusion.at<18>() = HKZ6; Hfusion.at<21>() = 1; // Kalman gains -Kfusion(0) = HKZ11*HKZ22; -Kfusion(1) = HKZ20*HKZ22; -Kfusion(2) = HKZ17*HKZ22; -Kfusion(3) = HKZ13*HKZ22; -Kfusion(4) = HKZ22*(HKZ10*P(1,4) - HKZ14*P(3,4) - HKZ16*P(4,16) - HKZ18*P(2,4) + HKZ7*P(4,18) + HKZ8*P(0,4) + HKZ9*P(4,17) - P(4,21)); -Kfusion(5) = HKZ22*(HKZ10*P(1,5) - HKZ14*P(3,5) - HKZ16*P(5,16) - HKZ18*P(2,5) + HKZ7*P(5,18) + HKZ8*P(0,5) + HKZ9*P(5,17) - P(5,21)); -Kfusion(6) = HKZ22*(HKZ10*P(1,6) - HKZ14*P(3,6) - HKZ16*P(6,16) - HKZ18*P(2,6) + HKZ7*P(6,18) + HKZ8*P(0,6) + HKZ9*P(6,17) - P(6,21)); -Kfusion(7) = HKZ22*(HKZ10*P(1,7) - HKZ14*P(3,7) - HKZ16*P(7,16) - HKZ18*P(2,7) + HKZ7*P(7,18) + HKZ8*P(0,7) + HKZ9*P(7,17) - P(7,21)); -Kfusion(8) = HKZ22*(HKZ10*P(1,8) - HKZ14*P(3,8) - HKZ16*P(8,16) - HKZ18*P(2,8) + HKZ7*P(8,18) + HKZ8*P(0,8) + HKZ9*P(8,17) - P(8,21)); -Kfusion(9) = HKZ22*(HKZ10*P(1,9) - HKZ14*P(3,9) - HKZ16*P(9,16) - HKZ18*P(2,9) + HKZ7*P(9,18) + HKZ8*P(0,9) + HKZ9*P(9,17) - P(9,21)); -Kfusion(10) = HKZ22*(HKZ10*P(1,10) - HKZ14*P(3,10) - HKZ16*P(10,16) - HKZ18*P(2,10) + HKZ7*P(10,18) + HKZ8*P(0,10) + HKZ9*P(10,17) - P(10,21)); -Kfusion(11) = HKZ22*(HKZ10*P(1,11) - HKZ14*P(3,11) - HKZ16*P(11,16) - HKZ18*P(2,11) + HKZ7*P(11,18) + HKZ8*P(0,11) + HKZ9*P(11,17) - P(11,21)); -Kfusion(12) = HKZ22*(HKZ10*P(1,12) - HKZ14*P(3,12) - HKZ16*P(12,16) - HKZ18*P(2,12) + HKZ7*P(12,18) + HKZ8*P(0,12) + HKZ9*P(12,17) - P(12,21)); -Kfusion(13) = HKZ22*(HKZ10*P(1,13) - HKZ14*P(3,13) - HKZ16*P(13,16) - HKZ18*P(2,13) + HKZ7*P(13,18) + HKZ8*P(0,13) + HKZ9*P(13,17) - P(13,21)); -Kfusion(14) = HKZ22*(HKZ10*P(1,14) - HKZ14*P(3,14) - HKZ16*P(14,16) - HKZ18*P(2,14) + HKZ7*P(14,18) + HKZ8*P(0,14) + HKZ9*P(14,17) - P(14,21)); -Kfusion(15) = HKZ22*(HKZ10*P(1,15) - HKZ14*P(3,15) - HKZ16*P(15,16) - HKZ18*P(2,15) + HKZ7*P(15,18) + HKZ8*P(0,15) + HKZ9*P(15,17) - P(15,21)); -Kfusion(16) = HKZ15*HKZ22; -Kfusion(17) = HKZ19*HKZ22; -Kfusion(18) = HKZ12*HKZ22; -Kfusion(19) = HKZ22*(HKZ10*P(1,19) - HKZ14*P(3,19) - HKZ16*P(16,19) - HKZ18*P(2,19) + HKZ7*P(18,19) + HKZ8*P(0,19) + HKZ9*P(17,19) - P(19,21)); -Kfusion(20) = HKZ22*(HKZ10*P(1,20) - HKZ14*P(3,20) - HKZ16*P(16,20) - HKZ18*P(2,20) + HKZ7*P(18,20) + HKZ8*P(0,20) + HKZ9*P(17,20) - P(20,21)); -Kfusion(21) = HKZ21*HKZ22; -Kfusion(22) = HKZ22*(HKZ10*P(1,22) - HKZ14*P(3,22) - HKZ16*P(16,22) - HKZ18*P(2,22) + HKZ7*P(18,22) + HKZ8*P(0,22) + HKZ9*P(17,22) - P(21,22)); -Kfusion(23) = HKZ22*(HKZ10*P(1,23) - HKZ14*P(3,23) - HKZ16*P(16,23) - HKZ18*P(2,23) + HKZ7*P(18,23) + HKZ8*P(0,23) + HKZ9*P(17,23) - P(21,23)); +Kfusion(0) = HKZ13*HKZ21; +Kfusion(1) = HKZ16*HKZ21; +Kfusion(2) = HKZ19*HKZ21; +Kfusion(3) = HKZ18*HKZ21; +Kfusion(4) = HKZ21*(HKZ10*P(0,4) - HKZ11*P(1,4) + HKZ12*P(2,4) + HKZ6*P(4,18) + HKZ7*P(4,16) - HKZ8*P(4,17) + HKZ9*P(3,4) + P(4,21)); +Kfusion(5) = HKZ21*(HKZ10*P(0,5) - HKZ11*P(1,5) + HKZ12*P(2,5) + HKZ6*P(5,18) + HKZ7*P(5,16) - HKZ8*P(5,17) + HKZ9*P(3,5) + P(5,21)); +Kfusion(6) = HKZ21*(HKZ10*P(0,6) - HKZ11*P(1,6) + HKZ12*P(2,6) + HKZ6*P(6,18) + HKZ7*P(6,16) - HKZ8*P(6,17) + HKZ9*P(3,6) + P(6,21)); +Kfusion(7) = HKZ21*(HKZ10*P(0,7) - HKZ11*P(1,7) + HKZ12*P(2,7) + HKZ6*P(7,18) + HKZ7*P(7,16) - HKZ8*P(7,17) + HKZ9*P(3,7) + P(7,21)); +Kfusion(8) = HKZ21*(HKZ10*P(0,8) - HKZ11*P(1,8) + HKZ12*P(2,8) + HKZ6*P(8,18) + HKZ7*P(8,16) - HKZ8*P(8,17) + HKZ9*P(3,8) + P(8,21)); +Kfusion(9) = HKZ21*(HKZ10*P(0,9) - HKZ11*P(1,9) + HKZ12*P(2,9) + HKZ6*P(9,18) + HKZ7*P(9,16) - HKZ8*P(9,17) + HKZ9*P(3,9) + P(9,21)); +Kfusion(10) = HKZ21*(HKZ10*P(0,10) - HKZ11*P(1,10) + HKZ12*P(2,10) + HKZ6*P(10,18) + HKZ7*P(10,16) - HKZ8*P(10,17) + HKZ9*P(3,10) + P(10,21)); +Kfusion(11) = HKZ21*(HKZ10*P(0,11) - HKZ11*P(1,11) + HKZ12*P(2,11) + HKZ6*P(11,18) + HKZ7*P(11,16) - HKZ8*P(11,17) + HKZ9*P(3,11) + P(11,21)); +Kfusion(12) = HKZ21*(HKZ10*P(0,12) - HKZ11*P(1,12) + HKZ12*P(2,12) + HKZ6*P(12,18) + HKZ7*P(12,16) - HKZ8*P(12,17) + HKZ9*P(3,12) + P(12,21)); +Kfusion(13) = HKZ21*(HKZ10*P(0,13) - HKZ11*P(1,13) + HKZ12*P(2,13) + HKZ6*P(13,18) + HKZ7*P(13,16) - HKZ8*P(13,17) + HKZ9*P(3,13) + P(13,21)); +Kfusion(14) = HKZ21*(HKZ10*P(0,14) - HKZ11*P(1,14) + HKZ12*P(2,14) + HKZ6*P(14,18) + HKZ7*P(14,16) - HKZ8*P(14,17) + HKZ9*P(3,14) + P(14,21)); +Kfusion(15) = HKZ21*(HKZ10*P(0,15) - HKZ11*P(1,15) + HKZ12*P(2,15) + HKZ6*P(15,18) + HKZ7*P(15,16) - HKZ8*P(15,17) + HKZ9*P(3,15) + P(15,21)); +Kfusion(16) = HKZ17*HKZ21; +Kfusion(17) = HKZ15*HKZ21; +Kfusion(18) = HKZ14*HKZ21; +Kfusion(19) = HKZ21*(HKZ10*P(0,19) - HKZ11*P(1,19) + HKZ12*P(2,19) + HKZ6*P(18,19) + HKZ7*P(16,19) - HKZ8*P(17,19) + HKZ9*P(3,19) + P(19,21)); +Kfusion(20) = HKZ21*(HKZ10*P(0,20) - HKZ11*P(1,20) + HKZ12*P(2,20) + HKZ6*P(18,20) + HKZ7*P(16,20) - HKZ8*P(17,20) + HKZ9*P(3,20) + P(20,21)); +Kfusion(21) = HKZ20*HKZ21; +Kfusion(22) = HKZ21*(HKZ10*P(0,22) - HKZ11*P(1,22) + HKZ12*P(2,22) + HKZ6*P(18,22) + HKZ7*P(16,22) - HKZ8*P(17,22) + HKZ9*P(3,22) + P(21,22)); +Kfusion(23) = HKZ21*(HKZ10*P(0,23) - HKZ11*P(1,23) + HKZ12*P(2,23) + HKZ6*P(18,23) + HKZ7*P(16,23) - HKZ8*P(17,23) + HKZ9*P(3,23) + P(21,23)); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated_alt.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated_alt.cpp index 3069162821..8335eaf49c 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated_alt.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated_alt.cpp @@ -1,204 +1,193 @@ // Sub Expressions -const float HK0 = magD*q2; -const float HK1 = HK0 - magE*q3; -const float HK2 = magD*q3; -const float HK3 = magE*q2; -const float HK4 = HK2 + HK3; -const float HK5 = magD*q0; -const float HK6 = magE*q1; -const float HK7 = magN*q2; -const float HK8 = HK5 - HK6 + 2*HK7; -const float HK9 = magD*q1; -const float HK10 = magE*q0; -const float HK11 = magN*q3; -const float HK12 = HK10 - 2*HK11 + HK9; -const float HK13 = 2*(q2)*(q2); -const float HK14 = 2*(q3)*(q3) - 1; -const float HK15 = HK13 + HK14; -const float HK16 = q0*q3; -const float HK17 = HK16 + q1*q2; -const float HK18 = q0*q2; -const float HK19 = HK18 - q1*q3; -const float HK20 = 2*HK1; -const float HK21 = 2*HK19; -const float HK22 = 2*HK8; -const float HK23 = -2*HK12*P(0,3) + HK15*P(0,16) - 2*HK17*P(0,17) + HK20*P(0,0) + HK21*P(0,18) + HK22*P(0,2) - 2*HK4*P(0,1) - P(0,19); -const float HK24 = -R_MAG; -const float HK25 = -2*HK12*P(3,16) + HK15*P(16,16) - 2*HK17*P(16,17) + HK20*P(0,16) + HK21*P(16,18) + HK22*P(2,16) - 2*HK4*P(1,16) - P(16,19); -const float HK26 = -2*HK12*P(1,3) + HK15*P(1,16) - 2*HK17*P(1,17) + HK20*P(0,1) + HK21*P(1,18) + HK22*P(1,2) - 2*HK4*P(1,1) - P(1,19); -const float HK27 = 2*HK4; -const float HK28 = -2*HK12*P(3,17) + HK15*P(16,17) - 2*HK17*P(17,17) + HK20*P(0,17) + HK21*P(17,18) + HK22*P(2,17) - 2*HK4*P(1,17) - P(17,19); -const float HK29 = 2*HK17; -const float HK30 = -2*HK12*P(3,3) + HK15*P(3,16) - 2*HK17*P(3,17) + HK20*P(0,3) + HK21*P(3,18) + HK22*P(2,3) - 2*HK4*P(1,3) - P(3,19); -const float HK31 = 2*HK12; -const float HK32 = -2*HK12*P(3,18) + HK15*P(16,18) - 2*HK17*P(17,18) + HK20*P(0,18) + HK21*P(18,18) + HK22*P(2,18) - 2*HK4*P(1,18) - P(18,19); -const float HK33 = -2*HK12*P(2,3) + HK15*P(2,16) - 2*HK17*P(2,17) + HK20*P(0,2) + HK21*P(2,18) + HK22*P(2,2) - 2*HK4*P(1,2) - P(2,19); -const float HK34 = HK15*P(16,19) + HK20*P(0,19) + HK21*P(18,19) + HK22*P(2,19) - HK27*P(1,19) - HK29*P(17,19) - HK31*P(3,19) - P(19,19); -const float HK35 = 1.0F/(-HK15*HK25 - HK20*HK23 - HK21*HK32 - HK22*HK33 + HK24 + HK26*HK27 + HK28*HK29 + HK30*HK31 + HK34); -const float HK36 = -P(19,21); -const float HK37 = -HK11; -const float HK38 = HK37 + HK9; -const float HK39 = HK5 - 2*HK6 + HK7; -const float HK40 = magN*q1; -const float HK41 = HK2 + HK40; -const float HK42 = magN*q0; -const float HK43 = magE*q3; -const float HK44 = -HK0 + HK42 + 2*HK43; -const float HK45 = HK16 - q1*q2; -const float HK46 = 2*(q1)*(q1); -const float HK47 = HK14 + HK46; -const float HK48 = q0*q1; -const float HK49 = HK48 + q2*q3; -const float HK50 = 2*HK49; -const float HK51 = 2*HK41; -const float HK52 = 2*HK38; -const float HK53 = 2*HK45; -const float HK54 = 2*HK39; -const float HK55 = 2*HK44; -const float HK56 = -HK47*P(0,17) + HK50*P(0,18) + HK51*P(0,2) + HK52*P(0,0) - HK53*P(0,16) + HK54*P(0,1) - HK55*P(0,3) + P(0,20); -const float HK57 = -HK47*P(17,17) + HK50*P(17,18) + HK51*P(2,17) + HK52*P(0,17) - HK53*P(16,17) + HK54*P(1,17) - HK55*P(3,17) + P(17,20); -const float HK58 = -HK47*P(16,17) + HK50*P(16,18) + HK51*P(2,16) + HK52*P(0,16) - HK53*P(16,16) + HK54*P(1,16) - HK55*P(3,16) + P(16,20); -const float HK59 = -HK47*P(3,17) + HK50*P(3,18) + HK51*P(2,3) + HK52*P(0,3) - HK53*P(3,16) + HK54*P(1,3) - HK55*P(3,3) + P(3,20); -const float HK60 = -HK47*P(2,17) + HK50*P(2,18) + HK51*P(2,2) + HK52*P(0,2) - HK53*P(2,16) + HK54*P(1,2) - HK55*P(2,3) + P(2,20); -const float HK61 = -HK47*P(17,18) + HK50*P(18,18) + HK51*P(2,18) + HK52*P(0,18) - HK53*P(16,18) + HK54*P(1,18) - HK55*P(3,18) + P(18,20); -const float HK62 = -HK47*P(1,17) + HK50*P(1,18) + HK51*P(1,2) + HK52*P(0,1) - HK53*P(1,16) + HK54*P(1,1) - HK55*P(1,3) + P(1,20); -const float HK63 = -HK47*P(17,20) + HK50*P(18,20) + HK51*P(2,20) + HK52*P(0,20) - HK53*P(16,20) + HK54*P(1,20) - HK55*P(3,20) + P(20,20); -const float HK64 = 1.0F/(-HK47*HK57 + HK50*HK61 + HK51*HK60 + HK52*HK56 - HK53*HK58 + HK54*HK62 - HK55*HK59 + HK63 + R_MAG); -const float HK65 = HK6 - magN*q2; -const float HK66 = HK10 + HK37 + 2*HK9; -const float HK67 = -2*HK0 + HK42 + HK43; -const float HK68 = HK3 + HK40; -const float HK69 = HK18 + q1*q3; -const float HK70 = HK48 - q2*q3; -const float HK71 = HK13 + HK46 - 1; -const float HK72 = 2*HK65; -const float HK73 = 2*HK70; -const float HK74 = 2*HK66; -const float HK75 = -2*HK67*P(0,2) - 2*HK68*P(0,3) - 2*HK69*P(0,16) + HK71*P(0,18) + HK72*P(0,0) + HK73*P(0,17) + HK74*P(0,1) - P(0,21); -const float HK76 = -2*HK67*P(2,18) - 2*HK68*P(3,18) - 2*HK69*P(16,18) + HK71*P(18,18) + HK72*P(0,18) + HK73*P(17,18) + HK74*P(1,18) - P(18,21); -const float HK77 = -2*HK67*P(2,3) - 2*HK68*P(3,3) - 2*HK69*P(3,16) + HK71*P(3,18) + HK72*P(0,3) + HK73*P(3,17) + HK74*P(1,3) - P(3,21); -const float HK78 = 2*HK68; -const float HK79 = -2*HK67*P(2,16) - 2*HK68*P(3,16) - 2*HK69*P(16,16) + HK71*P(16,18) + HK72*P(0,16) + HK73*P(16,17) + HK74*P(1,16) - P(16,21); -const float HK80 = 2*HK69; -const float HK81 = -2*HK67*P(2,2) - 2*HK68*P(2,3) - 2*HK69*P(2,16) + HK71*P(2,18) + HK72*P(0,2) + HK73*P(2,17) + HK74*P(1,2) - P(2,21); -const float HK82 = 2*HK67; -const float HK83 = -2*HK67*P(2,17) - 2*HK68*P(3,17) - 2*HK69*P(16,17) + HK71*P(17,18) + HK72*P(0,17) + HK73*P(17,17) + HK74*P(1,17) - P(17,21); -const float HK84 = -2*HK67*P(1,2) - 2*HK68*P(1,3) - 2*HK69*P(1,16) + HK71*P(1,18) + HK72*P(0,1) + HK73*P(1,17) + HK74*P(1,1) - P(1,21); -const float HK85 = HK71*P(18,21) + HK72*P(0,21) + HK73*P(17,21) + HK74*P(1,21) - HK78*P(3,21) - HK80*P(16,21) - HK82*P(2,21) - P(21,21); -const float HK86 = 1.0F/(HK24 - HK71*HK76 - HK72*HK75 - HK73*HK83 - HK74*HK84 + HK77*HK78 + HK79*HK80 + HK81*HK82 + HK85); +const float HK0 = -magD*q2 + magE*q3 + magN*q0; +const float HK1 = 2*HK0; +const float HK2 = magD*q3 + magE*q2 + magN*q1; +const float HK3 = 2*HK2; +const float HK4 = magD*q0 - magE*q1 + magN*q2; +const float HK5 = magD*q1 + magE*q0 - magN*q3; +const float HK6 = 2*HK5; +const float HK7 = (q1)*(q1); +const float HK8 = (q2)*(q2); +const float HK9 = -HK8; +const float HK10 = (q0)*(q0); +const float HK11 = (q3)*(q3); +const float HK12 = HK10 - HK11; +const float HK13 = HK12 + HK7 + HK9; +const float HK14 = q0*q3; +const float HK15 = HK14 + q1*q2; +const float HK16 = q0*q2; +const float HK17 = HK16 - q1*q3; +const float HK18 = 2*HK15; +const float HK19 = 2*HK17; +const float HK20 = 2*HK2; +const float HK21 = 2*HK0; +const float HK22 = 2*HK4; +const float HK23 = HK22*P(0,2); +const float HK24 = 2*HK5; +const float HK25 = HK24*P(0,3); +const float HK26 = HK13*P(0,16) + HK18*P(0,17) - HK19*P(0,18) + HK20*P(0,1) + HK21*P(0,0) - HK23 + HK25 + P(0,19); +const float HK27 = HK13*P(16,16) + HK18*P(16,17) - HK19*P(16,18) + HK20*P(1,16) + HK21*P(0,16) - HK22*P(2,16) + HK24*P(3,16) + P(16,19); +const float HK28 = HK13*P(16,18) + HK18*P(17,18) - HK19*P(18,18) + HK20*P(1,18) + HK21*P(0,18) - HK22*P(2,18) + HK24*P(3,18) + P(18,19); +const float HK29 = HK20*P(1,2); +const float HK30 = HK21*P(0,2); +const float HK31 = HK13*P(2,16) + HK18*P(2,17) - HK19*P(2,18) - HK22*P(2,2) + HK24*P(2,3) + HK29 + HK30 + P(2,19); +const float HK32 = HK13*P(16,17) + HK18*P(17,17) - HK19*P(17,18) + HK20*P(1,17) + HK21*P(0,17) - HK22*P(2,17) + HK24*P(3,17) + P(17,19); +const float HK33 = HK20*P(1,3); +const float HK34 = HK21*P(0,3); +const float HK35 = HK13*P(3,16) + HK18*P(3,17) - HK19*P(3,18) - HK22*P(2,3) + HK24*P(3,3) + HK33 + HK34 + P(3,19); +const float HK36 = HK22*P(1,2); +const float HK37 = HK24*P(1,3); +const float HK38 = HK13*P(1,16) + HK18*P(1,17) - HK19*P(1,18) + HK20*P(1,1) + HK21*P(0,1) - HK36 + HK37 + P(1,19); +const float HK39 = HK13*P(16,19) + HK18*P(17,19) - HK19*P(18,19) + HK20*P(1,19) + HK21*P(0,19) - HK22*P(2,19) + HK24*P(3,19) + P(19,19); +const float HK40 = 1.0F/(HK13*HK27 + HK18*HK32 - HK19*HK28 + HK20*HK38 + HK21*HK26 - HK22*HK31 + HK24*HK35 + HK39 + R_MAG); +const float HK41 = 2*HK4; +const float HK42 = HK14 - q1*q2; +const float HK43 = -HK7; +const float HK44 = HK12 + HK43 + HK8; +const float HK45 = q0*q1; +const float HK46 = HK45 + q2*q3; +const float HK47 = 2*HK46; +const float HK48 = 2*HK42; +const float HK49 = HK22*P(0,1); +const float HK50 = HK20*P(0,2) + HK24*P(0,0) - HK34 + HK44*P(0,17) + HK47*P(0,18) - HK48*P(0,16) + HK49 + P(0,20); +const float HK51 = HK20*P(2,17) - HK21*P(3,17) + HK22*P(1,17) + HK24*P(0,17) + HK44*P(17,17) + HK47*P(17,18) - HK48*P(16,17) + P(17,20); +const float HK52 = HK20*P(2,16) - HK21*P(3,16) + HK22*P(1,16) + HK24*P(0,16) + HK44*P(16,17) + HK47*P(16,18) - HK48*P(16,16) + P(16,20); +const float HK53 = HK20*P(2,3); +const float HK54 = -HK21*P(3,3) + HK22*P(1,3) + HK25 + HK44*P(3,17) + HK47*P(3,18) - HK48*P(3,16) + HK53 + P(3,20); +const float HK55 = HK20*P(2,18) - HK21*P(3,18) + HK22*P(1,18) + HK24*P(0,18) + HK44*P(17,18) + HK47*P(18,18) - HK48*P(16,18) + P(18,20); +const float HK56 = HK24*P(0,1); +const float HK57 = -HK21*P(1,3) + HK22*P(1,1) + HK29 + HK44*P(1,17) + HK47*P(1,18) - HK48*P(1,16) + HK56 + P(1,20); +const float HK58 = HK21*P(2,3); +const float HK59 = HK20*P(2,2) + HK24*P(0,2) + HK36 + HK44*P(2,17) + HK47*P(2,18) - HK48*P(2,16) - HK58 + P(2,20); +const float HK60 = HK20*P(2,20) - HK21*P(3,20) + HK22*P(1,20) + HK24*P(0,20) + HK44*P(17,20) + HK47*P(18,20) - HK48*P(16,20) + P(20,20); +const float HK61 = 1.0F/(HK20*HK59 - HK21*HK54 + HK22*HK57 + HK24*HK50 + HK44*HK51 + HK47*HK55 - HK48*HK52 + HK60 + R_MAG); +const float HK62 = HK16 + q1*q3; +const float HK63 = HK45 - q2*q3; +const float HK64 = HK10 + HK11 + HK43 + HK9; +const float HK65 = 2*HK62; +const float HK66 = 2*HK63; +const float HK67 = HK20*P(0,3) + HK22*P(0,0) + HK30 - HK56 + HK64*P(0,18) + HK65*P(0,16) - HK66*P(0,17) + P(0,21); +const float HK68 = HK20*P(3,18) + HK21*P(2,18) + HK22*P(0,18) - HK24*P(1,18) + HK64*P(18,18) + HK65*P(16,18) - HK66*P(17,18) + P(18,21); +const float HK69 = HK20*P(3,17) + HK21*P(2,17) + HK22*P(0,17) - HK24*P(1,17) + HK64*P(17,18) + HK65*P(16,17) - HK66*P(17,17) + P(17,21); +const float HK70 = HK21*P(1,2) - HK24*P(1,1) + HK33 + HK49 + HK64*P(1,18) + HK65*P(1,16) - HK66*P(1,17) + P(1,21); +const float HK71 = HK20*P(3,16) + HK21*P(2,16) + HK22*P(0,16) - HK24*P(1,16) + HK64*P(16,18) + HK65*P(16,16) - HK66*P(16,17) + P(16,21); +const float HK72 = HK20*P(3,3) + HK22*P(0,3) - HK37 + HK58 + HK64*P(3,18) + HK65*P(3,16) - HK66*P(3,17) + P(3,21); +const float HK73 = HK21*P(2,2) + HK23 - HK24*P(1,2) + HK53 + HK64*P(2,18) + HK65*P(2,16) - HK66*P(2,17) + P(2,21); +const float HK74 = HK20*P(3,21) + HK21*P(2,21) + HK22*P(0,21) - HK24*P(1,21) + HK64*P(18,21) + HK65*P(16,21) - HK66*P(17,21) + P(21,21); +const float HK75 = 1.0F/(HK20*HK72 + HK21*HK73 + HK22*HK67 - HK24*HK70 + HK64*HK68 + HK65*HK71 - HK66*HK69 + HK74 + R_MAG); // Observation Jacobians - axis 0 -Hfusion.at<0>() = -2*HK1; -Hfusion.at<1>() = 2*HK4; -Hfusion.at<2>() = -2*HK8; -Hfusion.at<3>() = 2*HK12; -Hfusion.at<16>() = -HK15; -Hfusion.at<17>() = 2*HK17; -Hfusion.at<18>() = -2*HK19; +Hfusion.at<0>() = HK1; +Hfusion.at<1>() = HK3; +Hfusion.at<2>() = -2*HK4; +Hfusion.at<3>() = HK6; +Hfusion.at<16>() = HK13; +Hfusion.at<17>() = 2*HK15; +Hfusion.at<18>() = -2*HK17; Hfusion.at<19>() = 1; // Kalman gains - axis 0 -Kfusion(0) = HK23*HK35; -Kfusion(1) = HK26*HK35; -Kfusion(2) = HK33*HK35; -Kfusion(3) = HK30*HK35; -Kfusion(4) = HK35*(HK15*P(4,16) + HK20*P(0,4) + HK21*P(4,18) + HK22*P(2,4) - HK27*P(1,4) - HK29*P(4,17) - HK31*P(3,4) - P(4,19)); -Kfusion(5) = HK35*(HK15*P(5,16) + HK20*P(0,5) + HK21*P(5,18) + HK22*P(2,5) - HK27*P(1,5) - HK29*P(5,17) - HK31*P(3,5) - P(5,19)); -Kfusion(6) = HK35*(HK15*P(6,16) + HK20*P(0,6) + HK21*P(6,18) + HK22*P(2,6) - HK27*P(1,6) - HK29*P(6,17) - HK31*P(3,6) - P(6,19)); -Kfusion(7) = HK35*(HK15*P(7,16) + HK20*P(0,7) + HK21*P(7,18) + HK22*P(2,7) - HK27*P(1,7) - HK29*P(7,17) - HK31*P(3,7) - P(7,19)); -Kfusion(8) = HK35*(HK15*P(8,16) + HK20*P(0,8) + HK21*P(8,18) + HK22*P(2,8) - HK27*P(1,8) - HK29*P(8,17) - HK31*P(3,8) - P(8,19)); -Kfusion(9) = HK35*(HK15*P(9,16) + HK20*P(0,9) + HK21*P(9,18) + HK22*P(2,9) - HK27*P(1,9) - HK29*P(9,17) - HK31*P(3,9) - P(9,19)); -Kfusion(10) = HK35*(HK15*P(10,16) + HK20*P(0,10) + HK21*P(10,18) + HK22*P(2,10) - HK27*P(1,10) - HK29*P(10,17) - HK31*P(3,10) - P(10,19)); -Kfusion(11) = HK35*(HK15*P(11,16) + HK20*P(0,11) + HK21*P(11,18) + HK22*P(2,11) - HK27*P(1,11) - HK29*P(11,17) - HK31*P(3,11) - P(11,19)); -Kfusion(12) = HK35*(HK15*P(12,16) + HK20*P(0,12) + HK21*P(12,18) + HK22*P(2,12) - HK27*P(1,12) - HK29*P(12,17) - HK31*P(3,12) - P(12,19)); -Kfusion(13) = HK35*(HK15*P(13,16) + HK20*P(0,13) + HK21*P(13,18) + HK22*P(2,13) - HK27*P(1,13) - HK29*P(13,17) - HK31*P(3,13) - P(13,19)); -Kfusion(14) = HK35*(HK15*P(14,16) + HK20*P(0,14) + HK21*P(14,18) + HK22*P(2,14) - HK27*P(1,14) - HK29*P(14,17) - HK31*P(3,14) - P(14,19)); -Kfusion(15) = HK35*(HK15*P(15,16) + HK20*P(0,15) + HK21*P(15,18) + HK22*P(2,15) - HK27*P(1,15) - HK29*P(15,17) - HK31*P(3,15) - P(15,19)); -Kfusion(16) = HK25*HK35; -Kfusion(17) = HK28*HK35; -Kfusion(18) = HK32*HK35; -Kfusion(19) = HK34*HK35; -Kfusion(20) = HK35*(HK15*P(16,20) + HK20*P(0,20) + HK21*P(18,20) + HK22*P(2,20) - HK27*P(1,20) - HK29*P(17,20) - HK31*P(3,20) - P(19,20)); -Kfusion(21) = HK35*(HK15*P(16,21) + HK20*P(0,21) + HK21*P(18,21) + HK22*P(2,21) - HK27*P(1,21) - HK29*P(17,21) - HK31*P(3,21) + HK36); -Kfusion(22) = HK35*(HK15*P(16,22) + HK20*P(0,22) + HK21*P(18,22) + HK22*P(2,22) - HK27*P(1,22) - HK29*P(17,22) - HK31*P(3,22) - P(19,22)); -Kfusion(23) = HK35*(HK15*P(16,23) + HK20*P(0,23) + HK21*P(18,23) + HK22*P(2,23) - HK27*P(1,23) - HK29*P(17,23) - HK31*P(3,23) - P(19,23)); +Kfusion(0) = HK26*HK40; +Kfusion(1) = HK38*HK40; +Kfusion(2) = HK31*HK40; +Kfusion(3) = HK35*HK40; +Kfusion(4) = HK40*(HK13*P(4,16) + HK18*P(4,17) - HK19*P(4,18) + HK20*P(1,4) + HK21*P(0,4) - HK22*P(2,4) + HK24*P(3,4) + P(4,19)); +Kfusion(5) = HK40*(HK13*P(5,16) + HK18*P(5,17) - HK19*P(5,18) + HK20*P(1,5) + HK21*P(0,5) - HK22*P(2,5) + HK24*P(3,5) + P(5,19)); +Kfusion(6) = HK40*(HK13*P(6,16) + HK18*P(6,17) - HK19*P(6,18) + HK20*P(1,6) + HK21*P(0,6) - HK22*P(2,6) + HK24*P(3,6) + P(6,19)); +Kfusion(7) = HK40*(HK13*P(7,16) + HK18*P(7,17) - HK19*P(7,18) + HK20*P(1,7) + HK21*P(0,7) - HK22*P(2,7) + HK24*P(3,7) + P(7,19)); +Kfusion(8) = HK40*(HK13*P(8,16) + HK18*P(8,17) - HK19*P(8,18) + HK20*P(1,8) + HK21*P(0,8) - HK22*P(2,8) + HK24*P(3,8) + P(8,19)); +Kfusion(9) = HK40*(HK13*P(9,16) + HK18*P(9,17) - HK19*P(9,18) + HK20*P(1,9) + HK21*P(0,9) - HK22*P(2,9) + HK24*P(3,9) + P(9,19)); +Kfusion(10) = HK40*(HK13*P(10,16) + HK18*P(10,17) - HK19*P(10,18) + HK20*P(1,10) + HK21*P(0,10) - HK22*P(2,10) + HK24*P(3,10) + P(10,19)); +Kfusion(11) = HK40*(HK13*P(11,16) + HK18*P(11,17) - HK19*P(11,18) + HK20*P(1,11) + HK21*P(0,11) - HK22*P(2,11) + HK24*P(3,11) + P(11,19)); +Kfusion(12) = HK40*(HK13*P(12,16) + HK18*P(12,17) - HK19*P(12,18) + HK20*P(1,12) + HK21*P(0,12) - HK22*P(2,12) + HK24*P(3,12) + P(12,19)); +Kfusion(13) = HK40*(HK13*P(13,16) + HK18*P(13,17) - HK19*P(13,18) + HK20*P(1,13) + HK21*P(0,13) - HK22*P(2,13) + HK24*P(3,13) + P(13,19)); +Kfusion(14) = HK40*(HK13*P(14,16) + HK18*P(14,17) - HK19*P(14,18) + HK20*P(1,14) + HK21*P(0,14) - HK22*P(2,14) + HK24*P(3,14) + P(14,19)); +Kfusion(15) = HK40*(HK13*P(15,16) + HK18*P(15,17) - HK19*P(15,18) + HK20*P(1,15) + HK21*P(0,15) - HK22*P(2,15) + HK24*P(3,15) + P(15,19)); +Kfusion(16) = HK27*HK40; +Kfusion(17) = HK32*HK40; +Kfusion(18) = HK28*HK40; +Kfusion(19) = HK39*HK40; +Kfusion(20) = HK40*(HK13*P(16,20) + HK18*P(17,20) - HK19*P(18,20) + HK20*P(1,20) + HK21*P(0,20) - HK22*P(2,20) + HK24*P(3,20) + P(19,20)); +Kfusion(21) = HK40*(HK13*P(16,21) + HK18*P(17,21) - HK19*P(18,21) + HK20*P(1,21) + HK21*P(0,21) - HK22*P(2,21) + HK24*P(3,21) + P(19,21)); +Kfusion(22) = HK40*(HK13*P(16,22) + HK18*P(17,22) - HK19*P(18,22) + HK20*P(1,22) + HK21*P(0,22) - HK22*P(2,22) + HK24*P(3,22) + P(19,22)); +Kfusion(23) = HK40*(HK13*P(16,23) + HK18*P(17,23) - HK19*P(18,23) + HK20*P(1,23) + HK21*P(0,23) - HK22*P(2,23) + HK24*P(3,23) + P(19,23)); // Observation Jacobians - axis 1 -Hfusion.at<0>() = 2*HK38; -Hfusion.at<1>() = 2*HK39; -Hfusion.at<2>() = 2*HK41; -Hfusion.at<3>() = -2*HK44; -Hfusion.at<16>() = -2*HK45; -Hfusion.at<17>() = -HK47; -Hfusion.at<18>() = 2*HK49; +Hfusion.at<0>() = HK6; +Hfusion.at<1>() = HK41; +Hfusion.at<2>() = HK3; +Hfusion.at<3>() = -2*HK0; +Hfusion.at<16>() = -2*HK42; +Hfusion.at<17>() = HK44; +Hfusion.at<18>() = 2*HK46; Hfusion.at<20>() = 1; // Kalman gains - axis 1 -Kfusion(0) = HK56*HK64; -Kfusion(1) = HK62*HK64; -Kfusion(2) = HK60*HK64; -Kfusion(3) = HK59*HK64; -Kfusion(4) = HK64*(-HK47*P(4,17) + HK50*P(4,18) + HK51*P(2,4) + HK52*P(0,4) - HK53*P(4,16) + HK54*P(1,4) - HK55*P(3,4) + P(4,20)); -Kfusion(5) = HK64*(-HK47*P(5,17) + HK50*P(5,18) + HK51*P(2,5) + HK52*P(0,5) - HK53*P(5,16) + HK54*P(1,5) - HK55*P(3,5) + P(5,20)); -Kfusion(6) = HK64*(-HK47*P(6,17) + HK50*P(6,18) + HK51*P(2,6) + HK52*P(0,6) - HK53*P(6,16) + HK54*P(1,6) - HK55*P(3,6) + P(6,20)); -Kfusion(7) = HK64*(-HK47*P(7,17) + HK50*P(7,18) + HK51*P(2,7) + HK52*P(0,7) - HK53*P(7,16) + HK54*P(1,7) - HK55*P(3,7) + P(7,20)); -Kfusion(8) = HK64*(-HK47*P(8,17) + HK50*P(8,18) + HK51*P(2,8) + HK52*P(0,8) - HK53*P(8,16) + HK54*P(1,8) - HK55*P(3,8) + P(8,20)); -Kfusion(9) = HK64*(-HK47*P(9,17) + HK50*P(9,18) + HK51*P(2,9) + HK52*P(0,9) - HK53*P(9,16) + HK54*P(1,9) - HK55*P(3,9) + P(9,20)); -Kfusion(10) = HK64*(-HK47*P(10,17) + HK50*P(10,18) + HK51*P(2,10) + HK52*P(0,10) - HK53*P(10,16) + HK54*P(1,10) - HK55*P(3,10) + P(10,20)); -Kfusion(11) = HK64*(-HK47*P(11,17) + HK50*P(11,18) + HK51*P(2,11) + HK52*P(0,11) - HK53*P(11,16) + HK54*P(1,11) - HK55*P(3,11) + P(11,20)); -Kfusion(12) = HK64*(-HK47*P(12,17) + HK50*P(12,18) + HK51*P(2,12) + HK52*P(0,12) - HK53*P(12,16) + HK54*P(1,12) - HK55*P(3,12) + P(12,20)); -Kfusion(13) = HK64*(-HK47*P(13,17) + HK50*P(13,18) + HK51*P(2,13) + HK52*P(0,13) - HK53*P(13,16) + HK54*P(1,13) - HK55*P(3,13) + P(13,20)); -Kfusion(14) = HK64*(-HK47*P(14,17) + HK50*P(14,18) + HK51*P(2,14) + HK52*P(0,14) - HK53*P(14,16) + HK54*P(1,14) - HK55*P(3,14) + P(14,20)); -Kfusion(15) = HK64*(-HK47*P(15,17) + HK50*P(15,18) + HK51*P(2,15) + HK52*P(0,15) - HK53*P(15,16) + HK54*P(1,15) - HK55*P(3,15) + P(15,20)); -Kfusion(16) = HK58*HK64; -Kfusion(17) = HK57*HK64; -Kfusion(18) = HK61*HK64; -Kfusion(19) = HK64*(-HK47*P(17,19) + HK50*P(18,19) + HK51*P(2,19) + HK52*P(0,19) - HK53*P(16,19) + HK54*P(1,19) - HK55*P(3,19) + P(19,20)); -Kfusion(20) = HK63*HK64; -Kfusion(21) = HK64*(-HK47*P(17,21) + HK50*P(18,21) + HK51*P(2,21) + HK52*P(0,21) - HK53*P(16,21) + HK54*P(1,21) - HK55*P(3,21) + P(20,21)); -Kfusion(22) = HK64*(-HK47*P(17,22) + HK50*P(18,22) + HK51*P(2,22) + HK52*P(0,22) - HK53*P(16,22) + HK54*P(1,22) - HK55*P(3,22) + P(20,22)); -Kfusion(23) = HK64*(-HK47*P(17,23) + HK50*P(18,23) + HK51*P(2,23) + HK52*P(0,23) - HK53*P(16,23) + HK54*P(1,23) - HK55*P(3,23) + P(20,23)); +Kfusion(0) = HK50*HK61; +Kfusion(1) = HK57*HK61; +Kfusion(2) = HK59*HK61; +Kfusion(3) = HK54*HK61; +Kfusion(4) = HK61*(HK20*P(2,4) - HK21*P(3,4) + HK22*P(1,4) + HK24*P(0,4) + HK44*P(4,17) + HK47*P(4,18) - HK48*P(4,16) + P(4,20)); +Kfusion(5) = HK61*(HK20*P(2,5) - HK21*P(3,5) + HK22*P(1,5) + HK24*P(0,5) + HK44*P(5,17) + HK47*P(5,18) - HK48*P(5,16) + P(5,20)); +Kfusion(6) = HK61*(HK20*P(2,6) - HK21*P(3,6) + HK22*P(1,6) + HK24*P(0,6) + HK44*P(6,17) + HK47*P(6,18) - HK48*P(6,16) + P(6,20)); +Kfusion(7) = HK61*(HK20*P(2,7) - HK21*P(3,7) + HK22*P(1,7) + HK24*P(0,7) + HK44*P(7,17) + HK47*P(7,18) - HK48*P(7,16) + P(7,20)); +Kfusion(8) = HK61*(HK20*P(2,8) - HK21*P(3,8) + HK22*P(1,8) + HK24*P(0,8) + HK44*P(8,17) + HK47*P(8,18) - HK48*P(8,16) + P(8,20)); +Kfusion(9) = HK61*(HK20*P(2,9) - HK21*P(3,9) + HK22*P(1,9) + HK24*P(0,9) + HK44*P(9,17) + HK47*P(9,18) - HK48*P(9,16) + P(9,20)); +Kfusion(10) = HK61*(HK20*P(2,10) - HK21*P(3,10) + HK22*P(1,10) + HK24*P(0,10) + HK44*P(10,17) + HK47*P(10,18) - HK48*P(10,16) + P(10,20)); +Kfusion(11) = HK61*(HK20*P(2,11) - HK21*P(3,11) + HK22*P(1,11) + HK24*P(0,11) + HK44*P(11,17) + HK47*P(11,18) - HK48*P(11,16) + P(11,20)); +Kfusion(12) = HK61*(HK20*P(2,12) - HK21*P(3,12) + HK22*P(1,12) + HK24*P(0,12) + HK44*P(12,17) + HK47*P(12,18) - HK48*P(12,16) + P(12,20)); +Kfusion(13) = HK61*(HK20*P(2,13) - HK21*P(3,13) + HK22*P(1,13) + HK24*P(0,13) + HK44*P(13,17) + HK47*P(13,18) - HK48*P(13,16) + P(13,20)); +Kfusion(14) = HK61*(HK20*P(2,14) - HK21*P(3,14) + HK22*P(1,14) + HK24*P(0,14) + HK44*P(14,17) + HK47*P(14,18) - HK48*P(14,16) + P(14,20)); +Kfusion(15) = HK61*(HK20*P(2,15) - HK21*P(3,15) + HK22*P(1,15) + HK24*P(0,15) + HK44*P(15,17) + HK47*P(15,18) - HK48*P(15,16) + P(15,20)); +Kfusion(16) = HK52*HK61; +Kfusion(17) = HK51*HK61; +Kfusion(18) = HK55*HK61; +Kfusion(19) = HK61*(HK20*P(2,19) - HK21*P(3,19) + HK22*P(1,19) + HK24*P(0,19) + HK44*P(17,19) + HK47*P(18,19) - HK48*P(16,19) + P(19,20)); +Kfusion(20) = HK60*HK61; +Kfusion(21) = HK61*(HK20*P(2,21) - HK21*P(3,21) + HK22*P(1,21) + HK24*P(0,21) + HK44*P(17,21) + HK47*P(18,21) - HK48*P(16,21) + P(20,21)); +Kfusion(22) = HK61*(HK20*P(2,22) - HK21*P(3,22) + HK22*P(1,22) + HK24*P(0,22) + HK44*P(17,22) + HK47*P(18,22) - HK48*P(16,22) + P(20,22)); +Kfusion(23) = HK61*(HK20*P(2,23) - HK21*P(3,23) + HK22*P(1,23) + HK24*P(0,23) + HK44*P(17,23) + HK47*P(18,23) - HK48*P(16,23) + P(20,23)); // Observation Jacobians - axis 2 -Hfusion.at<0>() = -2*HK65; -Hfusion.at<1>() = -2*HK66; -Hfusion.at<2>() = 2*HK67; -Hfusion.at<3>() = 2*HK68; -Hfusion.at<16>() = 2*HK69; -Hfusion.at<17>() = -2*HK70; -Hfusion.at<18>() = -HK71; +Hfusion.at<0>() = HK41; +Hfusion.at<1>() = -2*HK5; +Hfusion.at<2>() = HK1; +Hfusion.at<3>() = HK3; +Hfusion.at<16>() = 2*HK62; +Hfusion.at<17>() = -2*HK63; +Hfusion.at<18>() = HK64; Hfusion.at<21>() = 1; // Kalman gains - axis 2 -Kfusion(0) = HK75*HK86; -Kfusion(1) = HK84*HK86; -Kfusion(2) = HK81*HK86; -Kfusion(3) = HK77*HK86; -Kfusion(4) = HK86*(HK71*P(4,18) + HK72*P(0,4) + HK73*P(4,17) + HK74*P(1,4) - HK78*P(3,4) - HK80*P(4,16) - HK82*P(2,4) - P(4,21)); -Kfusion(5) = HK86*(HK71*P(5,18) + HK72*P(0,5) + HK73*P(5,17) + HK74*P(1,5) - HK78*P(3,5) - HK80*P(5,16) - HK82*P(2,5) - P(5,21)); -Kfusion(6) = HK86*(HK71*P(6,18) + HK72*P(0,6) + HK73*P(6,17) + HK74*P(1,6) - HK78*P(3,6) - HK80*P(6,16) - HK82*P(2,6) - P(6,21)); -Kfusion(7) = HK86*(HK71*P(7,18) + HK72*P(0,7) + HK73*P(7,17) + HK74*P(1,7) - HK78*P(3,7) - HK80*P(7,16) - HK82*P(2,7) - P(7,21)); -Kfusion(8) = HK86*(HK71*P(8,18) + HK72*P(0,8) + HK73*P(8,17) + HK74*P(1,8) - HK78*P(3,8) - HK80*P(8,16) - HK82*P(2,8) - P(8,21)); -Kfusion(9) = HK86*(HK71*P(9,18) + HK72*P(0,9) + HK73*P(9,17) + HK74*P(1,9) - HK78*P(3,9) - HK80*P(9,16) - HK82*P(2,9) - P(9,21)); -Kfusion(10) = HK86*(HK71*P(10,18) + HK72*P(0,10) + HK73*P(10,17) + HK74*P(1,10) - HK78*P(3,10) - HK80*P(10,16) - HK82*P(2,10) - P(10,21)); -Kfusion(11) = HK86*(HK71*P(11,18) + HK72*P(0,11) + HK73*P(11,17) + HK74*P(1,11) - HK78*P(3,11) - HK80*P(11,16) - HK82*P(2,11) - P(11,21)); -Kfusion(12) = HK86*(HK71*P(12,18) + HK72*P(0,12) + HK73*P(12,17) + HK74*P(1,12) - HK78*P(3,12) - HK80*P(12,16) - HK82*P(2,12) - P(12,21)); -Kfusion(13) = HK86*(HK71*P(13,18) + HK72*P(0,13) + HK73*P(13,17) + HK74*P(1,13) - HK78*P(3,13) - HK80*P(13,16) - HK82*P(2,13) - P(13,21)); -Kfusion(14) = HK86*(HK71*P(14,18) + HK72*P(0,14) + HK73*P(14,17) + HK74*P(1,14) - HK78*P(3,14) - HK80*P(14,16) - HK82*P(2,14) - P(14,21)); -Kfusion(15) = HK86*(HK71*P(15,18) + HK72*P(0,15) + HK73*P(15,17) + HK74*P(1,15) - HK78*P(3,15) - HK80*P(15,16) - HK82*P(2,15) - P(15,21)); -Kfusion(16) = HK79*HK86; -Kfusion(17) = HK83*HK86; -Kfusion(18) = HK76*HK86; -Kfusion(19) = HK86*(HK36 + HK71*P(18,19) + HK72*P(0,19) + HK73*P(17,19) + HK74*P(1,19) - HK78*P(3,19) - HK80*P(16,19) - HK82*P(2,19)); -Kfusion(20) = HK86*(HK71*P(18,20) + HK72*P(0,20) + HK73*P(17,20) + HK74*P(1,20) - HK78*P(3,20) - HK80*P(16,20) - HK82*P(2,20) - P(20,21)); -Kfusion(21) = HK85*HK86; -Kfusion(22) = HK86*(HK71*P(18,22) + HK72*P(0,22) + HK73*P(17,22) + HK74*P(1,22) - HK78*P(3,22) - HK80*P(16,22) - HK82*P(2,22) - P(21,22)); -Kfusion(23) = HK86*(HK71*P(18,23) + HK72*P(0,23) + HK73*P(17,23) + HK74*P(1,23) - HK78*P(3,23) - HK80*P(16,23) - HK82*P(2,23) - P(21,23)); +Kfusion(0) = HK67*HK75; +Kfusion(1) = HK70*HK75; +Kfusion(2) = HK73*HK75; +Kfusion(3) = HK72*HK75; +Kfusion(4) = HK75*(HK20*P(3,4) + HK21*P(2,4) + HK22*P(0,4) - HK24*P(1,4) + HK64*P(4,18) + HK65*P(4,16) - HK66*P(4,17) + P(4,21)); +Kfusion(5) = HK75*(HK20*P(3,5) + HK21*P(2,5) + HK22*P(0,5) - HK24*P(1,5) + HK64*P(5,18) + HK65*P(5,16) - HK66*P(5,17) + P(5,21)); +Kfusion(6) = HK75*(HK20*P(3,6) + HK21*P(2,6) + HK22*P(0,6) - HK24*P(1,6) + HK64*P(6,18) + HK65*P(6,16) - HK66*P(6,17) + P(6,21)); +Kfusion(7) = HK75*(HK20*P(3,7) + HK21*P(2,7) + HK22*P(0,7) - HK24*P(1,7) + HK64*P(7,18) + HK65*P(7,16) - HK66*P(7,17) + P(7,21)); +Kfusion(8) = HK75*(HK20*P(3,8) + HK21*P(2,8) + HK22*P(0,8) - HK24*P(1,8) + HK64*P(8,18) + HK65*P(8,16) - HK66*P(8,17) + P(8,21)); +Kfusion(9) = HK75*(HK20*P(3,9) + HK21*P(2,9) + HK22*P(0,9) - HK24*P(1,9) + HK64*P(9,18) + HK65*P(9,16) - HK66*P(9,17) + P(9,21)); +Kfusion(10) = HK75*(HK20*P(3,10) + HK21*P(2,10) + HK22*P(0,10) - HK24*P(1,10) + HK64*P(10,18) + HK65*P(10,16) - HK66*P(10,17) + P(10,21)); +Kfusion(11) = HK75*(HK20*P(3,11) + HK21*P(2,11) + HK22*P(0,11) - HK24*P(1,11) + HK64*P(11,18) + HK65*P(11,16) - HK66*P(11,17) + P(11,21)); +Kfusion(12) = HK75*(HK20*P(3,12) + HK21*P(2,12) + HK22*P(0,12) - HK24*P(1,12) + HK64*P(12,18) + HK65*P(12,16) - HK66*P(12,17) + P(12,21)); +Kfusion(13) = HK75*(HK20*P(3,13) + HK21*P(2,13) + HK22*P(0,13) - HK24*P(1,13) + HK64*P(13,18) + HK65*P(13,16) - HK66*P(13,17) + P(13,21)); +Kfusion(14) = HK75*(HK20*P(3,14) + HK21*P(2,14) + HK22*P(0,14) - HK24*P(1,14) + HK64*P(14,18) + HK65*P(14,16) - HK66*P(14,17) + P(14,21)); +Kfusion(15) = HK75*(HK20*P(3,15) + HK21*P(2,15) + HK22*P(0,15) - HK24*P(1,15) + HK64*P(15,18) + HK65*P(15,16) - HK66*P(15,17) + P(15,21)); +Kfusion(16) = HK71*HK75; +Kfusion(17) = HK69*HK75; +Kfusion(18) = HK68*HK75; +Kfusion(19) = HK75*(HK20*P(3,19) + HK21*P(2,19) + HK22*P(0,19) - HK24*P(1,19) + HK64*P(18,19) + HK65*P(16,19) - HK66*P(17,19) + P(19,21)); +Kfusion(20) = HK75*(HK20*P(3,20) + HK21*P(2,20) + HK22*P(0,20) - HK24*P(1,20) + HK64*P(18,20) + HK65*P(16,20) - HK66*P(17,20) + P(20,21)); +Kfusion(21) = HK74*HK75; +Kfusion(22) = HK75*(HK20*P(3,22) + HK21*P(2,22) + HK22*P(0,22) - HK24*P(1,22) + HK64*P(18,22) + HK65*P(16,22) - HK66*P(17,22) + P(21,22)); +Kfusion(23) = HK75*(HK20*P(3,23) + HK21*P(2,23) + HK22*P(0,23) - HK24*P(1,23) + HK64*P(18,23) + HK65*P(16,23) - HK66*P(17,23) + P(21,23)); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_innov_var_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_innov_var_generated.cpp index fc5f5fc12f..a02d0af8c7 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_innov_var_generated.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_innov_var_generated.cpp @@ -2,34 +2,28 @@ 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*(q1)*(q1) - 1; -const float IV13 = IV12 + 2*(q3)*(q3); -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 = q0*q2 + q1*q3; -const float IV19 = IV3 + magE*q2; -const float IV20 = 2*IV10 - 2*IV9; -const float IV21 = 2*IV0 - 2*IV1; -const float IV22 = IV12 + 2*(q2)*(q2); -const float IV23 = IV14 - 2*IV15 + IV16; -const float IV24 = 4*IV5 + 2*IV6 + 2*magE*q0; +const float IV3 = 2*q0*q3 - 2*q1*q2; +const float IV4 = 2*magD*q3 + 2*magE*q2 + 2*magN*q1; +const float IV5 = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; +const float IV6 = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; +const float IV7 = -2*magD*q2 + 2*magE*q3 + 2*magN*q0; +const float IV8 = (q2)*(q2); +const float IV9 = (q3)*(q3); +const float IV10 = (q0)*(q0) - (q1)*(q1); +const float IV11 = IV10 + IV8 - IV9; +const float IV12 = IV7*P(2,3); +const float IV13 = IV5*P(0,1); +const float IV14 = IV6*P(0,1); +const float IV15 = IV4*P(2,3); +const float IV16 = 2*q0*q2 + 2*q1*q3; +const float IV17 = 2*IV0 - 2*IV1; +const float IV18 = IV10 - IV8 + IV9; // 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>() = 2*IV18*P(16,21) + 2*IV18*(2*IV18*P(16,16) + 2*IV19*P(3,16) - IV20*P(0,16) - IV21*P(16,17) - IV22*P(16,18) + 2*IV23*P(2,16) - IV24*P(1,16) + P(16,21)) + 2*IV19*P(3,21) + 2*IV19*(2*IV18*P(3,16) + 2*IV19*P(3,3) - IV20*P(0,3) - IV21*P(3,17) - IV22*P(3,18) + 2*IV23*P(2,3) - IV24*P(1,3) + P(3,21)) - IV20*P(0,21) - IV20*(2*IV18*P(0,16) + 2*IV19*P(0,3) - IV20*P(0,0) - IV21*P(0,17) - IV22*P(0,18) + 2*IV23*P(0,2) - IV24*P(0,1) + P(0,21)) - IV21*P(17,21) - IV21*(2*IV18*P(16,17) + 2*IV19*P(3,17) - IV20*P(0,17) - IV21*P(17,17) - IV22*P(17,18) + 2*IV23*P(2,17) - IV24*P(1,17) + P(17,21)) - IV22*P(18,21) - IV22*(2*IV18*P(16,18) + 2*IV19*P(3,18) - IV20*P(0,18) - IV21*P(17,18) - IV22*P(18,18) + 2*IV23*P(2,18) - IV24*P(1,18) + P(18,21)) + 2*IV23*P(2,21) + 2*IV23*(2*IV18*P(2,16) + 2*IV19*P(2,3) - IV20*P(0,2) - IV21*P(2,17) - IV22*P(2,18) + 2*IV23*P(2,2) - IV24*P(1,2) + P(2,21)) - IV24*P(1,21) - IV24*(2*IV18*P(1,16) + 2*IV19*P(1,3) - IV20*P(0,1) - IV21*P(1,17) - IV22*P(1,18) + 2*IV23*P(1,2) - IV24*P(1,1) + P(1,21)) + P(21,21) + R_MAG; +Hfusion.at<1>() = IV11*P(17,20) + IV11*(IV11*P(17,17) + IV2*P(17,18) - IV3*P(16,17) + IV4*P(2,17) + IV5*P(0,17) + IV6*P(1,17) - IV7*P(3,17) + P(17,20)) + IV2*P(18,20) + IV2*(IV11*P(17,18) + IV2*P(18,18) - IV3*P(16,18) + IV4*P(2,18) + IV5*P(0,18) + IV6*P(1,18) - IV7*P(3,18) + P(18,20)) - IV3*P(16,20) - IV3*(IV11*P(16,17) + IV2*P(16,18) - IV3*P(16,16) + IV4*P(2,16) + IV5*P(0,16) + IV6*P(1,16) - IV7*P(3,16) + P(16,20)) + IV4*P(2,20) + IV4*(IV11*P(2,17) - IV12 + IV2*P(2,18) - IV3*P(2,16) + IV4*P(2,2) + IV5*P(0,2) + IV6*P(1,2) + P(2,20)) + IV5*P(0,20) + IV5*(IV11*P(0,17) + IV14 + IV2*P(0,18) - IV3*P(0,16) + IV4*P(0,2) + IV5*P(0,0) - IV7*P(0,3) + P(0,20)) + IV6*P(1,20) + IV6*(IV11*P(1,17) + IV13 + IV2*P(1,18) - IV3*P(1,16) + IV4*P(1,2) + IV6*P(1,1) - IV7*P(1,3) + P(1,20)) - IV7*P(3,20) - IV7*(IV11*P(3,17) + IV15 + IV2*P(3,18) - IV3*P(3,16) + IV5*P(0,3) + IV6*P(1,3) - IV7*P(3,3) + P(3,20)) + P(20,20) + R_MAG; +Hfusion.at<2>() = IV16*P(16,21) + IV16*(IV16*P(16,16) - IV17*P(16,17) + IV18*P(16,18) + IV4*P(3,16) - IV5*P(1,16) + IV6*P(0,16) + IV7*P(2,16) + P(16,21)) - IV17*P(17,21) - IV17*(IV16*P(16,17) - IV17*P(17,17) + IV18*P(17,18) + IV4*P(3,17) - IV5*P(1,17) + IV6*P(0,17) + IV7*P(2,17) + P(17,21)) + IV18*P(18,21) + IV18*(IV16*P(16,18) - IV17*P(17,18) + IV18*P(18,18) + IV4*P(3,18) - IV5*P(1,18) + IV6*P(0,18) + IV7*P(2,18) + P(18,21)) + IV4*P(3,21) + IV4*(IV12 + IV16*P(3,16) - IV17*P(3,17) + IV18*P(3,18) + IV4*P(3,3) - IV5*P(1,3) + IV6*P(0,3) + P(3,21)) - IV5*P(1,21) - IV5*(IV14 + IV16*P(1,16) - IV17*P(1,17) + IV18*P(1,18) + IV4*P(1,3) - IV5*P(1,1) + IV7*P(1,2) + P(1,21)) + IV6*P(0,21) + IV6*(-IV13 + IV16*P(0,16) - IV17*P(0,17) + IV18*P(0,18) + IV4*P(0,3) + IV6*P(0,0) + IV7*P(0,2) + P(0,21)) + IV7*P(2,21) + IV7*(IV15 + IV16*P(2,16) - IV17*P(2,17) + IV18*P(2,18) - IV5*P(1,2) + IV6*P(0,2) + IV7*P(2,2) + P(2,21)) + P(21,21) + R_MAG; // Kalman gains - axis 0 diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp index 3a6c4aae1d..f2ea0b81bf 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp @@ -1,157 +1,154 @@ // Axis 0 equations // Sub Expressions -const float HK0 = ve - vwe; -const float HK1 = -HK0*q3 + q2*vd; -const float HK2 = 2*Kaccx; -const float HK3 = HK0*q2 + q3*vd; -const float HK4 = 2*vn - 2*vwn; -const float HK5 = -HK0*q1 + HK4*q2 + q0*vd; -const float HK6 = HK0*q0 - HK4*q3 + q1*vd; -const float HK7 = 2*(q2)*(q2) + 2*(q3)*(q3) - 1; +const float HK0 = vn - vwn; +const float HK1 = ve - vwe; +const float HK2 = HK0*q0 + HK1*q3 - q2*vd; +const float HK3 = 2*Kaccx; +const float HK4 = HK0*q1 + HK1*q2 + q3*vd; +const float HK5 = HK0*q2 - HK1*q1 + q0*vd; +const float HK6 = -HK0*q3 + HK1*q0 + q1*vd; +const float HK7 = (q0)*(q0) + (q1)*(q1) - (q2)*(q2) - (q3)*(q3); const float HK8 = HK7*Kaccx; const float HK9 = q0*q3 + q1*q2; -const float HK10 = HK2*HK9; +const float HK10 = HK3*HK9; const float HK11 = q0*q2 - q1*q3; -const float HK12 = 2*HK9; -const float HK13 = HK12*P(0,23); -const float HK14 = 2*HK11; -const float HK15 = 2*HK1; -const float HK16 = 2*HK5; -const float HK17 = HK12*P(23,23); -const float HK18 = HK12*P(5,23); -const float HK19 = -2*HK11; -const float HK20 = 2*HK3; -const float HK21 = -HK7; -const float HK22 = -2*HK1; -const float HK23 = 2*HK6; -const float HK24 = -2*HK5; -const float HK25 = (Kaccx)*(Kaccx); -const float HK26 = HK12*HK25; -const float HK27 = -HK18; -const float HK28 = HK12*P(6,23); -const float HK29 = HK12*P(1,23); -const float HK30 = HK12*P(4,23); -const float HK31 = HK7*P(4,22); -const float HK32 = HK25*HK7; -const float HK33 = HK12*P(22,23); -const float HK34 = HK12*P(3,23); -const float HK35 = HK12*P(2,23); -const float HK36 = Kaccx/(HK14*HK25*(HK12*P(5,6) + HK19*P(6,6) + HK20*P(1,6) + HK21*P(4,6) + HK22*P(0,6) + HK23*P(3,6) + HK24*P(2,6) - HK28 + HK7*P(6,22)) + HK15*HK25*(HK12*P(0,5) - HK13 + HK19*P(0,6) + HK20*P(0,1) + HK21*P(0,4) + HK22*P(0,0) + HK23*P(0,3) + HK24*P(0,2) + HK7*P(0,22)) + HK16*HK25*(HK12*P(2,5) + HK19*P(2,6) + HK20*P(1,2) + HK21*P(2,4) + HK22*P(0,2) + HK23*P(2,3) + HK24*P(2,2) - HK35 + HK7*P(2,22)) - HK20*HK25*(HK12*P(1,5) + HK19*P(1,6) + HK20*P(1,1) + HK21*P(1,4) + HK22*P(0,1) + HK23*P(1,3) + HK24*P(1,2) - HK29 + HK7*P(1,22)) - HK23*HK25*(HK12*P(3,5) + HK19*P(3,6) + HK20*P(1,3) + HK21*P(3,4) + HK22*P(0,3) + HK23*P(3,3) + HK24*P(2,3) - HK34 + HK7*P(3,22)) + HK26*(-HK17 + HK18 + HK19*P(6,23) + HK20*P(1,23) + HK21*P(4,23) + HK22*P(0,23) + HK23*P(3,23) + HK24*P(2,23) + HK7*P(22,23)) - HK26*(HK12*P(5,5) + HK19*P(5,6) + HK20*P(1,5) + HK21*P(4,5) + HK22*P(0,5) + HK23*P(3,5) + HK24*P(2,5) + HK27 + HK7*P(5,22)) + HK32*(HK12*P(4,5) + HK19*P(4,6) + HK20*P(1,4) + HK21*P(4,4) + HK22*P(0,4) + HK23*P(3,4) + HK24*P(2,4) - HK30 + HK31) - HK32*(HK12*P(5,22) + HK19*P(6,22) + HK20*P(1,22) + HK21*P(4,22) + HK22*P(0,22) + HK23*P(3,22) + HK24*P(2,22) - HK33 + HK7*P(22,22)) - R_ACC); +const float HK12 = 2*HK5; +const float HK13 = 2*HK11; +const float HK14 = 2*HK9; +const float HK15 = 2*HK2; +const float HK16 = 2*HK4; +const float HK17 = 2*HK6; +const float HK18 = -HK14*P(0,23) + HK14*P(0,5) + HK15*P(0,0) + HK16*P(0,1) + HK17*P(0,3) + HK7*P(0,4); +const float HK19 = (Kaccx)*(Kaccx); +const float HK20 = -HK7; +const float HK21 = -2*HK5; +const float HK22 = -2*HK11; +const float HK23 = HK14*P(5,23); +const float HK24 = -HK14*P(23,23) + HK15*P(0,23) + HK16*P(1,23) + HK17*P(3,23) + HK23 + HK7*P(4,23); +const float HK25 = HK14*P(5,5) + HK15*P(0,5) + HK16*P(1,5) + HK17*P(3,5) - HK23 + HK7*P(4,5); +const float HK26 = HK14*P(5,6) - HK14*P(6,23) + HK15*P(0,6) + HK16*P(1,6) + HK17*P(3,6) + HK7*P(4,6); +const float HK27 = -HK14*P(4,23) + HK14*P(4,5) + HK15*P(0,4) + HK16*P(1,4) + HK17*P(3,4) + HK7*P(4,4); +const float HK28 = HK7*P(4,22); +const float HK29 = -HK14*P(22,23) + HK14*P(5,22) + HK15*P(0,22) + HK16*P(1,22) + HK17*P(3,22) + HK28; +const float HK30 = -HK14*P(1,23) + HK14*P(1,5) + HK15*P(0,1) + HK16*P(1,1) + HK17*P(1,3) + HK7*P(1,4); +const float HK31 = -HK14*P(2,23) + HK14*P(2,5) + HK15*P(0,2) + HK16*P(1,2) + HK17*P(2,3) + HK7*P(2,4); +const float HK32 = -HK14*P(3,23) + HK14*P(3,5) + HK15*P(0,3) + HK16*P(1,3) + HK17*P(3,3) + HK7*P(3,4); +const float HK33 = Kaccx/(2*HK11*HK19*(HK20*P(6,22) + HK21*P(2,6) + HK22*P(6,6) + HK26) - HK14*HK19*(HK20*P(5,22) + HK21*P(2,5) + HK22*P(5,6) + HK25) - HK15*HK19*(HK18 + HK20*P(0,22) + HK21*P(0,2) + HK22*P(0,6)) - HK16*HK19*(HK20*P(1,22) + HK21*P(1,2) + HK22*P(1,6) + HK30) - HK17*HK19*(HK20*P(3,22) + HK21*P(2,3) + HK22*P(3,6) + HK32) + 2*HK19*HK5*(HK20*P(2,22) + HK21*P(2,2) + HK22*P(2,6) + HK31) + HK19*HK7*(HK20*P(22,22) + HK21*P(2,22) + HK22*P(6,22) + HK29) - HK19*HK7*(HK20*P(4,22) + HK21*P(2,4) + HK22*P(4,6) + HK27) + 2*HK19*HK9*(HK20*P(22,23) + HK21*P(2,23) + HK22*P(6,23) + HK24) - R_ACC); // Observation Jacobians -Hfusion.at<0>() = HK1*HK2; -Hfusion.at<1>() = -HK2*HK3; -Hfusion.at<2>() = HK2*HK5; -Hfusion.at<3>() = -HK2*HK6; +Hfusion.at<0>() = -HK2*HK3; +Hfusion.at<1>() = -HK3*HK4; +Hfusion.at<2>() = HK3*HK5; +Hfusion.at<3>() = -HK3*HK6; +Hfusion.at<4>() = -HK8; +Hfusion.at<5>() = -HK10; +Hfusion.at<6>() = HK11*HK3; +Hfusion.at<22>() = HK8; +Hfusion.at<23>() = HK10; + + +// Kalman gains +Kfusion(0) = HK33*(-HK12*P(0,2) - HK13*P(0,6) + HK18 - HK7*P(0,22)); +Kfusion(1) = HK33*(-HK12*P(1,2) - HK13*P(1,6) + HK30 - HK7*P(1,22)); +Kfusion(2) = HK33*(-HK12*P(2,2) - HK13*P(2,6) + HK31 - HK7*P(2,22)); +Kfusion(3) = HK33*(-HK12*P(2,3) - HK13*P(3,6) + HK32 - HK7*P(3,22)); +Kfusion(4) = HK33*(-HK12*P(2,4) - HK13*P(4,6) + HK27 - HK28); +Kfusion(5) = HK33*(-HK12*P(2,5) - HK13*P(5,6) + HK25 - HK7*P(5,22)); +Kfusion(6) = HK33*(-HK12*P(2,6) - HK13*P(6,6) + HK26 - HK7*P(6,22)); +Kfusion(7) = HK33*(-HK12*P(2,7) - HK13*P(6,7) + HK14*P(5,7) - HK14*P(7,23) + HK15*P(0,7) + HK16*P(1,7) + HK17*P(3,7) + HK7*P(4,7) - HK7*P(7,22)); +Kfusion(8) = HK33*(-HK12*P(2,8) - HK13*P(6,8) + HK14*P(5,8) - HK14*P(8,23) + HK15*P(0,8) + HK16*P(1,8) + HK17*P(3,8) + HK7*P(4,8) - HK7*P(8,22)); +Kfusion(9) = HK33*(-HK12*P(2,9) - HK13*P(6,9) + HK14*P(5,9) - HK14*P(9,23) + HK15*P(0,9) + HK16*P(1,9) + HK17*P(3,9) + HK7*P(4,9) - HK7*P(9,22)); +Kfusion(10) = HK33*(-HK12*P(2,10) - HK13*P(6,10) - HK14*P(10,23) + HK14*P(5,10) + HK15*P(0,10) + HK16*P(1,10) + HK17*P(3,10) - HK7*P(10,22) + HK7*P(4,10)); +Kfusion(11) = HK33*(-HK12*P(2,11) - HK13*P(6,11) - HK14*P(11,23) + HK14*P(5,11) + HK15*P(0,11) + HK16*P(1,11) + HK17*P(3,11) - HK7*P(11,22) + HK7*P(4,11)); +Kfusion(12) = HK33*(-HK12*P(2,12) - HK13*P(6,12) - HK14*P(12,23) + HK14*P(5,12) + HK15*P(0,12) + HK16*P(1,12) + HK17*P(3,12) - HK7*P(12,22) + HK7*P(4,12)); +Kfusion(13) = HK33*(-HK12*P(2,13) - HK13*P(6,13) - HK14*P(13,23) + HK14*P(5,13) + HK15*P(0,13) + HK16*P(1,13) + HK17*P(3,13) - HK7*P(13,22) + HK7*P(4,13)); +Kfusion(14) = HK33*(-HK12*P(2,14) - HK13*P(6,14) - HK14*P(14,23) + HK14*P(5,14) + HK15*P(0,14) + HK16*P(1,14) + HK17*P(3,14) - HK7*P(14,22) + HK7*P(4,14)); +Kfusion(15) = HK33*(-HK12*P(2,15) - HK13*P(6,15) - HK14*P(15,23) + HK14*P(5,15) + HK15*P(0,15) + HK16*P(1,15) + HK17*P(3,15) - HK7*P(15,22) + HK7*P(4,15)); +Kfusion(16) = HK33*(-HK12*P(2,16) - HK13*P(6,16) - HK14*P(16,23) + HK14*P(5,16) + HK15*P(0,16) + HK16*P(1,16) + HK17*P(3,16) - HK7*P(16,22) + HK7*P(4,16)); +Kfusion(17) = HK33*(-HK12*P(2,17) - HK13*P(6,17) - HK14*P(17,23) + HK14*P(5,17) + HK15*P(0,17) + HK16*P(1,17) + HK17*P(3,17) - HK7*P(17,22) + HK7*P(4,17)); +Kfusion(18) = HK33*(-HK12*P(2,18) - HK13*P(6,18) - HK14*P(18,23) + HK14*P(5,18) + HK15*P(0,18) + HK16*P(1,18) + HK17*P(3,18) - HK7*P(18,22) + HK7*P(4,18)); +Kfusion(19) = HK33*(-HK12*P(2,19) - HK13*P(6,19) - HK14*P(19,23) + HK14*P(5,19) + HK15*P(0,19) + HK16*P(1,19) + HK17*P(3,19) - HK7*P(19,22) + HK7*P(4,19)); +Kfusion(20) = HK33*(-HK12*P(2,20) - HK13*P(6,20) - HK14*P(20,23) + HK14*P(5,20) + HK15*P(0,20) + HK16*P(1,20) + HK17*P(3,20) - HK7*P(20,22) + HK7*P(4,20)); +Kfusion(21) = HK33*(-HK12*P(2,21) - HK13*P(6,21) - HK14*P(21,23) + HK14*P(5,21) + HK15*P(0,21) + HK16*P(1,21) + HK17*P(3,21) - HK7*P(21,22) + HK7*P(4,21)); +Kfusion(22) = HK33*(-HK12*P(2,22) - HK13*P(6,22) + HK29 - HK7*P(22,22)); +Kfusion(23) = HK33*(-HK12*P(2,23) - HK13*P(6,23) + HK24 - HK7*P(22,23)); + + +// Axis 1 equations +// Sub Expressions +const float HK0 = ve - vwe; +const float HK1 = vn - vwn; +const float HK2 = HK0*q0 - HK1*q3 + q1*vd; +const float HK3 = 2*Kaccy; +const float HK4 = -HK0*q1 + HK1*q2 + q0*vd; +const float HK5 = HK0*q2 + HK1*q1 + q3*vd; +const float HK6 = HK0*q3 + HK1*q0 - q2*vd; +const float HK7 = q0*q3 - q1*q2; +const float HK8 = HK3*HK7; +const float HK9 = (q0)*(q0) - (q1)*(q1) + (q2)*(q2) - (q3)*(q3); +const float HK10 = HK9*Kaccy; +const float HK11 = q0*q1 + q2*q3; +const float HK12 = 2*HK6; +const float HK13 = 2*HK7; +const float HK14 = 2*HK2; +const float HK15 = 2*HK4; +const float HK16 = 2*HK5; +const float HK17 = 2*HK11; +const float HK18 = HK13*P(0,22) + HK14*P(0,0) + HK15*P(0,1) + HK16*P(0,2) + HK17*P(0,6) + HK9*P(0,5); +const float HK19 = (Kaccy)*(Kaccy); +const float HK20 = -HK9; +const float HK21 = -2*HK6; +const float HK22 = -2*HK7; +const float HK23 = HK13*P(6,22) + HK14*P(0,6) + HK15*P(1,6) + HK16*P(2,6) + HK17*P(6,6) + HK9*P(5,6); +const float HK24 = HK13*P(22,22) + HK14*P(0,22) + HK15*P(1,22) + HK16*P(2,22) + HK17*P(6,22) + HK9*P(5,22); +const float HK25 = HK13*P(4,22); +const float HK26 = HK14*P(0,4) + HK15*P(1,4) + HK16*P(2,4) + HK17*P(4,6) + HK25 + HK9*P(4,5); +const float HK27 = HK13*P(5,22) + HK14*P(0,5) + HK15*P(1,5) + HK16*P(2,5) + HK17*P(5,6) + HK9*P(5,5); +const float HK28 = HK9*P(5,23); +const float HK29 = HK13*P(22,23) + HK14*P(0,23) + HK15*P(1,23) + HK16*P(2,23) + HK17*P(6,23) + HK28; +const float HK30 = HK13*P(2,22) + HK14*P(0,2) + HK15*P(1,2) + HK16*P(2,2) + HK17*P(2,6) + HK9*P(2,5); +const float HK31 = HK13*P(1,22) + HK14*P(0,1) + HK15*P(1,1) + HK16*P(1,2) + HK17*P(1,6) + HK9*P(1,5); +const float HK32 = HK13*P(3,22) + HK14*P(0,3) + HK15*P(1,3) + HK16*P(2,3) + HK17*P(3,6) + HK9*P(3,5); +const float HK33 = Kaccy/(-HK13*HK19*(HK20*P(22,23) + HK21*P(3,22) + HK22*P(4,22) + HK24) - HK14*HK19*(HK18 + HK20*P(0,23) + HK21*P(0,3) + HK22*P(0,4)) - HK15*HK19*(HK20*P(1,23) + HK21*P(1,3) + HK22*P(1,4) + HK31) - HK16*HK19*(HK20*P(2,23) + HK21*P(2,3) + HK22*P(2,4) + HK30) - HK17*HK19*(HK20*P(6,23) + HK21*P(3,6) + HK22*P(4,6) + HK23) + 2*HK19*HK6*(HK20*P(3,23) + HK21*P(3,3) + HK22*P(3,4) + HK32) + 2*HK19*HK7*(HK20*P(4,23) + HK21*P(3,4) + HK22*P(4,4) + HK26) + HK19*HK9*(HK20*P(23,23) + HK21*P(3,23) + HK22*P(4,23) + HK29) - HK19*HK9*(HK20*P(5,23) + HK21*P(3,5) + HK22*P(4,5) + HK27) - R_ACC); + + +// Observation Jacobians +Hfusion.at<0>() = -HK2*HK3; +Hfusion.at<1>() = -HK3*HK4; +Hfusion.at<2>() = -HK3*HK5; +Hfusion.at<3>() = HK3*HK6; Hfusion.at<4>() = HK8; Hfusion.at<5>() = -HK10; -Hfusion.at<6>() = HK11*HK2; +Hfusion.at<6>() = -HK11*HK3; Hfusion.at<22>() = -HK8; Hfusion.at<23>() = HK10; // Kalman gains -Kfusion(0) = HK36*(-HK13 - HK14*P(0,6) - HK15*P(0,0) - HK16*P(0,2) + 2*HK3*P(0,1) + 2*HK6*P(0,3) + HK7*P(0,22) - HK7*P(0,4) + 2*HK9*P(0,5)); -Kfusion(1) = HK36*(-HK14*P(1,6) - HK15*P(0,1) - HK16*P(1,2) - HK29 + 2*HK3*P(1,1) + 2*HK6*P(1,3) + HK7*P(1,22) - HK7*P(1,4) + 2*HK9*P(1,5)); -Kfusion(2) = HK36*(-HK14*P(2,6) - HK15*P(0,2) - HK16*P(2,2) + 2*HK3*P(1,2) - HK35 + 2*HK6*P(2,3) + HK7*P(2,22) - HK7*P(2,4) + 2*HK9*P(2,5)); -Kfusion(3) = HK36*(-HK14*P(3,6) - HK15*P(0,3) - HK16*P(2,3) + 2*HK3*P(1,3) - HK34 + 2*HK6*P(3,3) + HK7*P(3,22) - HK7*P(3,4) + 2*HK9*P(3,5)); -Kfusion(4) = HK36*(-HK14*P(4,6) - HK15*P(0,4) - HK16*P(2,4) + 2*HK3*P(1,4) - HK30 + HK31 + 2*HK6*P(3,4) - HK7*P(4,4) + 2*HK9*P(4,5)); -Kfusion(5) = HK36*(-HK14*P(5,6) - HK15*P(0,5) - HK16*P(2,5) - HK18 + 2*HK3*P(1,5) + 2*HK6*P(3,5) - HK7*P(4,5) + HK7*P(5,22) + 2*HK9*P(5,5)); -Kfusion(6) = HK36*(-HK14*P(6,6) - HK15*P(0,6) - HK16*P(2,6) - HK28 + 2*HK3*P(1,6) + 2*HK6*P(3,6) - HK7*P(4,6) + HK7*P(6,22) + 2*HK9*P(5,6)); -Kfusion(7) = HK36*(-HK12*P(7,23) - HK14*P(6,7) - HK15*P(0,7) - HK16*P(2,7) + 2*HK3*P(1,7) + 2*HK6*P(3,7) - HK7*P(4,7) + HK7*P(7,22) + 2*HK9*P(5,7)); -Kfusion(8) = HK36*(-HK12*P(8,23) - HK14*P(6,8) - HK15*P(0,8) - HK16*P(2,8) + 2*HK3*P(1,8) + 2*HK6*P(3,8) - HK7*P(4,8) + HK7*P(8,22) + 2*HK9*P(5,8)); -Kfusion(9) = HK36*(-HK12*P(9,23) - HK14*P(6,9) - HK15*P(0,9) - HK16*P(2,9) + 2*HK3*P(1,9) + 2*HK6*P(3,9) - HK7*P(4,9) + HK7*P(9,22) + 2*HK9*P(5,9)); -Kfusion(10) = HK36*(-HK12*P(10,23) - HK14*P(6,10) - HK15*P(0,10) - HK16*P(2,10) + 2*HK3*P(1,10) + 2*HK6*P(3,10) + HK7*P(10,22) - HK7*P(4,10) + 2*HK9*P(5,10)); -Kfusion(11) = HK36*(-HK12*P(11,23) - HK14*P(6,11) - HK15*P(0,11) - HK16*P(2,11) + 2*HK3*P(1,11) + 2*HK6*P(3,11) + HK7*P(11,22) - HK7*P(4,11) + 2*HK9*P(5,11)); -Kfusion(12) = HK36*(-HK12*P(12,23) - HK14*P(6,12) - HK15*P(0,12) - HK16*P(2,12) + 2*HK3*P(1,12) + 2*HK6*P(3,12) + HK7*P(12,22) - HK7*P(4,12) + 2*HK9*P(5,12)); -Kfusion(13) = HK36*(-HK12*P(13,23) - HK14*P(6,13) - HK15*P(0,13) - HK16*P(2,13) + 2*HK3*P(1,13) + 2*HK6*P(3,13) + HK7*P(13,22) - HK7*P(4,13) + 2*HK9*P(5,13)); -Kfusion(14) = HK36*(-HK12*P(14,23) - HK14*P(6,14) - HK15*P(0,14) - HK16*P(2,14) + 2*HK3*P(1,14) + 2*HK6*P(3,14) + HK7*P(14,22) - HK7*P(4,14) + 2*HK9*P(5,14)); -Kfusion(15) = HK36*(-HK12*P(15,23) - HK14*P(6,15) - HK15*P(0,15) - HK16*P(2,15) + 2*HK3*P(1,15) + 2*HK6*P(3,15) + HK7*P(15,22) - HK7*P(4,15) + 2*HK9*P(5,15)); -Kfusion(16) = HK36*(-HK12*P(16,23) - HK14*P(6,16) - HK15*P(0,16) - HK16*P(2,16) + 2*HK3*P(1,16) + 2*HK6*P(3,16) + HK7*P(16,22) - HK7*P(4,16) + 2*HK9*P(5,16)); -Kfusion(17) = HK36*(-HK12*P(17,23) - HK14*P(6,17) - HK15*P(0,17) - HK16*P(2,17) + 2*HK3*P(1,17) + 2*HK6*P(3,17) + HK7*P(17,22) - HK7*P(4,17) + 2*HK9*P(5,17)); -Kfusion(18) = HK36*(-HK12*P(18,23) - HK14*P(6,18) - HK15*P(0,18) - HK16*P(2,18) + 2*HK3*P(1,18) + 2*HK6*P(3,18) + HK7*P(18,22) - HK7*P(4,18) + 2*HK9*P(5,18)); -Kfusion(19) = HK36*(-HK12*P(19,23) - HK14*P(6,19) - HK15*P(0,19) - HK16*P(2,19) + 2*HK3*P(1,19) + 2*HK6*P(3,19) + HK7*P(19,22) - HK7*P(4,19) + 2*HK9*P(5,19)); -Kfusion(20) = HK36*(-HK12*P(20,23) - HK14*P(6,20) - HK15*P(0,20) - HK16*P(2,20) + 2*HK3*P(1,20) + 2*HK6*P(3,20) + HK7*P(20,22) - HK7*P(4,20) + 2*HK9*P(5,20)); -Kfusion(21) = HK36*(-HK12*P(21,23) - HK14*P(6,21) - HK15*P(0,21) - HK16*P(2,21) + 2*HK3*P(1,21) + 2*HK6*P(3,21) + HK7*P(21,22) - HK7*P(4,21) + 2*HK9*P(5,21)); -Kfusion(22) = HK36*(-HK14*P(6,22) - HK15*P(0,22) - HK16*P(2,22) + 2*HK3*P(1,22) - HK31 - HK33 + 2*HK6*P(3,22) + HK7*P(22,22) + 2*HK9*P(5,22)); -Kfusion(23) = HK36*(-HK14*P(6,23) - HK15*P(0,23) - HK16*P(2,23) - HK17 - HK27 + 2*HK3*P(1,23) + 2*HK6*P(3,23) + HK7*P(22,23) - HK7*P(4,23)); - - -// 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*(q1)*(q1) + 2*(q3)*(q3) - 1; -const float HK10 = HK9*Kaccy; -const float HK11 = q0*q1 + q2*q3; -const float HK12 = 2*HK6; -const float HK13 = 2*HK7; -const float HK14 = 2*HK1; -const float HK15 = 2*HK4; -const float HK16 = 2*HK5; -const float HK17 = 2*HK11; -const float HK18 = HK13*P(0,22) + HK14*P(0,0) + HK15*P(0,1) + HK16*P(0,2) + HK17*P(0,6) + HK9*P(0,23); -const float HK19 = (Kaccy)*(Kaccy); -const float HK20 = -HK9; -const float HK21 = -2*HK6; -const float HK22 = -2*HK7; -const float HK23 = HK13*P(6,22) + HK14*P(0,6) + HK15*P(1,6) + HK16*P(2,6) + HK17*P(6,6) + HK9*P(6,23); -const float HK24 = HK13*P(22,22) + HK14*P(0,22) + HK15*P(1,22) + HK16*P(2,22) + HK17*P(6,22) + HK9*P(22,23); -const float HK25 = HK13*P(4,22); -const float HK26 = HK14*P(0,4) + HK15*P(1,4) + HK16*P(2,4) + HK17*P(4,6) + HK25 + HK9*P(4,23); -const float HK27 = HK13*P(2,22) + HK14*P(0,2) + HK15*P(1,2) + HK16*P(2,2) + HK17*P(2,6) + HK9*P(2,23); -const float HK28 = HK13*P(22,23) + HK14*P(0,23) + HK15*P(1,23) + HK16*P(2,23) + HK17*P(6,23) + HK9*P(23,23); -const float HK29 = HK9*P(5,23); -const float HK30 = HK13*P(5,22) + HK14*P(0,5) + HK15*P(1,5) + HK16*P(2,5) + HK17*P(5,6) + HK29; -const float HK31 = HK13*P(1,22) + HK14*P(0,1) + HK15*P(1,1) + HK16*P(1,2) + HK17*P(1,6) + HK9*P(1,23); -const float HK32 = HK13*P(3,22) + HK14*P(0,3) + HK15*P(1,3) + HK16*P(2,3) + HK17*P(3,6) + HK9*P(3,23); -const float HK33 = Kaccy/(-HK13*HK19*(HK20*P(5,22) + HK21*P(3,22) + HK22*P(4,22) + HK24) - HK14*HK19*(HK18 + HK20*P(0,5) + HK21*P(0,3) + HK22*P(0,4)) - HK15*HK19*(HK20*P(1,5) + HK21*P(1,3) + HK22*P(1,4) + HK31) - HK16*HK19*(HK20*P(2,5) + HK21*P(2,3) + HK22*P(2,4) + HK27) - HK17*HK19*(HK20*P(5,6) + HK21*P(3,6) + HK22*P(4,6) + HK23) + 2*HK19*HK6*(HK20*P(3,5) + HK21*P(3,3) + HK22*P(3,4) + HK32) + 2*HK19*HK7*(HK20*P(4,5) + HK21*P(3,4) + HK22*P(4,4) + HK26) - HK19*HK9*(HK20*P(5,23) + HK21*P(3,23) + HK22*P(4,23) + HK28) + HK19*HK9*(HK20*P(5,5) + HK21*P(3,5) + HK22*P(4,5) + HK30) - 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<22>() = -HK8; -Hfusion.at<23>() = -HK10; - - -// Kalman gains -Kfusion(0) = HK33*(-HK12*P(0,3) - HK13*P(0,4) + HK18 - HK9*P(0,5)); -Kfusion(1) = HK33*(-HK12*P(1,3) - HK13*P(1,4) + HK31 - HK9*P(1,5)); -Kfusion(2) = HK33*(-HK12*P(2,3) - HK13*P(2,4) + HK27 - HK9*P(2,5)); -Kfusion(3) = HK33*(-HK12*P(3,3) - HK13*P(3,4) + HK32 - HK9*P(3,5)); -Kfusion(4) = HK33*(-HK12*P(3,4) - HK13*P(4,4) + HK26 - HK9*P(4,5)); -Kfusion(5) = HK33*(-HK12*P(3,5) - HK13*P(4,5) + HK30 - HK9*P(5,5)); -Kfusion(6) = HK33*(-HK12*P(3,6) - HK13*P(4,6) + HK23 - HK9*P(5,6)); -Kfusion(7) = HK33*(-HK12*P(3,7) - HK13*P(4,7) + HK13*P(7,22) + HK14*P(0,7) + HK15*P(1,7) + HK16*P(2,7) + HK17*P(6,7) - HK9*P(5,7) + HK9*P(7,23)); -Kfusion(8) = HK33*(-HK12*P(3,8) - HK13*P(4,8) + HK13*P(8,22) + HK14*P(0,8) + HK15*P(1,8) + HK16*P(2,8) + HK17*P(6,8) - HK9*P(5,8) + HK9*P(8,23)); -Kfusion(9) = HK33*(-HK12*P(3,9) - HK13*P(4,9) + HK13*P(9,22) + HK14*P(0,9) + HK15*P(1,9) + HK16*P(2,9) + HK17*P(6,9) - HK9*P(5,9) + HK9*P(9,23)); -Kfusion(10) = HK33*(-HK12*P(3,10) + HK13*P(10,22) - HK13*P(4,10) + HK14*P(0,10) + HK15*P(1,10) + HK16*P(2,10) + HK17*P(6,10) + HK9*P(10,23) - HK9*P(5,10)); -Kfusion(11) = HK33*(-HK12*P(3,11) + HK13*P(11,22) - HK13*P(4,11) + HK14*P(0,11) + HK15*P(1,11) + HK16*P(2,11) + HK17*P(6,11) + HK9*P(11,23) - HK9*P(5,11)); -Kfusion(12) = HK33*(-HK12*P(3,12) + HK13*P(12,22) - HK13*P(4,12) + HK14*P(0,12) + HK15*P(1,12) + HK16*P(2,12) + HK17*P(6,12) + HK9*P(12,23) - HK9*P(5,12)); -Kfusion(13) = HK33*(-HK12*P(3,13) + HK13*P(13,22) - HK13*P(4,13) + HK14*P(0,13) + HK15*P(1,13) + HK16*P(2,13) + HK17*P(6,13) + HK9*P(13,23) - HK9*P(5,13)); -Kfusion(14) = HK33*(-HK12*P(3,14) + HK13*P(14,22) - HK13*P(4,14) + HK14*P(0,14) + HK15*P(1,14) + HK16*P(2,14) + HK17*P(6,14) + HK9*P(14,23) - HK9*P(5,14)); -Kfusion(15) = HK33*(-HK12*P(3,15) + HK13*P(15,22) - HK13*P(4,15) + HK14*P(0,15) + HK15*P(1,15) + HK16*P(2,15) + HK17*P(6,15) + HK9*P(15,23) - HK9*P(5,15)); -Kfusion(16) = HK33*(-HK12*P(3,16) + HK13*P(16,22) - HK13*P(4,16) + HK14*P(0,16) + HK15*P(1,16) + HK16*P(2,16) + HK17*P(6,16) + HK9*P(16,23) - HK9*P(5,16)); -Kfusion(17) = HK33*(-HK12*P(3,17) + HK13*P(17,22) - HK13*P(4,17) + HK14*P(0,17) + HK15*P(1,17) + HK16*P(2,17) + HK17*P(6,17) + HK9*P(17,23) - HK9*P(5,17)); -Kfusion(18) = HK33*(-HK12*P(3,18) + HK13*P(18,22) - HK13*P(4,18) + HK14*P(0,18) + HK15*P(1,18) + HK16*P(2,18) + HK17*P(6,18) + HK9*P(18,23) - HK9*P(5,18)); -Kfusion(19) = HK33*(-HK12*P(3,19) + HK13*P(19,22) - HK13*P(4,19) + HK14*P(0,19) + HK15*P(1,19) + HK16*P(2,19) + HK17*P(6,19) + HK9*P(19,23) - HK9*P(5,19)); -Kfusion(20) = HK33*(-HK12*P(3,20) + HK13*P(20,22) - HK13*P(4,20) + HK14*P(0,20) + HK15*P(1,20) + HK16*P(2,20) + HK17*P(6,20) + HK9*P(20,23) - HK9*P(5,20)); -Kfusion(21) = HK33*(-HK12*P(3,21) + HK13*P(21,22) - HK13*P(4,21) + HK14*P(0,21) + HK15*P(1,21) + HK16*P(2,21) + HK17*P(6,21) + HK9*P(21,23) - HK9*P(5,21)); -Kfusion(22) = HK33*(-HK12*P(3,22) + HK24 - HK25 - HK9*P(5,22)); -Kfusion(23) = HK33*(-HK12*P(3,23) - HK13*P(4,23) + HK28 - HK29); +Kfusion(0) = HK33*(-HK12*P(0,3) - HK13*P(0,4) + HK18 - HK9*P(0,23)); +Kfusion(1) = HK33*(-HK12*P(1,3) - HK13*P(1,4) + HK31 - HK9*P(1,23)); +Kfusion(2) = HK33*(-HK12*P(2,3) - HK13*P(2,4) + HK30 - HK9*P(2,23)); +Kfusion(3) = HK33*(-HK12*P(3,3) - HK13*P(3,4) + HK32 - HK9*P(3,23)); +Kfusion(4) = HK33*(-HK12*P(3,4) - HK13*P(4,4) + HK26 - HK9*P(4,23)); +Kfusion(5) = HK33*(-HK12*P(3,5) - HK13*P(4,5) + HK27 - HK28); +Kfusion(6) = HK33*(-HK12*P(3,6) - HK13*P(4,6) + HK23 - HK9*P(6,23)); +Kfusion(7) = HK33*(-HK12*P(3,7) - HK13*P(4,7) + HK13*P(7,22) + HK14*P(0,7) + HK15*P(1,7) + HK16*P(2,7) + HK17*P(6,7) + HK9*P(5,7) - HK9*P(7,23)); +Kfusion(8) = HK33*(-HK12*P(3,8) - HK13*P(4,8) + HK13*P(8,22) + HK14*P(0,8) + HK15*P(1,8) + HK16*P(2,8) + HK17*P(6,8) + HK9*P(5,8) - HK9*P(8,23)); +Kfusion(9) = HK33*(-HK12*P(3,9) - HK13*P(4,9) + HK13*P(9,22) + HK14*P(0,9) + HK15*P(1,9) + HK16*P(2,9) + HK17*P(6,9) + HK9*P(5,9) - HK9*P(9,23)); +Kfusion(10) = HK33*(-HK12*P(3,10) + HK13*P(10,22) - HK13*P(4,10) + HK14*P(0,10) + HK15*P(1,10) + HK16*P(2,10) + HK17*P(6,10) - HK9*P(10,23) + HK9*P(5,10)); +Kfusion(11) = HK33*(-HK12*P(3,11) + HK13*P(11,22) - HK13*P(4,11) + HK14*P(0,11) + HK15*P(1,11) + HK16*P(2,11) + HK17*P(6,11) - HK9*P(11,23) + HK9*P(5,11)); +Kfusion(12) = HK33*(-HK12*P(3,12) + HK13*P(12,22) - HK13*P(4,12) + HK14*P(0,12) + HK15*P(1,12) + HK16*P(2,12) + HK17*P(6,12) - HK9*P(12,23) + HK9*P(5,12)); +Kfusion(13) = HK33*(-HK12*P(3,13) + HK13*P(13,22) - HK13*P(4,13) + HK14*P(0,13) + HK15*P(1,13) + HK16*P(2,13) + HK17*P(6,13) - HK9*P(13,23) + HK9*P(5,13)); +Kfusion(14) = HK33*(-HK12*P(3,14) + HK13*P(14,22) - HK13*P(4,14) + HK14*P(0,14) + HK15*P(1,14) + HK16*P(2,14) + HK17*P(6,14) - HK9*P(14,23) + HK9*P(5,14)); +Kfusion(15) = HK33*(-HK12*P(3,15) + HK13*P(15,22) - HK13*P(4,15) + HK14*P(0,15) + HK15*P(1,15) + HK16*P(2,15) + HK17*P(6,15) - HK9*P(15,23) + HK9*P(5,15)); +Kfusion(16) = HK33*(-HK12*P(3,16) + HK13*P(16,22) - HK13*P(4,16) + HK14*P(0,16) + HK15*P(1,16) + HK16*P(2,16) + HK17*P(6,16) - HK9*P(16,23) + HK9*P(5,16)); +Kfusion(17) = HK33*(-HK12*P(3,17) + HK13*P(17,22) - HK13*P(4,17) + HK14*P(0,17) + HK15*P(1,17) + HK16*P(2,17) + HK17*P(6,17) - HK9*P(17,23) + HK9*P(5,17)); +Kfusion(18) = HK33*(-HK12*P(3,18) + HK13*P(18,22) - HK13*P(4,18) + HK14*P(0,18) + HK15*P(1,18) + HK16*P(2,18) + HK17*P(6,18) - HK9*P(18,23) + HK9*P(5,18)); +Kfusion(19) = HK33*(-HK12*P(3,19) + HK13*P(19,22) - HK13*P(4,19) + HK14*P(0,19) + HK15*P(1,19) + HK16*P(2,19) + HK17*P(6,19) - HK9*P(19,23) + HK9*P(5,19)); +Kfusion(20) = HK33*(-HK12*P(3,20) + HK13*P(20,22) - HK13*P(4,20) + HK14*P(0,20) + HK15*P(1,20) + HK16*P(2,20) + HK17*P(6,20) - HK9*P(20,23) + HK9*P(5,20)); +Kfusion(21) = HK33*(-HK12*P(3,21) + HK13*P(21,22) - HK13*P(4,21) + HK14*P(0,21) + HK15*P(1,21) + HK16*P(2,21) + HK17*P(6,21) - HK9*P(21,23) + HK9*P(5,21)); +Kfusion(22) = HK33*(-HK12*P(3,22) + HK24 - HK25 - HK9*P(22,23)); +Kfusion(23) = HK33*(-HK12*P(3,23) - HK13*P(4,23) + HK29 - HK9*P(23,23)); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp index 8fa4777f36..14a6b21a0b 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp @@ -1,162 +1,149 @@ // Sub Expressions -const float HK0 = q2*vd; +const float HK0 = vn - vwn; 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 + HK7 - HK8; -const float HK12 = q1*vd; -const float HK13 = HK9*q3; -const float HK14 = HK1*q0 + HK12 - 2*HK13; -const float HK15 = 2*(q3)*(q3) - 1; -const float HK16 = HK15 + 2*(q2)*(q2); -const float HK17 = HK16*Kaccx; -const float HK18 = q0*q3; -const float HK19 = q1*q2; -const float HK20 = HK18 + HK19; -const float HK21 = HK20*HK4; -const float HK22 = q0*q2 - q1*q3; -const float HK23 = 2*HK20; -const float HK24 = HK23*P(0,23); -const float HK25 = 2*HK22; -const float HK26 = 2*HK3; -const float HK27 = 2*HK11; -const float HK28 = HK23*P(23,23); -const float HK29 = HK23*P(5,23); -const float HK30 = -2*HK22; -const float HK31 = 2*HK6; -const float HK32 = -HK16; -const float HK33 = -2*HK3; -const float HK34 = 2*HK14; -const float HK35 = -2*HK11; -const float HK36 = (Kaccx)*(Kaccx); -const float HK37 = HK23*HK36; -const float HK38 = -HK29; -const float HK39 = HK23*P(6,23); -const float HK40 = HK23*P(1,23); -const float HK41 = HK23*P(4,23); -const float HK42 = HK16*P(4,22); -const float HK43 = HK16*HK36; -const float HK44 = HK23*P(22,23); -const float HK45 = HK23*P(3,23); -const float HK46 = HK23*P(2,23); -const float HK47 = Kaccx/(HK25*HK36*(HK16*P(6,22) + HK23*P(5,6) + HK30*P(6,6) + HK31*P(1,6) + HK32*P(4,6) + HK33*P(0,6) + HK34*P(3,6) + HK35*P(2,6) - HK39) + HK26*HK36*(HK16*P(0,22) + HK23*P(0,5) - HK24 + HK30*P(0,6) + HK31*P(0,1) + HK32*P(0,4) + HK33*P(0,0) + HK34*P(0,3) + HK35*P(0,2)) + HK27*HK36*(HK16*P(2,22) + HK23*P(2,5) + HK30*P(2,6) + HK31*P(1,2) + HK32*P(2,4) + HK33*P(0,2) + HK34*P(2,3) + HK35*P(2,2) - HK46) - HK31*HK36*(HK16*P(1,22) + HK23*P(1,5) + HK30*P(1,6) + HK31*P(1,1) + HK32*P(1,4) + HK33*P(0,1) + HK34*P(1,3) + HK35*P(1,2) - HK40) - HK34*HK36*(HK16*P(3,22) + HK23*P(3,5) + HK30*P(3,6) + HK31*P(1,3) + HK32*P(3,4) + HK33*P(0,3) + HK34*P(3,3) + HK35*P(2,3) - HK45) + HK37*(HK16*P(22,23) - HK28 + HK29 + HK30*P(6,23) + HK31*P(1,23) + HK32*P(4,23) + HK33*P(0,23) + HK34*P(3,23) + HK35*P(2,23)) - HK37*(HK16*P(5,22) + HK23*P(5,5) + HK30*P(5,6) + HK31*P(1,5) + HK32*P(4,5) + HK33*P(0,5) + HK34*P(3,5) + HK35*P(2,5) + HK38) - HK43*(HK16*P(22,22) + HK23*P(5,22) + HK30*P(6,22) + HK31*P(1,22) + HK32*P(4,22) + HK33*P(0,22) + HK34*P(3,22) + HK35*P(2,22) - HK44) + HK43*(HK23*P(4,5) + HK30*P(4,6) + HK31*P(1,4) + HK32*P(4,4) + HK33*P(0,4) + HK34*P(3,4) + HK35*P(2,4) - HK41 + HK42) - R_ACC); -const float HK48 = HK12 - HK13; -const float HK49 = 2*Kaccy; -const float HK50 = HK10 + HK7 - 2*HK8; -const float HK51 = HK5 + HK9*q1; -const float HK52 = -HK0 + 2*HK2 + HK9*q0; -const float HK53 = HK18 - HK19; -const float HK54 = HK49*HK53; -const float HK55 = HK15 + 2*(q1)*(q1); -const float HK56 = HK55*Kaccy; -const float HK57 = q0*q1 + q2*q3; -const float HK58 = 2*HK52; -const float HK59 = 2*HK53; -const float HK60 = 2*HK48; -const float HK61 = 2*HK50; -const float HK62 = 2*HK51; -const float HK63 = 2*HK57; -const float HK64 = HK55*P(0,23) + HK59*P(0,22) + HK60*P(0,0) + HK61*P(0,1) + HK62*P(0,2) + HK63*P(0,6); -const float HK65 = (Kaccy)*(Kaccy); -const float HK66 = -HK55; -const float HK67 = -2*HK52; -const float HK68 = -2*HK53; -const float HK69 = HK55*P(6,23) + HK59*P(6,22) + HK60*P(0,6) + HK61*P(1,6) + HK62*P(2,6) + HK63*P(6,6); -const float HK70 = HK55*P(22,23) + HK59*P(22,22) + HK60*P(0,22) + HK61*P(1,22) + HK62*P(2,22) + HK63*P(6,22); -const float HK71 = HK59*P(4,22); -const float HK72 = HK55*P(4,23) + HK60*P(0,4) + HK61*P(1,4) + HK62*P(2,4) + HK63*P(4,6) + HK71; -const float HK73 = HK55*P(2,23) + HK59*P(2,22) + HK60*P(0,2) + HK61*P(1,2) + HK62*P(2,2) + HK63*P(2,6); -const float HK74 = HK55*P(23,23) + HK59*P(22,23) + HK60*P(0,23) + HK61*P(1,23) + HK62*P(2,23) + HK63*P(6,23); -const float HK75 = HK55*P(5,23); -const float HK76 = HK59*P(5,22) + HK60*P(0,5) + HK61*P(1,5) + HK62*P(2,5) + HK63*P(5,6) + HK75; -const float HK77 = HK55*P(1,23) + HK59*P(1,22) + HK60*P(0,1) + HK61*P(1,1) + HK62*P(1,2) + HK63*P(1,6); -const float HK78 = HK55*P(3,23) + HK59*P(3,22) + HK60*P(0,3) + HK61*P(1,3) + HK62*P(2,3) + HK63*P(3,6); -const float HK79 = Kaccy/(2*HK52*HK65*(HK66*P(3,5) + HK67*P(3,3) + HK68*P(3,4) + HK78) + 2*HK53*HK65*(HK66*P(4,5) + HK67*P(3,4) + HK68*P(4,4) + HK72) - HK55*HK65*(HK66*P(5,23) + HK67*P(3,23) + HK68*P(4,23) + HK74) + HK55*HK65*(HK66*P(5,5) + HK67*P(3,5) + HK68*P(4,5) + HK76) - HK59*HK65*(HK66*P(5,22) + HK67*P(3,22) + HK68*P(4,22) + HK70) - HK60*HK65*(HK64 + HK66*P(0,5) + HK67*P(0,3) + HK68*P(0,4)) - HK61*HK65*(HK66*P(1,5) + HK67*P(1,3) + HK68*P(1,4) + HK77) - HK62*HK65*(HK66*P(2,5) + HK67*P(2,3) + HK68*P(2,4) + HK73) - HK63*HK65*(HK66*P(5,6) + HK67*P(3,6) + HK68*P(4,6) + HK69) - R_ACC); +const float HK2 = HK0*q0 + HK1*q3 - q2*vd; +const float HK3 = 2*Kaccx; +const float HK4 = HK0*q1 + HK1*q2 + q3*vd; +const float HK5 = HK0*q2 - HK1*q1 + q0*vd; +const float HK6 = -HK0*q3 + HK1*q0 + q1*vd; +const float HK7 = (q1)*(q1); +const float HK8 = (q2)*(q2); +const float HK9 = (q0)*(q0) - (q3)*(q3); +const float HK10 = HK7 - HK8 + HK9; +const float HK11 = HK10*Kaccx; +const float HK12 = q0*q3; +const float HK13 = q1*q2; +const float HK14 = HK12 + HK13; +const float HK15 = HK14*HK3; +const float HK16 = q0*q2 - q1*q3; +const float HK17 = 2*HK5; +const float HK18 = 2*HK16; +const float HK19 = 2*HK14; +const float HK20 = 2*HK2; +const float HK21 = 2*HK4; +const float HK22 = 2*HK6; +const float HK23 = HK22*P(0,3); +const float HK24 = HK10*P(0,4) - HK19*P(0,23) + HK19*P(0,5) + HK20*P(0,0) + HK21*P(0,1) + HK23; +const float HK25 = (Kaccx)*(Kaccx); +const float HK26 = -HK10; +const float HK27 = -2*HK5; +const float HK28 = -2*HK16; +const float HK29 = HK19*P(5,23); +const float HK30 = HK10*P(4,23) - HK19*P(23,23) + HK20*P(0,23) + HK21*P(1,23) + HK22*P(3,23) + HK29; +const float HK31 = HK10*P(4,5) + HK19*P(5,5) + HK20*P(0,5) + HK21*P(1,5) + HK22*P(3,5) - HK29; +const float HK32 = HK10*P(4,6) + HK19*P(5,6) - HK19*P(6,23) + HK20*P(0,6) + HK21*P(1,6) + HK22*P(3,6); +const float HK33 = HK10*P(4,4) - HK19*P(4,23) + HK19*P(4,5) + HK20*P(0,4) + HK21*P(1,4) + HK22*P(3,4); +const float HK34 = HK10*P(4,22); +const float HK35 = -HK19*P(22,23) + HK19*P(5,22) + HK20*P(0,22) + HK21*P(1,22) + HK22*P(3,22) + HK34; +const float HK36 = HK10*P(1,4) - HK19*P(1,23) + HK19*P(1,5) + HK20*P(0,1) + HK21*P(1,1) + HK22*P(1,3); +const float HK37 = HK21*P(1,2); +const float HK38 = HK10*P(2,4) - HK19*P(2,23) + HK19*P(2,5) + HK20*P(0,2) + HK22*P(2,3) + HK37; +const float HK39 = HK20*P(0,3); +const float HK40 = HK10*P(3,4) - HK19*P(3,23) + HK19*P(3,5) + HK21*P(1,3) + HK22*P(3,3) + HK39; +const float HK41 = Kaccx/(HK10*HK25*(HK26*P(22,22) + HK27*P(2,22) + HK28*P(6,22) + HK35) - HK10*HK25*(HK26*P(4,22) + HK27*P(2,4) + HK28*P(4,6) + HK33) + 2*HK14*HK25*(HK26*P(22,23) + HK27*P(2,23) + HK28*P(6,23) + HK30) + 2*HK16*HK25*(HK26*P(6,22) + HK27*P(2,6) + HK28*P(6,6) + HK32) - HK19*HK25*(HK26*P(5,22) + HK27*P(2,5) + HK28*P(5,6) + HK31) - HK20*HK25*(HK24 + HK26*P(0,22) + HK27*P(0,2) + HK28*P(0,6)) - HK21*HK25*(HK26*P(1,22) + HK27*P(1,2) + HK28*P(1,6) + HK36) - HK22*HK25*(HK26*P(3,22) + HK27*P(2,3) + HK28*P(3,6) + HK40) + 2*HK25*HK5*(HK26*P(2,22) + HK27*P(2,2) + HK28*P(2,6) + HK38) - R_ACC); +const float HK42 = HK17*P(1,2); +const float HK43 = HK12 - HK13; +const float HK44 = 2*Kaccy; +const float HK45 = HK43*HK44; +const float HK46 = -HK7 + HK8 + HK9; +const float HK47 = HK46*Kaccy; +const float HK48 = q0*q1 + q2*q3; +const float HK49 = 2*HK43; +const float HK50 = 2*HK48; +const float HK51 = HK17*P(0,1) + HK21*P(0,2) + HK22*P(0,0) + HK46*P(0,5) + HK49*P(0,22) + HK50*P(0,6); +const float HK52 = (Kaccy)*(Kaccy); +const float HK53 = -HK46; +const float HK54 = -2*HK2; +const float HK55 = -2*HK43; +const float HK56 = HK17*P(1,6) + HK21*P(2,6) + HK22*P(0,6) + HK46*P(5,6) + HK49*P(6,22) + HK50*P(6,6); +const float HK57 = HK17*P(1,22) + HK21*P(2,22) + HK22*P(0,22) + HK46*P(5,22) + HK49*P(22,22) + HK50*P(6,22); +const float HK58 = HK49*P(4,22); +const float HK59 = HK17*P(1,4) + HK21*P(2,4) + HK22*P(0,4) + HK46*P(4,5) + HK50*P(4,6) + HK58; +const float HK60 = HK17*P(1,5) + HK21*P(2,5) + HK22*P(0,5) + HK46*P(5,5) + HK49*P(5,22) + HK50*P(5,6); +const float HK61 = HK46*P(5,23); +const float HK62 = HK17*P(1,23) + HK21*P(2,23) + HK22*P(0,23) + HK49*P(22,23) + HK50*P(6,23) + HK61; +const float HK63 = HK21*P(2,2) + HK22*P(0,2) + HK42 + HK46*P(2,5) + HK49*P(2,22) + HK50*P(2,6); +const float HK64 = HK17*P(1,1) + HK22*P(0,1) + HK37 + HK46*P(1,5) + HK49*P(1,22) + HK50*P(1,6); +const float HK65 = HK17*P(1,3) + HK21*P(2,3) + HK23 + HK46*P(3,5) + HK49*P(3,22) + HK50*P(3,6); +const float HK66 = Kaccy/(-HK17*HK52*(HK53*P(1,23) + HK54*P(1,3) + HK55*P(1,4) + HK64) + 2*HK2*HK52*(HK53*P(3,23) + HK54*P(3,3) + HK55*P(3,4) + HK65) - HK21*HK52*(HK53*P(2,23) + HK54*P(2,3) + HK55*P(2,4) + HK63) - HK22*HK52*(HK51 + HK53*P(0,23) + HK54*P(0,3) + HK55*P(0,4)) + 2*HK43*HK52*(HK53*P(4,23) + HK54*P(3,4) + HK55*P(4,4) + HK59) + HK46*HK52*(HK53*P(23,23) + HK54*P(3,23) + HK55*P(4,23) + HK62) - HK46*HK52*(HK53*P(5,23) + HK54*P(3,5) + HK55*P(4,5) + HK60) - HK49*HK52*(HK53*P(22,23) + HK54*P(3,22) + HK55*P(4,22) + HK57) - HK50*HK52*(HK53*P(6,23) + HK54*P(3,6) + HK55*P(4,6) + HK56) - R_ACC); // Observation Jacobians - axis 0 -Hfusion.at<0>() = HK3*HK4; -Hfusion.at<1>() = -HK4*HK6; -Hfusion.at<2>() = HK11*HK4; -Hfusion.at<3>() = -HK14*HK4; -Hfusion.at<4>() = HK17; -Hfusion.at<5>() = -HK21; -Hfusion.at<6>() = HK22*HK4; -Hfusion.at<22>() = -HK17; -Hfusion.at<23>() = HK21; +Hfusion.at<0>() = -HK2*HK3; +Hfusion.at<1>() = -HK3*HK4; +Hfusion.at<2>() = HK3*HK5; +Hfusion.at<3>() = -HK3*HK6; +Hfusion.at<4>() = -HK11; +Hfusion.at<5>() = -HK15; +Hfusion.at<6>() = HK16*HK3; +Hfusion.at<22>() = HK11; +Hfusion.at<23>() = HK15; // Kalman gains - axis 0 -Kfusion(0) = HK47*(2*HK14*P(0,3) + HK16*P(0,22) - HK16*P(0,4) + 2*HK20*P(0,5) - HK24 - HK25*P(0,6) - HK26*P(0,0) - HK27*P(0,2) + 2*HK6*P(0,1)); -Kfusion(1) = HK47*(2*HK14*P(1,3) + HK16*P(1,22) - HK16*P(1,4) + 2*HK20*P(1,5) - HK25*P(1,6) - HK26*P(0,1) - HK27*P(1,2) - HK40 + 2*HK6*P(1,1)); -Kfusion(2) = HK47*(2*HK14*P(2,3) + HK16*P(2,22) - HK16*P(2,4) + 2*HK20*P(2,5) - HK25*P(2,6) - HK26*P(0,2) - HK27*P(2,2) - HK46 + 2*HK6*P(1,2)); -Kfusion(3) = HK47*(2*HK14*P(3,3) + HK16*P(3,22) - HK16*P(3,4) + 2*HK20*P(3,5) - HK25*P(3,6) - HK26*P(0,3) - HK27*P(2,3) - HK45 + 2*HK6*P(1,3)); -Kfusion(4) = HK47*(2*HK14*P(3,4) - HK16*P(4,4) + 2*HK20*P(4,5) - HK25*P(4,6) - HK26*P(0,4) - HK27*P(2,4) - HK41 + HK42 + 2*HK6*P(1,4)); -Kfusion(5) = HK47*(2*HK14*P(3,5) - HK16*P(4,5) + HK16*P(5,22) + 2*HK20*P(5,5) - HK25*P(5,6) - HK26*P(0,5) - HK27*P(2,5) - HK29 + 2*HK6*P(1,5)); -Kfusion(6) = HK47*(2*HK14*P(3,6) - HK16*P(4,6) + HK16*P(6,22) + 2*HK20*P(5,6) - HK25*P(6,6) - HK26*P(0,6) - HK27*P(2,6) - HK39 + 2*HK6*P(1,6)); -Kfusion(7) = HK47*(2*HK14*P(3,7) - HK16*P(4,7) + HK16*P(7,22) + 2*HK20*P(5,7) - HK23*P(7,23) - HK25*P(6,7) - HK26*P(0,7) - HK27*P(2,7) + 2*HK6*P(1,7)); -Kfusion(8) = HK47*(2*HK14*P(3,8) - HK16*P(4,8) + HK16*P(8,22) + 2*HK20*P(5,8) - HK23*P(8,23) - HK25*P(6,8) - HK26*P(0,8) - HK27*P(2,8) + 2*HK6*P(1,8)); -Kfusion(9) = HK47*(2*HK14*P(3,9) - HK16*P(4,9) + HK16*P(9,22) + 2*HK20*P(5,9) - HK23*P(9,23) - HK25*P(6,9) - HK26*P(0,9) - HK27*P(2,9) + 2*HK6*P(1,9)); -Kfusion(10) = HK47*(2*HK14*P(3,10) + HK16*P(10,22) - HK16*P(4,10) + 2*HK20*P(5,10) - HK23*P(10,23) - HK25*P(6,10) - HK26*P(0,10) - HK27*P(2,10) + 2*HK6*P(1,10)); -Kfusion(11) = HK47*(2*HK14*P(3,11) + HK16*P(11,22) - HK16*P(4,11) + 2*HK20*P(5,11) - HK23*P(11,23) - HK25*P(6,11) - HK26*P(0,11) - HK27*P(2,11) + 2*HK6*P(1,11)); -Kfusion(12) = HK47*(2*HK14*P(3,12) + HK16*P(12,22) - HK16*P(4,12) + 2*HK20*P(5,12) - HK23*P(12,23) - HK25*P(6,12) - HK26*P(0,12) - HK27*P(2,12) + 2*HK6*P(1,12)); -Kfusion(13) = HK47*(2*HK14*P(3,13) + HK16*P(13,22) - HK16*P(4,13) + 2*HK20*P(5,13) - HK23*P(13,23) - HK25*P(6,13) - HK26*P(0,13) - HK27*P(2,13) + 2*HK6*P(1,13)); -Kfusion(14) = HK47*(2*HK14*P(3,14) + HK16*P(14,22) - HK16*P(4,14) + 2*HK20*P(5,14) - HK23*P(14,23) - HK25*P(6,14) - HK26*P(0,14) - HK27*P(2,14) + 2*HK6*P(1,14)); -Kfusion(15) = HK47*(2*HK14*P(3,15) + HK16*P(15,22) - HK16*P(4,15) + 2*HK20*P(5,15) - HK23*P(15,23) - HK25*P(6,15) - HK26*P(0,15) - HK27*P(2,15) + 2*HK6*P(1,15)); -Kfusion(16) = HK47*(2*HK14*P(3,16) + HK16*P(16,22) - HK16*P(4,16) + 2*HK20*P(5,16) - HK23*P(16,23) - HK25*P(6,16) - HK26*P(0,16) - HK27*P(2,16) + 2*HK6*P(1,16)); -Kfusion(17) = HK47*(2*HK14*P(3,17) + HK16*P(17,22) - HK16*P(4,17) + 2*HK20*P(5,17) - HK23*P(17,23) - HK25*P(6,17) - HK26*P(0,17) - HK27*P(2,17) + 2*HK6*P(1,17)); -Kfusion(18) = HK47*(2*HK14*P(3,18) + HK16*P(18,22) - HK16*P(4,18) + 2*HK20*P(5,18) - HK23*P(18,23) - HK25*P(6,18) - HK26*P(0,18) - HK27*P(2,18) + 2*HK6*P(1,18)); -Kfusion(19) = HK47*(2*HK14*P(3,19) + HK16*P(19,22) - HK16*P(4,19) + 2*HK20*P(5,19) - HK23*P(19,23) - HK25*P(6,19) - HK26*P(0,19) - HK27*P(2,19) + 2*HK6*P(1,19)); -Kfusion(20) = HK47*(2*HK14*P(3,20) + HK16*P(20,22) - HK16*P(4,20) + 2*HK20*P(5,20) - HK23*P(20,23) - HK25*P(6,20) - HK26*P(0,20) - HK27*P(2,20) + 2*HK6*P(1,20)); -Kfusion(21) = HK47*(2*HK14*P(3,21) + HK16*P(21,22) - HK16*P(4,21) + 2*HK20*P(5,21) - HK23*P(21,23) - HK25*P(6,21) - HK26*P(0,21) - HK27*P(2,21) + 2*HK6*P(1,21)); -Kfusion(22) = HK47*(2*HK14*P(3,22) + HK16*P(22,22) + 2*HK20*P(5,22) - HK25*P(6,22) - HK26*P(0,22) - HK27*P(2,22) - HK42 - HK44 + 2*HK6*P(1,22)); -Kfusion(23) = HK47*(2*HK14*P(3,23) + HK16*P(22,23) - HK16*P(4,23) - HK25*P(6,23) - HK26*P(0,23) - HK27*P(2,23) - HK28 - HK38 + 2*HK6*P(1,23)); +Kfusion(0) = HK41*(-HK10*P(0,22) - HK17*P(0,2) - HK18*P(0,6) + HK24); +Kfusion(1) = HK41*(-HK10*P(1,22) - HK18*P(1,6) + HK36 - HK42); +Kfusion(2) = HK41*(-HK10*P(2,22) - HK17*P(2,2) - HK18*P(2,6) + HK38); +Kfusion(3) = HK41*(-HK10*P(3,22) - HK17*P(2,3) - HK18*P(3,6) + HK40); +Kfusion(4) = HK41*(-HK17*P(2,4) - HK18*P(4,6) + HK33 - HK34); +Kfusion(5) = HK41*(-HK10*P(5,22) - HK17*P(2,5) - HK18*P(5,6) + HK31); +Kfusion(6) = HK41*(-HK10*P(6,22) - HK17*P(2,6) - HK18*P(6,6) + HK32); +Kfusion(7) = HK41*(HK10*P(4,7) - HK10*P(7,22) - HK17*P(2,7) - HK18*P(6,7) + HK19*P(5,7) - HK19*P(7,23) + HK20*P(0,7) + HK21*P(1,7) + HK22*P(3,7)); +Kfusion(8) = HK41*(HK10*P(4,8) - HK10*P(8,22) - HK17*P(2,8) - HK18*P(6,8) + HK19*P(5,8) - HK19*P(8,23) + HK20*P(0,8) + HK21*P(1,8) + HK22*P(3,8)); +Kfusion(9) = HK41*(HK10*P(4,9) - HK10*P(9,22) - HK17*P(2,9) - HK18*P(6,9) + HK19*P(5,9) - HK19*P(9,23) + HK20*P(0,9) + HK21*P(1,9) + HK22*P(3,9)); +Kfusion(10) = HK41*(-HK10*P(10,22) + HK10*P(4,10) - HK17*P(2,10) - HK18*P(6,10) - HK19*P(10,23) + HK19*P(5,10) + HK20*P(0,10) + HK21*P(1,10) + HK22*P(3,10)); +Kfusion(11) = HK41*(-HK10*P(11,22) + HK10*P(4,11) - HK17*P(2,11) - HK18*P(6,11) - HK19*P(11,23) + HK19*P(5,11) + HK20*P(0,11) + HK21*P(1,11) + HK22*P(3,11)); +Kfusion(12) = HK41*(-HK10*P(12,22) + HK10*P(4,12) - HK17*P(2,12) - HK18*P(6,12) - HK19*P(12,23) + HK19*P(5,12) + HK20*P(0,12) + HK21*P(1,12) + HK22*P(3,12)); +Kfusion(13) = HK41*(-HK10*P(13,22) + HK10*P(4,13) - HK17*P(2,13) - HK18*P(6,13) - HK19*P(13,23) + HK19*P(5,13) + HK20*P(0,13) + HK21*P(1,13) + HK22*P(3,13)); +Kfusion(14) = HK41*(-HK10*P(14,22) + HK10*P(4,14) - HK17*P(2,14) - HK18*P(6,14) - HK19*P(14,23) + HK19*P(5,14) + HK20*P(0,14) + HK21*P(1,14) + HK22*P(3,14)); +Kfusion(15) = HK41*(-HK10*P(15,22) + HK10*P(4,15) - HK17*P(2,15) - HK18*P(6,15) - HK19*P(15,23) + HK19*P(5,15) + HK20*P(0,15) + HK21*P(1,15) + HK22*P(3,15)); +Kfusion(16) = HK41*(-HK10*P(16,22) + HK10*P(4,16) - HK17*P(2,16) - HK18*P(6,16) - HK19*P(16,23) + HK19*P(5,16) + HK20*P(0,16) + HK21*P(1,16) + HK22*P(3,16)); +Kfusion(17) = HK41*(-HK10*P(17,22) + HK10*P(4,17) - HK17*P(2,17) - HK18*P(6,17) - HK19*P(17,23) + HK19*P(5,17) + HK20*P(0,17) + HK21*P(1,17) + HK22*P(3,17)); +Kfusion(18) = HK41*(-HK10*P(18,22) + HK10*P(4,18) - HK17*P(2,18) - HK18*P(6,18) - HK19*P(18,23) + HK19*P(5,18) + HK20*P(0,18) + HK21*P(1,18) + HK22*P(3,18)); +Kfusion(19) = HK41*(-HK10*P(19,22) + HK10*P(4,19) - HK17*P(2,19) - HK18*P(6,19) - HK19*P(19,23) + HK19*P(5,19) + HK20*P(0,19) + HK21*P(1,19) + HK22*P(3,19)); +Kfusion(20) = HK41*(-HK10*P(20,22) + HK10*P(4,20) - HK17*P(2,20) - HK18*P(6,20) - HK19*P(20,23) + HK19*P(5,20) + HK20*P(0,20) + HK21*P(1,20) + HK22*P(3,20)); +Kfusion(21) = HK41*(-HK10*P(21,22) + HK10*P(4,21) - HK17*P(2,21) - HK18*P(6,21) - HK19*P(21,23) + HK19*P(5,21) + HK20*P(0,21) + HK21*P(1,21) + HK22*P(3,21)); +Kfusion(22) = HK41*(-HK10*P(22,22) - HK17*P(2,22) - HK18*P(6,22) + HK35); +Kfusion(23) = HK41*(-HK10*P(22,23) - HK17*P(2,23) - HK18*P(6,23) + HK30); // Observation Jacobians - axis 1 -Hfusion.at<0>() = -HK48*HK49; -Hfusion.at<1>() = -HK49*HK50; -Hfusion.at<2>() = -HK49*HK51; -Hfusion.at<3>() = HK49*HK52; -Hfusion.at<4>() = HK54; -Hfusion.at<5>() = HK56; -Hfusion.at<6>() = -HK49*HK57; -Hfusion.at<22>() = -HK54; -Hfusion.at<23>() = -HK56; +Hfusion.at<0>() = -HK22*Kaccy; +Hfusion.at<1>() = -HK17*Kaccy; +Hfusion.at<2>() = -HK21*Kaccy; +Hfusion.at<3>() = HK20*Kaccy; +Hfusion.at<4>() = HK45; +Hfusion.at<5>() = -HK47; +Hfusion.at<6>() = -HK44*HK48; +Hfusion.at<22>() = -HK45; +Hfusion.at<23>() = HK47; // Kalman gains - axis 1 -Kfusion(0) = HK79*(-HK55*P(0,5) - HK58*P(0,3) - HK59*P(0,4) + HK64); -Kfusion(1) = HK79*(-HK55*P(1,5) - HK58*P(1,3) - HK59*P(1,4) + HK77); -Kfusion(2) = HK79*(-HK55*P(2,5) - HK58*P(2,3) - HK59*P(2,4) + HK73); -Kfusion(3) = HK79*(-HK55*P(3,5) - HK58*P(3,3) - HK59*P(3,4) + HK78); -Kfusion(4) = HK79*(-HK55*P(4,5) - HK58*P(3,4) - HK59*P(4,4) + HK72); -Kfusion(5) = HK79*(-HK55*P(5,5) - HK58*P(3,5) - HK59*P(4,5) + HK76); -Kfusion(6) = HK79*(-HK55*P(5,6) - HK58*P(3,6) - HK59*P(4,6) + HK69); -Kfusion(7) = HK79*(-HK55*P(5,7) + HK55*P(7,23) - HK58*P(3,7) - HK59*P(4,7) + HK59*P(7,22) + HK60*P(0,7) + HK61*P(1,7) + HK62*P(2,7) + HK63*P(6,7)); -Kfusion(8) = HK79*(-HK55*P(5,8) + HK55*P(8,23) - HK58*P(3,8) - HK59*P(4,8) + HK59*P(8,22) + HK60*P(0,8) + HK61*P(1,8) + HK62*P(2,8) + HK63*P(6,8)); -Kfusion(9) = HK79*(-HK55*P(5,9) + HK55*P(9,23) - HK58*P(3,9) - HK59*P(4,9) + HK59*P(9,22) + HK60*P(0,9) + HK61*P(1,9) + HK62*P(2,9) + HK63*P(6,9)); -Kfusion(10) = HK79*(HK55*P(10,23) - HK55*P(5,10) - HK58*P(3,10) + HK59*P(10,22) - HK59*P(4,10) + HK60*P(0,10) + HK61*P(1,10) + HK62*P(2,10) + HK63*P(6,10)); -Kfusion(11) = HK79*(HK55*P(11,23) - HK55*P(5,11) - HK58*P(3,11) + HK59*P(11,22) - HK59*P(4,11) + HK60*P(0,11) + HK61*P(1,11) + HK62*P(2,11) + HK63*P(6,11)); -Kfusion(12) = HK79*(HK55*P(12,23) - HK55*P(5,12) - HK58*P(3,12) + HK59*P(12,22) - HK59*P(4,12) + HK60*P(0,12) + HK61*P(1,12) + HK62*P(2,12) + HK63*P(6,12)); -Kfusion(13) = HK79*(HK55*P(13,23) - HK55*P(5,13) - HK58*P(3,13) + HK59*P(13,22) - HK59*P(4,13) + HK60*P(0,13) + HK61*P(1,13) + HK62*P(2,13) + HK63*P(6,13)); -Kfusion(14) = HK79*(HK55*P(14,23) - HK55*P(5,14) - HK58*P(3,14) + HK59*P(14,22) - HK59*P(4,14) + HK60*P(0,14) + HK61*P(1,14) + HK62*P(2,14) + HK63*P(6,14)); -Kfusion(15) = HK79*(HK55*P(15,23) - HK55*P(5,15) - HK58*P(3,15) + HK59*P(15,22) - HK59*P(4,15) + HK60*P(0,15) + HK61*P(1,15) + HK62*P(2,15) + HK63*P(6,15)); -Kfusion(16) = HK79*(HK55*P(16,23) - HK55*P(5,16) - HK58*P(3,16) + HK59*P(16,22) - HK59*P(4,16) + HK60*P(0,16) + HK61*P(1,16) + HK62*P(2,16) + HK63*P(6,16)); -Kfusion(17) = HK79*(HK55*P(17,23) - HK55*P(5,17) - HK58*P(3,17) + HK59*P(17,22) - HK59*P(4,17) + HK60*P(0,17) + HK61*P(1,17) + HK62*P(2,17) + HK63*P(6,17)); -Kfusion(18) = HK79*(HK55*P(18,23) - HK55*P(5,18) - HK58*P(3,18) + HK59*P(18,22) - HK59*P(4,18) + HK60*P(0,18) + HK61*P(1,18) + HK62*P(2,18) + HK63*P(6,18)); -Kfusion(19) = HK79*(HK55*P(19,23) - HK55*P(5,19) - HK58*P(3,19) + HK59*P(19,22) - HK59*P(4,19) + HK60*P(0,19) + HK61*P(1,19) + HK62*P(2,19) + HK63*P(6,19)); -Kfusion(20) = HK79*(HK55*P(20,23) - HK55*P(5,20) - HK58*P(3,20) + HK59*P(20,22) - HK59*P(4,20) + HK60*P(0,20) + HK61*P(1,20) + HK62*P(2,20) + HK63*P(6,20)); -Kfusion(21) = HK79*(HK55*P(21,23) - HK55*P(5,21) - HK58*P(3,21) + HK59*P(21,22) - HK59*P(4,21) + HK60*P(0,21) + HK61*P(1,21) + HK62*P(2,21) + HK63*P(6,21)); -Kfusion(22) = HK79*(-HK55*P(5,22) - HK58*P(3,22) + HK70 - HK71); -Kfusion(23) = HK79*(-HK58*P(3,23) - HK59*P(4,23) + HK74 - HK75); +Kfusion(0) = HK66*(-HK39 - HK46*P(0,23) - HK49*P(0,4) + HK51); +Kfusion(1) = HK66*(-HK20*P(1,3) - HK46*P(1,23) - HK49*P(1,4) + HK64); +Kfusion(2) = HK66*(-HK20*P(2,3) - HK46*P(2,23) - HK49*P(2,4) + HK63); +Kfusion(3) = HK66*(-HK20*P(3,3) - HK46*P(3,23) - HK49*P(3,4) + HK65); +Kfusion(4) = HK66*(-HK20*P(3,4) - HK46*P(4,23) - HK49*P(4,4) + HK59); +Kfusion(5) = HK66*(-HK20*P(3,5) - HK49*P(4,5) + HK60 - HK61); +Kfusion(6) = HK66*(-HK20*P(3,6) - HK46*P(6,23) - HK49*P(4,6) + HK56); +Kfusion(7) = HK66*(HK17*P(1,7) - HK20*P(3,7) + HK21*P(2,7) + HK22*P(0,7) + HK46*P(5,7) - HK46*P(7,23) - HK49*P(4,7) + HK49*P(7,22) + HK50*P(6,7)); +Kfusion(8) = HK66*(HK17*P(1,8) - HK20*P(3,8) + HK21*P(2,8) + HK22*P(0,8) + HK46*P(5,8) - HK46*P(8,23) - HK49*P(4,8) + HK49*P(8,22) + HK50*P(6,8)); +Kfusion(9) = HK66*(HK17*P(1,9) - HK20*P(3,9) + HK21*P(2,9) + HK22*P(0,9) + HK46*P(5,9) - HK46*P(9,23) - HK49*P(4,9) + HK49*P(9,22) + HK50*P(6,9)); +Kfusion(10) = HK66*(HK17*P(1,10) - HK20*P(3,10) + HK21*P(2,10) + HK22*P(0,10) - HK46*P(10,23) + HK46*P(5,10) + HK49*P(10,22) - HK49*P(4,10) + HK50*P(6,10)); +Kfusion(11) = HK66*(HK17*P(1,11) - HK20*P(3,11) + HK21*P(2,11) + HK22*P(0,11) - HK46*P(11,23) + HK46*P(5,11) + HK49*P(11,22) - HK49*P(4,11) + HK50*P(6,11)); +Kfusion(12) = HK66*(HK17*P(1,12) - HK20*P(3,12) + HK21*P(2,12) + HK22*P(0,12) - HK46*P(12,23) + HK46*P(5,12) + HK49*P(12,22) - HK49*P(4,12) + HK50*P(6,12)); +Kfusion(13) = HK66*(HK17*P(1,13) - HK20*P(3,13) + HK21*P(2,13) + HK22*P(0,13) - HK46*P(13,23) + HK46*P(5,13) + HK49*P(13,22) - HK49*P(4,13) + HK50*P(6,13)); +Kfusion(14) = HK66*(HK17*P(1,14) - HK20*P(3,14) + HK21*P(2,14) + HK22*P(0,14) - HK46*P(14,23) + HK46*P(5,14) + HK49*P(14,22) - HK49*P(4,14) + HK50*P(6,14)); +Kfusion(15) = HK66*(HK17*P(1,15) - HK20*P(3,15) + HK21*P(2,15) + HK22*P(0,15) - HK46*P(15,23) + HK46*P(5,15) + HK49*P(15,22) - HK49*P(4,15) + HK50*P(6,15)); +Kfusion(16) = HK66*(HK17*P(1,16) - HK20*P(3,16) + HK21*P(2,16) + HK22*P(0,16) - HK46*P(16,23) + HK46*P(5,16) + HK49*P(16,22) - HK49*P(4,16) + HK50*P(6,16)); +Kfusion(17) = HK66*(HK17*P(1,17) - HK20*P(3,17) + HK21*P(2,17) + HK22*P(0,17) - HK46*P(17,23) + HK46*P(5,17) + HK49*P(17,22) - HK49*P(4,17) + HK50*P(6,17)); +Kfusion(18) = HK66*(HK17*P(1,18) - HK20*P(3,18) + HK21*P(2,18) + HK22*P(0,18) - HK46*P(18,23) + HK46*P(5,18) + HK49*P(18,22) - HK49*P(4,18) + HK50*P(6,18)); +Kfusion(19) = HK66*(HK17*P(1,19) - HK20*P(3,19) + HK21*P(2,19) + HK22*P(0,19) - HK46*P(19,23) + HK46*P(5,19) + HK49*P(19,22) - HK49*P(4,19) + HK50*P(6,19)); +Kfusion(20) = HK66*(HK17*P(1,20) - HK20*P(3,20) + HK21*P(2,20) + HK22*P(0,20) - HK46*P(20,23) + HK46*P(5,20) + HK49*P(20,22) - HK49*P(4,20) + HK50*P(6,20)); +Kfusion(21) = HK66*(HK17*P(1,21) - HK20*P(3,21) + HK21*P(2,21) + HK22*P(0,21) - HK46*P(21,23) + HK46*P(5,21) + HK49*P(21,22) - HK49*P(4,21) + HK50*P(6,21)); +Kfusion(22) = HK66*(-HK20*P(3,22) - HK46*P(22,23) + HK57 - HK58); +Kfusion(23) = HK66*(-HK20*P(3,23) - HK46*P(23,23) - HK49*P(4,23) + HK62); // Observation Jacobians - axis 2 diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_generated.cpp index 52e2f894c9..d493d966db 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_generated.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_generated.cpp @@ -1,91 +1,93 @@ // 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 = q0*q3; -const float HK9 = q1*q2; -const float HK10 = 2*HK8 + 2*HK9; -const float HK11 = 2*(q3)*(q3) - 1; -const float HK12 = HK11 + 2*(q2)*(q2); -const float HK13 = HK1*HK12 - HK10*HK4 + HK6*HK7; -const float HK14 = 1.0F/(HK13); -const float HK15 = q0*q1 + q2*q3; -const float HK16 = HK11 + 2*(q1)*(q1); -const float HK17 = 2*HK1*(HK8 - HK9) - HK15*HK7 + HK16*HK4; -const float HK18 = HK14*HK17; -const float HK19 = 2*HK14; -const float HK20 = HK19*(HK0 + HK18*(HK3 - HK5) - HK2); -const float HK21 = q0*vd; -const float HK22 = HK1*q2; -const float HK23 = HK4*q1; -const float HK24 = q3*vd; -const float HK25 = -1/HK13; -const float HK26 = -HK17*HK25; -const float HK27 = HK21 + HK22 - 2*HK23 - HK26*(HK24 + HK4*q2); -const float HK28 = 2*HK25; -const float HK29 = HK19*(HK1*q1 + HK18*(HK21 + 2*HK22 - HK23) + HK24); -const float HK30 = HK28*(HK1*q0 + HK26*(HK0 - 2*HK2 + HK4*q0) - HK3 + 2*HK5); -const float HK31 = -2*HK8 + 2*HK9; -const float HK32 = HK12*HK26 + HK31; -const float HK33 = HK25*(HK10*HK26 + HK16); -const float HK34 = HK19*(HK15 + HK18*HK6); -const float HK35 = HK12*HK18 + HK31; -const float HK36 = HK14*(HK10*HK18 + HK16); -const float HK37 = -HK25*HK32; -const float HK38 = -HK27*HK28; -const float HK39 = HK14*HK35*P(0,22) - HK20*P(0,0) - HK29*P(0,2) - HK30*P(0,3) - HK33*P(0,5) - HK34*P(0,6) - HK36*P(0,23) - HK37*P(0,4) - HK38*P(0,1); -const float HK40 = HK14*HK35*P(6,22) - HK20*P(0,6) - HK29*P(2,6) - HK30*P(3,6) - HK33*P(5,6) - HK34*P(6,6) - HK36*P(6,23) - HK37*P(4,6) - HK38*P(1,6); -const float HK41 = HK14*HK35*P(22,23) - HK20*P(0,23) - HK29*P(2,23) - HK30*P(3,23) - HK33*P(5,23) - HK34*P(6,23) - HK36*P(23,23) - HK37*P(4,23) - HK38*P(1,23); -const float HK42 = HK14*HK35*P(22,22) - HK20*P(0,22) - HK29*P(2,22) - HK30*P(3,22) - HK33*P(5,22) - HK34*P(6,22) - HK36*P(22,23) - HK37*P(4,22) - HK38*P(1,22); -const float HK43 = HK14*HK35*P(5,22) - HK20*P(0,5) - HK29*P(2,5) - HK30*P(3,5) - HK33*P(5,5) - HK34*P(5,6) - HK36*P(5,23) - HK37*P(4,5) - HK38*P(1,5); -const float HK44 = HK14*HK35*P(4,22) - HK20*P(0,4) - HK29*P(2,4) - HK30*P(3,4) - HK33*P(4,5) - HK34*P(4,6) - HK36*P(4,23) - HK37*P(4,4) - HK38*P(1,4); -const float HK45 = HK14*HK35*P(2,22) - HK20*P(0,2) - HK29*P(2,2) - HK30*P(2,3) - HK33*P(2,5) - HK34*P(2,6) - HK36*P(2,23) - HK37*P(2,4) - HK38*P(1,2); -const float HK46 = HK14*HK35*P(1,22) - HK20*P(0,1) - HK29*P(1,2) - HK30*P(1,3) - HK33*P(1,5) - HK34*P(1,6) - HK36*P(1,23) - HK37*P(1,4) - HK38*P(1,1); -const float HK47 = HK14*HK35*P(3,22) - HK20*P(0,3) - HK29*P(2,3) - HK30*P(3,3) - HK33*P(3,5) - HK34*P(3,6) - HK36*P(3,23) - HK37*P(3,4) - HK38*P(1,3); -const float HK48 = 1.0F/(HK14*HK35*HK42 - HK20*HK39 - HK29*HK45 - HK30*HK47 - HK33*HK43 - HK34*HK40 - HK36*HK41 - HK37*HK44 - HK38*HK46 + R_BETA); +const float HK0 = vn - vwn; +const float HK1 = ve - vwe; +const float HK2 = HK0*q0 + HK1*q3 - q2*vd; +const float HK3 = q0*q2 - q1*q3; +const float HK4 = 2*vd; +const float HK5 = q0*q3; +const float HK6 = q1*q2; +const float HK7 = 2*HK5 + 2*HK6; +const float HK8 = (q0)*(q0); +const float HK9 = (q3)*(q3); +const float HK10 = HK8 - HK9; +const float HK11 = (q1)*(q1); +const float HK12 = (q2)*(q2); +const float HK13 = HK11 - HK12; +const float HK14 = HK10 + HK13; +const float HK15 = HK0*HK14 + HK1*HK7 - HK3*HK4; +const float HK16 = 1.0F/(HK15); +const float HK17 = q0*q1 + q2*q3; +const float HK18 = HK16*(-2*HK0*(HK5 - HK6) + HK1*(HK10 - HK11 + HK12) + HK17*HK4); +const float HK19 = -HK0*q3 + HK1*q0 + q1*vd; +const float HK20 = -HK18*HK2 + HK19; +const float HK21 = 2*HK16; +const float HK22 = HK0*q1 + HK1*q2 + q3*vd; +const float HK23 = HK0*q2 - HK1*q1 + q0*vd; +const float HK24 = -HK18*HK22 + HK23; +const float HK25 = HK18*HK23 + HK22; +const float HK26 = HK18*HK19 + HK2; +const float HK27 = HK14*HK18 + 2*HK5 - 2*HK6; +const float HK28 = HK16*HK27; +const float HK29 = HK13 + HK18*HK7 - HK8 + HK9; +const float HK30 = HK17 + HK18*HK3; +const float HK31 = 2*HK30; +const float HK32 = 2*HK25; +const float HK33 = 2*HK24; +const float HK34 = 2*HK26; +const float HK35 = 2*HK20; +const float HK36 = HK27*P(0,22) - HK27*P(0,4) + HK29*P(0,23) - HK29*P(0,5) + HK31*P(0,6) + HK32*P(0,2) + HK33*P(0,1) - HK34*P(0,3) + HK35*P(0,0); +const float HK37 = 1.0F/((HK15)*(HK15)); +const float HK38 = -HK27*P(4,6) + HK27*P(6,22) - HK29*P(5,6) + HK29*P(6,23) + HK31*P(6,6) + HK32*P(2,6) + HK33*P(1,6) - HK34*P(3,6) + HK35*P(0,6); +const float HK39 = HK29*P(5,23); +const float HK40 = HK27*P(22,23) - HK27*P(4,23) + HK29*P(23,23) + HK31*P(6,23) + HK32*P(2,23) + HK33*P(1,23) - HK34*P(3,23) + HK35*P(0,23) - HK39; +const float HK41 = HK29*HK37; +const float HK42 = HK27*P(4,22); +const float HK43 = HK27*P(22,22) + HK29*P(22,23) - HK29*P(5,22) + HK31*P(6,22) + HK32*P(2,22) + HK33*P(1,22) - HK34*P(3,22) + HK35*P(0,22) - HK42; +const float HK44 = HK27*HK37; +const float HK45 = -HK27*P(4,5) + HK27*P(5,22) - HK29*P(5,5) + HK31*P(5,6) + HK32*P(2,5) + HK33*P(1,5) - HK34*P(3,5) + HK35*P(0,5) + HK39; +const float HK46 = -HK27*P(4,4) + HK29*P(4,23) - HK29*P(4,5) + HK31*P(4,6) + HK32*P(2,4) + HK33*P(1,4) - HK34*P(3,4) + HK35*P(0,4) + HK42; +const float HK47 = HK27*P(2,22) - HK27*P(2,4) + HK29*P(2,23) - HK29*P(2,5) + HK31*P(2,6) + HK32*P(2,2) + HK33*P(1,2) - HK34*P(2,3) + HK35*P(0,2); +const float HK48 = HK27*P(1,22) - HK27*P(1,4) + HK29*P(1,23) - HK29*P(1,5) + HK31*P(1,6) + HK32*P(1,2) + HK33*P(1,1) - HK34*P(1,3) + HK35*P(0,1); +const float HK49 = HK27*P(3,22) - HK27*P(3,4) + HK29*P(3,23) - HK29*P(3,5) + HK31*P(3,6) + HK32*P(2,3) + HK33*P(1,3) - HK34*P(3,3) + HK35*P(0,3); +const float HK50 = HK16/(HK31*HK37*HK38 + HK32*HK37*HK47 + HK33*HK37*HK48 - HK34*HK37*HK49 + HK35*HK36*HK37 + HK40*HK41 - HK41*HK45 + HK43*HK44 - HK44*HK46 + R_BETA); // Observation Jacobians -Hfusion.at<0>() = -HK20; -Hfusion.at<1>() = HK27*HK28; -Hfusion.at<2>() = -HK29; -Hfusion.at<3>() = -HK30; -Hfusion.at<4>() = HK25*HK32; -Hfusion.at<5>() = -HK33; -Hfusion.at<6>() = -HK34; -Hfusion.at<22>() = HK14*HK35; -Hfusion.at<23>() = -HK36; +Hfusion.at<0>() = HK20*HK21; +Hfusion.at<1>() = HK21*HK24; +Hfusion.at<2>() = HK21*HK25; +Hfusion.at<3>() = -HK21*HK26; +Hfusion.at<4>() = -HK28; +Hfusion.at<5>() = -HK16*HK29; +Hfusion.at<6>() = HK21*HK30; +Hfusion.at<22>() = HK28; +Hfusion.at<23>() = HK16*HK29; // Kalman gains -Kfusion(0) = HK39*HK48; -Kfusion(1) = HK46*HK48; -Kfusion(2) = HK45*HK48; -Kfusion(3) = HK47*HK48; -Kfusion(4) = HK44*HK48; -Kfusion(5) = HK43*HK48; -Kfusion(6) = HK40*HK48; -Kfusion(7) = HK48*(HK14*HK35*P(7,22) - HK20*P(0,7) - HK29*P(2,7) - HK30*P(3,7) - HK33*P(5,7) - HK34*P(6,7) - HK36*P(7,23) - HK37*P(4,7) - HK38*P(1,7)); -Kfusion(8) = HK48*(HK14*HK35*P(8,22) - HK20*P(0,8) - HK29*P(2,8) - HK30*P(3,8) - HK33*P(5,8) - HK34*P(6,8) - HK36*P(8,23) - HK37*P(4,8) - HK38*P(1,8)); -Kfusion(9) = HK48*(HK14*HK35*P(9,22) - HK20*P(0,9) - HK29*P(2,9) - HK30*P(3,9) - HK33*P(5,9) - HK34*P(6,9) - HK36*P(9,23) - HK37*P(4,9) - HK38*P(1,9)); -Kfusion(10) = HK48*(HK14*HK35*P(10,22) - HK20*P(0,10) - HK29*P(2,10) - HK30*P(3,10) - HK33*P(5,10) - HK34*P(6,10) - HK36*P(10,23) - HK37*P(4,10) - HK38*P(1,10)); -Kfusion(11) = HK48*(HK14*HK35*P(11,22) - HK20*P(0,11) - HK29*P(2,11) - HK30*P(3,11) - HK33*P(5,11) - HK34*P(6,11) - HK36*P(11,23) - HK37*P(4,11) - HK38*P(1,11)); -Kfusion(12) = HK48*(HK14*HK35*P(12,22) - HK20*P(0,12) - HK29*P(2,12) - HK30*P(3,12) - HK33*P(5,12) - HK34*P(6,12) - HK36*P(12,23) - HK37*P(4,12) - HK38*P(1,12)); -Kfusion(13) = HK48*(HK14*HK35*P(13,22) - HK20*P(0,13) - HK29*P(2,13) - HK30*P(3,13) - HK33*P(5,13) - HK34*P(6,13) - HK36*P(13,23) - HK37*P(4,13) - HK38*P(1,13)); -Kfusion(14) = HK48*(HK14*HK35*P(14,22) - HK20*P(0,14) - HK29*P(2,14) - HK30*P(3,14) - HK33*P(5,14) - HK34*P(6,14) - HK36*P(14,23) - HK37*P(4,14) - HK38*P(1,14)); -Kfusion(15) = HK48*(HK14*HK35*P(15,22) - HK20*P(0,15) - HK29*P(2,15) - HK30*P(3,15) - HK33*P(5,15) - HK34*P(6,15) - HK36*P(15,23) - HK37*P(4,15) - HK38*P(1,15)); -Kfusion(16) = HK48*(HK14*HK35*P(16,22) - HK20*P(0,16) - HK29*P(2,16) - HK30*P(3,16) - HK33*P(5,16) - HK34*P(6,16) - HK36*P(16,23) - HK37*P(4,16) - HK38*P(1,16)); -Kfusion(17) = HK48*(HK14*HK35*P(17,22) - HK20*P(0,17) - HK29*P(2,17) - HK30*P(3,17) - HK33*P(5,17) - HK34*P(6,17) - HK36*P(17,23) - HK37*P(4,17) - HK38*P(1,17)); -Kfusion(18) = HK48*(HK14*HK35*P(18,22) - HK20*P(0,18) - HK29*P(2,18) - HK30*P(3,18) - HK33*P(5,18) - HK34*P(6,18) - HK36*P(18,23) - HK37*P(4,18) - HK38*P(1,18)); -Kfusion(19) = HK48*(HK14*HK35*P(19,22) - HK20*P(0,19) - HK29*P(2,19) - HK30*P(3,19) - HK33*P(5,19) - HK34*P(6,19) - HK36*P(19,23) - HK37*P(4,19) - HK38*P(1,19)); -Kfusion(20) = HK48*(HK14*HK35*P(20,22) - HK20*P(0,20) - HK29*P(2,20) - HK30*P(3,20) - HK33*P(5,20) - HK34*P(6,20) - HK36*P(20,23) - HK37*P(4,20) - HK38*P(1,20)); -Kfusion(21) = HK48*(HK14*HK35*P(21,22) - HK20*P(0,21) - HK29*P(2,21) - HK30*P(3,21) - HK33*P(5,21) - HK34*P(6,21) - HK36*P(21,23) - HK37*P(4,21) - HK38*P(1,21)); -Kfusion(22) = HK42*HK48; -Kfusion(23) = HK41*HK48; +Kfusion(0) = HK36*HK50; +Kfusion(1) = HK48*HK50; +Kfusion(2) = HK47*HK50; +Kfusion(3) = HK49*HK50; +Kfusion(4) = HK46*HK50; +Kfusion(5) = HK45*HK50; +Kfusion(6) = HK38*HK50; +Kfusion(7) = HK50*(-HK27*P(4,7) + HK27*P(7,22) - HK29*P(5,7) + HK29*P(7,23) + HK31*P(6,7) + HK32*P(2,7) + HK33*P(1,7) - HK34*P(3,7) + HK35*P(0,7)); +Kfusion(8) = HK50*(-HK27*P(4,8) + HK27*P(8,22) - HK29*P(5,8) + HK29*P(8,23) + HK31*P(6,8) + HK32*P(2,8) + HK33*P(1,8) - HK34*P(3,8) + HK35*P(0,8)); +Kfusion(9) = HK50*(-HK27*P(4,9) + HK27*P(9,22) - HK29*P(5,9) + HK29*P(9,23) + HK31*P(6,9) + HK32*P(2,9) + HK33*P(1,9) - HK34*P(3,9) + HK35*P(0,9)); +Kfusion(10) = HK50*(HK27*P(10,22) - HK27*P(4,10) + HK29*P(10,23) - HK29*P(5,10) + HK31*P(6,10) + HK32*P(2,10) + HK33*P(1,10) - HK34*P(3,10) + HK35*P(0,10)); +Kfusion(11) = HK50*(HK27*P(11,22) - HK27*P(4,11) + HK29*P(11,23) - HK29*P(5,11) + HK31*P(6,11) + HK32*P(2,11) + HK33*P(1,11) - HK34*P(3,11) + HK35*P(0,11)); +Kfusion(12) = HK50*(HK27*P(12,22) - HK27*P(4,12) + HK29*P(12,23) - HK29*P(5,12) + HK31*P(6,12) + HK32*P(2,12) + HK33*P(1,12) - HK34*P(3,12) + HK35*P(0,12)); +Kfusion(13) = HK50*(HK27*P(13,22) - HK27*P(4,13) + HK29*P(13,23) - HK29*P(5,13) + HK31*P(6,13) + HK32*P(2,13) + HK33*P(1,13) - HK34*P(3,13) + HK35*P(0,13)); +Kfusion(14) = HK50*(HK27*P(14,22) - HK27*P(4,14) + HK29*P(14,23) - HK29*P(5,14) + HK31*P(6,14) + HK32*P(2,14) + HK33*P(1,14) - HK34*P(3,14) + HK35*P(0,14)); +Kfusion(15) = HK50*(HK27*P(15,22) - HK27*P(4,15) + HK29*P(15,23) - HK29*P(5,15) + HK31*P(6,15) + HK32*P(2,15) + HK33*P(1,15) - HK34*P(3,15) + HK35*P(0,15)); +Kfusion(16) = HK50*(HK27*P(16,22) - HK27*P(4,16) + HK29*P(16,23) - HK29*P(5,16) + HK31*P(6,16) + HK32*P(2,16) + HK33*P(1,16) - HK34*P(3,16) + HK35*P(0,16)); +Kfusion(17) = HK50*(HK27*P(17,22) - HK27*P(4,17) + HK29*P(17,23) - HK29*P(5,17) + HK31*P(6,17) + HK32*P(2,17) + HK33*P(1,17) - HK34*P(3,17) + HK35*P(0,17)); +Kfusion(18) = HK50*(HK27*P(18,22) - HK27*P(4,18) + HK29*P(18,23) - HK29*P(5,18) + HK31*P(6,18) + HK32*P(2,18) + HK33*P(1,18) - HK34*P(3,18) + HK35*P(0,18)); +Kfusion(19) = HK50*(HK27*P(19,22) - HK27*P(4,19) + HK29*P(19,23) - HK29*P(5,19) + HK31*P(6,19) + HK32*P(2,19) + HK33*P(1,19) - HK34*P(3,19) + HK35*P(0,19)); +Kfusion(20) = HK50*(HK27*P(20,22) - HK27*P(4,20) + HK29*P(20,23) - HK29*P(5,20) + HK31*P(6,20) + HK32*P(2,20) + HK33*P(1,20) - HK34*P(3,20) + HK35*P(0,20)); +Kfusion(21) = HK50*(HK27*P(21,22) - HK27*P(4,21) + HK29*P(21,23) - HK29*P(5,21) + HK31*P(6,21) + HK32*P(2,21) + HK33*P(1,21) - HK34*P(3,21) + HK35*P(0,21)); +Kfusion(22) = HK43*HK50; +Kfusion(23) = HK40*HK50; diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated.cpp index 96370ce1cd..39ca3be12c 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated.cpp @@ -1,187 +1,172 @@ // 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*(q2)*(q2); -const float HK26 = 2*(q3)*(q3) - 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*(q1)*(q1); -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*P(0,1) + 2*HK17*P(0,2) + 2*HK18*P(0,3) - HK27*P(0,4) - HK32*P(0,5) - HK33*P(0,6) - HK34*P(0,0); -const float HK36 = 1.0F/((range)*(range)); -const float HK37 = 2*HK14*P(1,6) + 2*HK17*P(2,6) + 2*HK18*P(3,6) - HK27*P(4,6) - HK32*P(5,6) - HK33*P(6,6) - HK34*P(0,6); -const float HK38 = 2*HK14*P(1,5) + 2*HK17*P(2,5) + 2*HK18*P(3,5) - HK27*P(4,5) - HK32*P(5,5) - HK33*P(5,6) - HK34*P(0,5); -const float HK39 = 2*HK14*P(1,4) + 2*HK17*P(2,4) + 2*HK18*P(3,4) - HK27*P(4,4) - HK32*P(4,5) - HK33*P(4,6) - HK34*P(0,4); -const float HK40 = 2*HK14*P(1,3) + 2*HK17*P(2,3) + 2*HK18*P(3,3) - HK27*P(3,4) - HK32*P(3,5) - HK33*P(3,6) - HK34*P(0,3); -const float HK41 = 2*HK14*P(1,2) + 2*HK17*P(2,2) + 2*HK18*P(2,3) - HK27*P(2,4) - HK32*P(2,5) - HK33*P(2,6) - HK34*P(0,2); -const float HK42 = 2*HK14*P(1,1) + 2*HK17*P(1,2) + 2*HK18*P(1,3) - HK27*P(1,4) - HK32*P(1,5) - HK33*P(1,6) - HK34*P(0,1); -const float HK43 = HK8/(2*HK14*HK36*HK42 + 2*HK17*HK36*HK41 + 2*HK18*HK36*HK40 - HK27*HK36*HK39 - HK32*HK36*HK38 - HK33*HK36*HK37 - HK34*HK35*HK36 + R_LOS); +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 = 2*Tbs(1,2); +const float HK13 = q0*q3; +const float HK14 = q1*q2; +const float HK15 = 2*Tbs(1,1); +const float HK16 = (q1)*(q1); +const float HK17 = (q2)*(q2); +const float HK18 = -HK17; +const float HK19 = (q0)*(q0); +const float HK20 = (q3)*(q3); +const float HK21 = HK19 - HK20; +const float HK22 = HK12*(HK10 + HK11) - HK15*(HK13 - HK14) + Tbs(1,0)*(HK16 + HK18 + HK21); +const float HK23 = 2*Tbs(1,0); +const float HK24 = q0*q1; +const float HK25 = q2*q3; +const float HK26 = -HK16; +const float HK27 = -HK12*(HK24 - HK25) + HK23*(HK13 + HK14) + Tbs(1,1)*(HK17 + HK21 + HK26); +const float HK28 = HK15*(HK24 + HK25) - HK23*(HK10 - HK11) + Tbs(1,2)*(HK18 + HK19 + HK20 + HK26); +const float HK29 = 2*HK3; +const float HK30 = 2*HK7; +const float HK31 = 2*HK8; +const float HK32 = 2*HK9; +const float HK33 = HK22*P(0,4) + HK27*P(0,5) + HK28*P(0,6) + HK29*P(0,0) + HK30*P(0,1) + HK31*P(0,2) + HK32*P(0,3); +const float HK34 = 1.0F/((range)*(range)); +const float HK35 = HK22*P(4,6) + HK27*P(5,6) + HK28*P(6,6) + HK29*P(0,6) + HK30*P(1,6) + HK31*P(2,6) + HK32*P(3,6); +const float HK36 = HK22*P(4,5) + HK27*P(5,5) + HK28*P(5,6) + HK29*P(0,5) + HK30*P(1,5) + HK31*P(2,5) + HK32*P(3,5); +const float HK37 = HK22*P(4,4) + HK27*P(4,5) + HK28*P(4,6) + HK29*P(0,4) + HK30*P(1,4) + HK31*P(2,4) + HK32*P(3,4); +const float HK38 = HK22*P(2,4) + HK27*P(2,5) + HK28*P(2,6) + HK29*P(0,2) + HK30*P(1,2) + HK31*P(2,2) + HK32*P(2,3); +const float HK39 = HK22*P(3,4) + HK27*P(3,5) + HK28*P(3,6) + HK29*P(0,3) + HK30*P(1,3) + HK31*P(2,3) + HK32*P(3,3); +const float HK40 = HK22*P(1,4) + HK27*P(1,5) + HK28*P(1,6) + HK29*P(0,1) + HK30*P(1,1) + HK31*P(1,2) + HK32*P(1,3); +const float HK41 = HK4/(HK22*HK34*HK37 + HK27*HK34*HK36 + HK28*HK34*HK35 + HK29*HK33*HK34 + HK30*HK34*HK40 + HK31*HK34*HK38 + HK32*HK34*HK39 + 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<0>() = HK3*HK5; +Hfusion.at<1>() = HK5*HK7; +Hfusion.at<2>() = HK5*HK8; +Hfusion.at<3>() = HK5*HK9; +Hfusion.at<4>() = HK22*HK4; +Hfusion.at<5>() = HK27*HK4; +Hfusion.at<6>() = HK28*HK4; // Kalman gains -Kfusion(0) = HK35*HK43; -Kfusion(1) = HK42*HK43; -Kfusion(2) = HK41*HK43; -Kfusion(3) = HK40*HK43; -Kfusion(4) = HK39*HK43; -Kfusion(5) = HK38*HK43; -Kfusion(6) = HK37*HK43; -Kfusion(7) = HK43*(2*HK14*P(1,7) + 2*HK17*P(2,7) + 2*HK18*P(3,7) - HK27*P(4,7) - HK32*P(5,7) - HK33*P(6,7) - HK34*P(0,7)); -Kfusion(8) = HK43*(2*HK14*P(1,8) + 2*HK17*P(2,8) + 2*HK18*P(3,8) - HK27*P(4,8) - HK32*P(5,8) - HK33*P(6,8) - HK34*P(0,8)); -Kfusion(9) = HK43*(2*HK14*P(1,9) + 2*HK17*P(2,9) + 2*HK18*P(3,9) - HK27*P(4,9) - HK32*P(5,9) - HK33*P(6,9) - HK34*P(0,9)); -Kfusion(10) = HK43*(2*HK14*P(1,10) + 2*HK17*P(2,10) + 2*HK18*P(3,10) - HK27*P(4,10) - HK32*P(5,10) - HK33*P(6,10) - HK34*P(0,10)); -Kfusion(11) = HK43*(2*HK14*P(1,11) + 2*HK17*P(2,11) + 2*HK18*P(3,11) - HK27*P(4,11) - HK32*P(5,11) - HK33*P(6,11) - HK34*P(0,11)); -Kfusion(12) = HK43*(2*HK14*P(1,12) + 2*HK17*P(2,12) + 2*HK18*P(3,12) - HK27*P(4,12) - HK32*P(5,12) - HK33*P(6,12) - HK34*P(0,12)); -Kfusion(13) = HK43*(2*HK14*P(1,13) + 2*HK17*P(2,13) + 2*HK18*P(3,13) - HK27*P(4,13) - HK32*P(5,13) - HK33*P(6,13) - HK34*P(0,13)); -Kfusion(14) = HK43*(2*HK14*P(1,14) + 2*HK17*P(2,14) + 2*HK18*P(3,14) - HK27*P(4,14) - HK32*P(5,14) - HK33*P(6,14) - HK34*P(0,14)); -Kfusion(15) = HK43*(2*HK14*P(1,15) + 2*HK17*P(2,15) + 2*HK18*P(3,15) - HK27*P(4,15) - HK32*P(5,15) - HK33*P(6,15) - HK34*P(0,15)); -Kfusion(16) = HK43*(2*HK14*P(1,16) + 2*HK17*P(2,16) + 2*HK18*P(3,16) - HK27*P(4,16) - HK32*P(5,16) - HK33*P(6,16) - HK34*P(0,16)); -Kfusion(17) = HK43*(2*HK14*P(1,17) + 2*HK17*P(2,17) + 2*HK18*P(3,17) - HK27*P(4,17) - HK32*P(5,17) - HK33*P(6,17) - HK34*P(0,17)); -Kfusion(18) = HK43*(2*HK14*P(1,18) + 2*HK17*P(2,18) + 2*HK18*P(3,18) - HK27*P(4,18) - HK32*P(5,18) - HK33*P(6,18) - HK34*P(0,18)); -Kfusion(19) = HK43*(2*HK14*P(1,19) + 2*HK17*P(2,19) + 2*HK18*P(3,19) - HK27*P(4,19) - HK32*P(5,19) - HK33*P(6,19) - HK34*P(0,19)); -Kfusion(20) = HK43*(2*HK14*P(1,20) + 2*HK17*P(2,20) + 2*HK18*P(3,20) - HK27*P(4,20) - HK32*P(5,20) - HK33*P(6,20) - HK34*P(0,20)); -Kfusion(21) = HK43*(2*HK14*P(1,21) + 2*HK17*P(2,21) + 2*HK18*P(3,21) - HK27*P(4,21) - HK32*P(5,21) - HK33*P(6,21) - HK34*P(0,21)); -Kfusion(22) = HK43*(2*HK14*P(1,22) + 2*HK17*P(2,22) + 2*HK18*P(3,22) - HK27*P(4,22) - HK32*P(5,22) - HK33*P(6,22) - HK34*P(0,22)); -Kfusion(23) = HK43*(2*HK14*P(1,23) + 2*HK17*P(2,23) + 2*HK18*P(3,23) - HK27*P(4,23) - HK32*P(5,23) - HK33*P(6,23) - HK34*P(0,23)); +Kfusion(0) = HK33*HK41; +Kfusion(1) = HK40*HK41; +Kfusion(2) = HK38*HK41; +Kfusion(3) = HK39*HK41; +Kfusion(4) = HK37*HK41; +Kfusion(5) = HK36*HK41; +Kfusion(6) = HK35*HK41; +Kfusion(7) = HK41*(HK22*P(4,7) + HK27*P(5,7) + HK28*P(6,7) + HK29*P(0,7) + HK30*P(1,7) + HK31*P(2,7) + HK32*P(3,7)); +Kfusion(8) = HK41*(HK22*P(4,8) + HK27*P(5,8) + HK28*P(6,8) + HK29*P(0,8) + HK30*P(1,8) + HK31*P(2,8) + HK32*P(3,8)); +Kfusion(9) = HK41*(HK22*P(4,9) + HK27*P(5,9) + HK28*P(6,9) + HK29*P(0,9) + HK30*P(1,9) + HK31*P(2,9) + HK32*P(3,9)); +Kfusion(10) = HK41*(HK22*P(4,10) + HK27*P(5,10) + HK28*P(6,10) + HK29*P(0,10) + HK30*P(1,10) + HK31*P(2,10) + HK32*P(3,10)); +Kfusion(11) = HK41*(HK22*P(4,11) + HK27*P(5,11) + HK28*P(6,11) + HK29*P(0,11) + HK30*P(1,11) + HK31*P(2,11) + HK32*P(3,11)); +Kfusion(12) = HK41*(HK22*P(4,12) + HK27*P(5,12) + HK28*P(6,12) + HK29*P(0,12) + HK30*P(1,12) + HK31*P(2,12) + HK32*P(3,12)); +Kfusion(13) = HK41*(HK22*P(4,13) + HK27*P(5,13) + HK28*P(6,13) + HK29*P(0,13) + HK30*P(1,13) + HK31*P(2,13) + HK32*P(3,13)); +Kfusion(14) = HK41*(HK22*P(4,14) + HK27*P(5,14) + HK28*P(6,14) + HK29*P(0,14) + HK30*P(1,14) + HK31*P(2,14) + HK32*P(3,14)); +Kfusion(15) = HK41*(HK22*P(4,15) + HK27*P(5,15) + HK28*P(6,15) + HK29*P(0,15) + HK30*P(1,15) + HK31*P(2,15) + HK32*P(3,15)); +Kfusion(16) = HK41*(HK22*P(4,16) + HK27*P(5,16) + HK28*P(6,16) + HK29*P(0,16) + HK30*P(1,16) + HK31*P(2,16) + HK32*P(3,16)); +Kfusion(17) = HK41*(HK22*P(4,17) + HK27*P(5,17) + HK28*P(6,17) + HK29*P(0,17) + HK30*P(1,17) + HK31*P(2,17) + HK32*P(3,17)); +Kfusion(18) = HK41*(HK22*P(4,18) + HK27*P(5,18) + HK28*P(6,18) + HK29*P(0,18) + HK30*P(1,18) + HK31*P(2,18) + HK32*P(3,18)); +Kfusion(19) = HK41*(HK22*P(4,19) + HK27*P(5,19) + HK28*P(6,19) + HK29*P(0,19) + HK30*P(1,19) + HK31*P(2,19) + HK32*P(3,19)); +Kfusion(20) = HK41*(HK22*P(4,20) + HK27*P(5,20) + HK28*P(6,20) + HK29*P(0,20) + HK30*P(1,20) + HK31*P(2,20) + HK32*P(3,20)); +Kfusion(21) = HK41*(HK22*P(4,21) + HK27*P(5,21) + HK28*P(6,21) + HK29*P(0,21) + HK30*P(1,21) + HK31*P(2,21) + HK32*P(3,21)); +Kfusion(22) = HK41*(HK22*P(4,22) + HK27*P(5,22) + HK28*P(6,22) + HK29*P(0,22) + HK30*P(1,22) + HK31*P(2,22) + HK32*P(3,22)); +Kfusion(23) = HK41*(HK22*P(4,23) + HK27*P(5,23) + HK28*P(6,23) + HK29*P(0,23) + HK30*P(1,23) + HK31*P(2,23) + HK32*P(3,23)); // Y Axis Equations // Sub Expressions -const float HK0 = Tbs(0,0)*q2; -const float HK1 = Tbs(0,1)*q1; -const float HK2 = HK0 - HK1; -const float HK3 = Tbs(0,1)*q3; -const float HK4 = Tbs(0,2)*q2; -const float HK5 = HK3 - HK4; -const float HK6 = Tbs(0,0)*q3; -const float HK7 = Tbs(0,2)*q1; -const float HK8 = -HK7; -const float HK9 = HK6 + HK8; -const float HK10 = 1.0F/(range); -const float HK11 = 2*HK10; -const float HK12 = Tbs(0,2)*q0; -const float HK13 = -HK0 + 2*HK1 + HK12; -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*(HK14 + HK6 - 2*HK7) + vn*(HK15 + HK16); -const float HK18 = Tbs(0,0)*q0; -const float HK19 = HK18 - HK3 + 2*HK4; -const float HK20 = Tbs(0,0)*q1; -const float HK21 = ve*(HK16 + HK20) + vn*(-2*HK0 + HK1 + HK12); -const float HK22 = HK14 + 2*HK6 + HK8; -const float HK23 = vd*(HK15 + HK20) + ve*(HK18 - 2*HK3 + HK4); -const float HK24 = q0*q2; -const float HK25 = q1*q3; -const float HK26 = HK24 + HK25; -const float HK27 = q0*q3; -const float HK28 = q1*q2; -const float HK29 = HK27 - HK28; -const float HK30 = 2*Tbs(0,1); -const float HK31 = 2*(q2)*(q2); -const float HK32 = 2*(q3)*(q3) - 1; -const float HK33 = HK31 + HK32; -const float HK34 = HK27 + HK28; -const float HK35 = q0*q1; -const float HK36 = q2*q3; -const float HK37 = HK35 - HK36; -const float HK38 = 2*Tbs(0,2); -const float HK39 = 2*(q1)*(q1); -const float HK40 = HK32 + HK39; -const float HK41 = HK35 + HK36; -const float HK42 = HK24 - HK25; -const float HK43 = 2*Tbs(0,0); -const float HK44 = HK31 + HK39 - 1; -const float HK45 = -2*HK2*vd - 2*HK5*vn + 2*HK9*ve; -const float HK46 = HK26*HK38 - HK29*HK30 - HK33*Tbs(0,0); -const float HK47 = HK34*HK43 - HK37*HK38 - HK40*Tbs(0,1); -const float HK48 = HK30*HK41 - HK42*HK43 - HK44*Tbs(0,2); -const float HK49 = -2*HK13*ve + 2*HK17; -const float HK50 = -2*HK19*vd + 2*HK21; -const float HK51 = -2*HK22*vn + 2*HK23; -const float HK52 = HK45*P(0,0) + HK46*P(0,4) + HK47*P(0,5) + HK48*P(0,6) + HK49*P(0,1) + HK50*P(0,2) + HK51*P(0,3); -const float HK53 = 1.0F/((range)*(range)); -const float HK54 = HK45*P(0,6) + HK46*P(4,6) + HK47*P(5,6) + HK48*P(6,6) + HK49*P(1,6) + HK50*P(2,6) + HK51*P(3,6); -const float HK55 = HK45*P(0,5) + HK46*P(4,5) + HK47*P(5,5) + HK48*P(5,6) + HK49*P(1,5) + HK50*P(2,5) + HK51*P(3,5); -const float HK56 = HK45*P(0,4) + HK46*P(4,4) + HK47*P(4,5) + HK48*P(4,6) + HK49*P(1,4) + HK50*P(2,4) + HK51*P(3,4); -const float HK57 = HK45*P(0,3) + HK46*P(3,4) + HK47*P(3,5) + HK48*P(3,6) + HK49*P(1,3) + HK50*P(2,3) + HK51*P(3,3); -const float HK58 = HK45*P(0,2) + HK46*P(2,4) + HK47*P(2,5) + HK48*P(2,6) + HK49*P(1,2) + HK50*P(2,2) + HK51*P(2,3); -const float HK59 = HK45*P(0,1) + HK46*P(1,4) + HK47*P(1,5) + HK48*P(1,6) + HK49*P(1,1) + HK50*P(1,2) + HK51*P(1,3); -const float HK60 = HK10/(HK45*HK52*HK53 + HK46*HK53*HK56 + HK47*HK53*HK55 + HK48*HK53*HK54 + HK49*HK53*HK59 + HK50*HK53*HK58 + HK51*HK53*HK57 + R_LOS); +const float HK0 = -Tbs(0,0)*q2 + Tbs(0,1)*q1 + Tbs(0,2)*q0; +const float HK1 = Tbs(0,0)*q3 + Tbs(0,1)*q0 - Tbs(0,2)*q1; +const float HK2 = Tbs(0,0)*q0 - Tbs(0,1)*q3 + Tbs(0,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(0,0)*q1 + Tbs(0,1)*q2 + Tbs(0,2)*q3; +const float HK7 = HK1*vd + HK6*vn; +const float HK8 = HK0*vn + HK6*ve; +const float HK9 = HK2*ve + HK6*vd; +const float HK10 = q0*q3; +const float HK11 = q1*q2; +const float HK12 = HK10 - HK11; +const float HK13 = 2*Tbs(0,1); +const float HK14 = (q1)*(q1); +const float HK15 = (q2)*(q2); +const float HK16 = -HK15; +const float HK17 = (q0)*(q0); +const float HK18 = (q3)*(q3); +const float HK19 = HK17 - HK18; +const float HK20 = q0*q2; +const float HK21 = q1*q3; +const float HK22 = 2*Tbs(0,2); +const float HK23 = HK22*(HK20 + HK21) + Tbs(0,0)*(HK14 + HK16 + HK19); +const float HK24 = q0*q1; +const float HK25 = q2*q3; +const float HK26 = HK24 - HK25; +const float HK27 = -HK14; +const float HK28 = 2*Tbs(0,0); +const float HK29 = HK28*(HK10 + HK11) + Tbs(0,1)*(HK15 + HK19 + HK27); +const float HK30 = HK20 - HK21; +const float HK31 = HK13*(HK24 + HK25) + Tbs(0,2)*(HK16 + HK17 + HK18 + HK27); +const float HK32 = 2*HK3; +const float HK33 = -2*HK0*ve + 2*HK7; +const float HK34 = -2*HK2*vd + 2*HK8; +const float HK35 = -2*HK1*vn + 2*HK9; +const float HK36 = -HK12*HK13 + HK23; +const float HK37 = -HK22*HK26 + HK29; +const float HK38 = -HK28*HK30 + HK31; +const float HK39 = HK32*P(0,0) + HK33*P(0,1) + HK34*P(0,2) + HK35*P(0,3) + HK36*P(0,4) + HK37*P(0,5) + HK38*P(0,6); +const float HK40 = 1.0F/((range)*(range)); +const float HK41 = HK32*P(0,6) + HK33*P(1,6) + HK34*P(2,6) + HK35*P(3,6) + HK36*P(4,6) + HK37*P(5,6) + HK38*P(6,6); +const float HK42 = HK32*P(0,5) + HK33*P(1,5) + HK34*P(2,5) + HK35*P(3,5) + HK36*P(4,5) + HK37*P(5,5) + HK38*P(5,6); +const float HK43 = HK32*P(0,4) + HK33*P(1,4) + HK34*P(2,4) + HK35*P(3,4) + HK36*P(4,4) + HK37*P(4,5) + HK38*P(4,6); +const float HK44 = HK32*P(0,2) + HK33*P(1,2) + HK34*P(2,2) + HK35*P(2,3) + HK36*P(2,4) + HK37*P(2,5) + HK38*P(2,6); +const float HK45 = HK32*P(0,3) + HK33*P(1,3) + HK34*P(2,3) + HK35*P(3,3) + HK36*P(3,4) + HK37*P(3,5) + HK38*P(3,6); +const float HK46 = HK32*P(0,1) + HK33*P(1,1) + HK34*P(1,2) + HK35*P(1,3) + HK36*P(1,4) + HK37*P(1,5) + HK38*P(1,6); +const float HK47 = HK4/(HK32*HK39*HK40 + HK33*HK40*HK46 + HK34*HK40*HK44 + HK35*HK40*HK45 + HK36*HK40*HK43 + HK37*HK40*HK42 + HK38*HK40*HK41 + R_LOS); // Observation Jacobians -Hfusion.at<0>() = -HK11*(-HK2*vd - HK5*vn + HK9*ve); -Hfusion.at<1>() = -HK11*(-HK13*ve + HK17); -Hfusion.at<2>() = -HK11*(-HK19*vd + HK21); -Hfusion.at<3>() = -HK11*(-HK22*vn + HK23); -Hfusion.at<4>() = -HK10*(2*HK26*Tbs(0,2) - HK29*HK30 - HK33*Tbs(0,0)); -Hfusion.at<5>() = -HK10*(2*HK34*Tbs(0,0) - HK37*HK38 - HK40*Tbs(0,1)); -Hfusion.at<6>() = -HK10*(2*HK41*Tbs(0,1) - HK42*HK43 - HK44*Tbs(0,2)); +Hfusion.at<0>() = -HK3*HK5; +Hfusion.at<1>() = -HK5*(-HK0*ve + HK7); +Hfusion.at<2>() = -HK5*(-HK2*vd + HK8); +Hfusion.at<3>() = -HK5*(-HK1*vn + HK9); +Hfusion.at<4>() = -HK4*(-HK12*HK13 + HK23); +Hfusion.at<5>() = -HK4*(-HK22*HK26 + HK29); +Hfusion.at<6>() = -HK4*(-HK28*HK30 + HK31); // Kalman gains -Kfusion(0) = -HK52*HK60; -Kfusion(1) = -HK59*HK60; -Kfusion(2) = -HK58*HK60; -Kfusion(3) = -HK57*HK60; -Kfusion(4) = -HK56*HK60; -Kfusion(5) = -HK55*HK60; -Kfusion(6) = -HK54*HK60; -Kfusion(7) = -HK60*(HK45*P(0,7) + HK46*P(4,7) + HK47*P(5,7) + HK48*P(6,7) + HK49*P(1,7) + HK50*P(2,7) + HK51*P(3,7)); -Kfusion(8) = -HK60*(HK45*P(0,8) + HK46*P(4,8) + HK47*P(5,8) + HK48*P(6,8) + HK49*P(1,8) + HK50*P(2,8) + HK51*P(3,8)); -Kfusion(9) = -HK60*(HK45*P(0,9) + HK46*P(4,9) + HK47*P(5,9) + HK48*P(6,9) + HK49*P(1,9) + HK50*P(2,9) + HK51*P(3,9)); -Kfusion(10) = -HK60*(HK45*P(0,10) + HK46*P(4,10) + HK47*P(5,10) + HK48*P(6,10) + HK49*P(1,10) + HK50*P(2,10) + HK51*P(3,10)); -Kfusion(11) = -HK60*(HK45*P(0,11) + HK46*P(4,11) + HK47*P(5,11) + HK48*P(6,11) + HK49*P(1,11) + HK50*P(2,11) + HK51*P(3,11)); -Kfusion(12) = -HK60*(HK45*P(0,12) + HK46*P(4,12) + HK47*P(5,12) + HK48*P(6,12) + HK49*P(1,12) + HK50*P(2,12) + HK51*P(3,12)); -Kfusion(13) = -HK60*(HK45*P(0,13) + HK46*P(4,13) + HK47*P(5,13) + HK48*P(6,13) + HK49*P(1,13) + HK50*P(2,13) + HK51*P(3,13)); -Kfusion(14) = -HK60*(HK45*P(0,14) + HK46*P(4,14) + HK47*P(5,14) + HK48*P(6,14) + HK49*P(1,14) + HK50*P(2,14) + HK51*P(3,14)); -Kfusion(15) = -HK60*(HK45*P(0,15) + HK46*P(4,15) + HK47*P(5,15) + HK48*P(6,15) + HK49*P(1,15) + HK50*P(2,15) + HK51*P(3,15)); -Kfusion(16) = -HK60*(HK45*P(0,16) + HK46*P(4,16) + HK47*P(5,16) + HK48*P(6,16) + HK49*P(1,16) + HK50*P(2,16) + HK51*P(3,16)); -Kfusion(17) = -HK60*(HK45*P(0,17) + HK46*P(4,17) + HK47*P(5,17) + HK48*P(6,17) + HK49*P(1,17) + HK50*P(2,17) + HK51*P(3,17)); -Kfusion(18) = -HK60*(HK45*P(0,18) + HK46*P(4,18) + HK47*P(5,18) + HK48*P(6,18) + HK49*P(1,18) + HK50*P(2,18) + HK51*P(3,18)); -Kfusion(19) = -HK60*(HK45*P(0,19) + HK46*P(4,19) + HK47*P(5,19) + HK48*P(6,19) + HK49*P(1,19) + HK50*P(2,19) + HK51*P(3,19)); -Kfusion(20) = -HK60*(HK45*P(0,20) + HK46*P(4,20) + HK47*P(5,20) + HK48*P(6,20) + HK49*P(1,20) + HK50*P(2,20) + HK51*P(3,20)); -Kfusion(21) = -HK60*(HK45*P(0,21) + HK46*P(4,21) + HK47*P(5,21) + HK48*P(6,21) + HK49*P(1,21) + HK50*P(2,21) + HK51*P(3,21)); -Kfusion(22) = -HK60*(HK45*P(0,22) + HK46*P(4,22) + HK47*P(5,22) + HK48*P(6,22) + HK49*P(1,22) + HK50*P(2,22) + HK51*P(3,22)); -Kfusion(23) = -HK60*(HK45*P(0,23) + HK46*P(4,23) + HK47*P(5,23) + HK48*P(6,23) + HK49*P(1,23) + HK50*P(2,23) + HK51*P(3,23)); +Kfusion(0) = -HK39*HK47; +Kfusion(1) = -HK46*HK47; +Kfusion(2) = -HK44*HK47; +Kfusion(3) = -HK45*HK47; +Kfusion(4) = -HK43*HK47; +Kfusion(5) = -HK42*HK47; +Kfusion(6) = -HK41*HK47; +Kfusion(7) = -HK47*(HK32*P(0,7) + HK33*P(1,7) + HK34*P(2,7) + HK35*P(3,7) + HK36*P(4,7) + HK37*P(5,7) + HK38*P(6,7)); +Kfusion(8) = -HK47*(HK32*P(0,8) + HK33*P(1,8) + HK34*P(2,8) + HK35*P(3,8) + HK36*P(4,8) + HK37*P(5,8) + HK38*P(6,8)); +Kfusion(9) = -HK47*(HK32*P(0,9) + HK33*P(1,9) + HK34*P(2,9) + HK35*P(3,9) + HK36*P(4,9) + HK37*P(5,9) + HK38*P(6,9)); +Kfusion(10) = -HK47*(HK32*P(0,10) + HK33*P(1,10) + HK34*P(2,10) + HK35*P(3,10) + HK36*P(4,10) + HK37*P(5,10) + HK38*P(6,10)); +Kfusion(11) = -HK47*(HK32*P(0,11) + HK33*P(1,11) + HK34*P(2,11) + HK35*P(3,11) + HK36*P(4,11) + HK37*P(5,11) + HK38*P(6,11)); +Kfusion(12) = -HK47*(HK32*P(0,12) + HK33*P(1,12) + HK34*P(2,12) + HK35*P(3,12) + HK36*P(4,12) + HK37*P(5,12) + HK38*P(6,12)); +Kfusion(13) = -HK47*(HK32*P(0,13) + HK33*P(1,13) + HK34*P(2,13) + HK35*P(3,13) + HK36*P(4,13) + HK37*P(5,13) + HK38*P(6,13)); +Kfusion(14) = -HK47*(HK32*P(0,14) + HK33*P(1,14) + HK34*P(2,14) + HK35*P(3,14) + HK36*P(4,14) + HK37*P(5,14) + HK38*P(6,14)); +Kfusion(15) = -HK47*(HK32*P(0,15) + HK33*P(1,15) + HK34*P(2,15) + HK35*P(3,15) + HK36*P(4,15) + HK37*P(5,15) + HK38*P(6,15)); +Kfusion(16) = -HK47*(HK32*P(0,16) + HK33*P(1,16) + HK34*P(2,16) + HK35*P(3,16) + HK36*P(4,16) + HK37*P(5,16) + HK38*P(6,16)); +Kfusion(17) = -HK47*(HK32*P(0,17) + HK33*P(1,17) + HK34*P(2,17) + HK35*P(3,17) + HK36*P(4,17) + HK37*P(5,17) + HK38*P(6,17)); +Kfusion(18) = -HK47*(HK32*P(0,18) + HK33*P(1,18) + HK34*P(2,18) + HK35*P(3,18) + HK36*P(4,18) + HK37*P(5,18) + HK38*P(6,18)); +Kfusion(19) = -HK47*(HK32*P(0,19) + HK33*P(1,19) + HK34*P(2,19) + HK35*P(3,19) + HK36*P(4,19) + HK37*P(5,19) + HK38*P(6,19)); +Kfusion(20) = -HK47*(HK32*P(0,20) + HK33*P(1,20) + HK34*P(2,20) + HK35*P(3,20) + HK36*P(4,20) + HK37*P(5,20) + HK38*P(6,20)); +Kfusion(21) = -HK47*(HK32*P(0,21) + HK33*P(1,21) + HK34*P(2,21) + HK35*P(3,21) + HK36*P(4,21) + HK37*P(5,21) + HK38*P(6,21)); +Kfusion(22) = -HK47*(HK32*P(0,22) + HK33*P(1,22) + HK34*P(2,22) + HK35*P(3,22) + HK36*P(4,22) + HK37*P(5,22) + HK38*P(6,22)); +Kfusion(23) = -HK47*(HK32*P(0,23) + HK33*P(1,23) + HK34*P(2,23) + HK35*P(3,23) + HK36*P(4,23) + HK37*P(5,23) + HK38*P(6,23)); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated_alt.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated_alt.cpp index 7b45e81bd0..8d32c8d5f2 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated_alt.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated_alt.cpp @@ -1,170 +1,157 @@ // 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*(q2)*(q2); -const float HK28 = 2*(q3)*(q3) - 1; -const float HK29 = HK27 + HK28; -const float HK30 = -HK21*HK22 + HK25*HK26 + HK29*Tbs(1,0); -const float HK31 = HK23 + HK24; -const float HK32 = 2*Tbs(1,0); -const float HK33 = q0*q1; -const float HK34 = q2*q3; -const float HK35 = HK33 - HK34; -const float HK36 = 2*(q1)*(q1); -const float HK37 = HK28 + HK36; -const float HK38 = HK22*HK35 - HK31*HK32 + HK37*Tbs(1,1); -const float HK39 = HK33 + HK34; -const float HK40 = HK19 - HK20; -const float HK41 = HK27 + HK36 - 1; -const float HK42 = -HK26*HK39 + HK32*HK40 + HK41*Tbs(1,2); -const float HK43 = 2*HK7; -const float HK44 = 2*HK14*P(0,1) + 2*HK17*P(0,2) + 2*HK18*P(0,3) - HK30*P(0,4) - HK38*P(0,5) - HK42*P(0,6) - HK43*P(0,0); -const float HK45 = 1.0F/((range)*(range)); -const float HK46 = 2*HK14*P(1,6) + 2*HK17*P(2,6) + 2*HK18*P(3,6) - HK30*P(4,6) - HK38*P(5,6) - HK42*P(6,6) - HK43*P(0,6); -const float HK47 = 2*HK14*P(1,5) + 2*HK17*P(2,5) + 2*HK18*P(3,5) - HK30*P(4,5) - HK38*P(5,5) - HK42*P(5,6) - HK43*P(0,5); -const float HK48 = 2*HK14*P(1,4) + 2*HK17*P(2,4) + 2*HK18*P(3,4) - HK30*P(4,4) - HK38*P(4,5) - HK42*P(4,6) - HK43*P(0,4); -const float HK49 = 2*HK14*P(1,3) + 2*HK17*P(2,3) + 2*HK18*P(3,3) - HK30*P(3,4) - HK38*P(3,5) - HK42*P(3,6) - HK43*P(0,3); -const float HK50 = 2*HK14*P(1,2) + 2*HK17*P(2,2) + 2*HK18*P(2,3) - HK30*P(2,4) - HK38*P(2,5) - HK42*P(2,6) - HK43*P(0,2); -const float HK51 = 2*HK14*P(1,1) + 2*HK17*P(1,2) + 2*HK18*P(1,3) - HK30*P(1,4) - HK38*P(1,5) - HK42*P(1,6) - HK43*P(0,1); -const float HK52 = HK8/(2*HK14*HK45*HK51 + 2*HK17*HK45*HK50 + 2*HK18*HK45*HK49 - HK30*HK45*HK48 - HK38*HK45*HK47 - HK42*HK45*HK46 - HK43*HK44*HK45 + R_LOS); -const float HK53 = Tbs(0,0)*q2; -const float HK54 = Tbs(0,1)*q1; -const float HK55 = HK53 - HK54; -const float HK56 = Tbs(0,1)*q3; -const float HK57 = Tbs(0,2)*q2; -const float HK58 = HK56 - HK57; -const float HK59 = Tbs(0,0)*q3; -const float HK60 = Tbs(0,2)*q1; -const float HK61 = -HK60; -const float HK62 = HK59 + HK61; -const float HK63 = Tbs(0,2)*q0; -const float HK64 = -HK53 + 2*HK54 + HK63; -const float HK65 = Tbs(0,1)*q0; -const float HK66 = Tbs(0,1)*q2; -const float HK67 = Tbs(0,2)*q3; -const float HK68 = vd*(HK59 - 2*HK60 + HK65) + vn*(HK66 + HK67); -const float HK69 = Tbs(0,0)*q0; -const float HK70 = -HK56 + 2*HK57 + HK69; -const float HK71 = Tbs(0,0)*q1; -const float HK72 = ve*(HK67 + HK71) + vn*(-2*HK53 + HK54 + HK63); -const float HK73 = 2*HK59 + HK61 + HK65; -const float HK74 = vd*(HK66 + HK71) + ve*(-2*HK56 + HK57 + HK69); -const float HK75 = 2*Tbs(0,1); -const float HK76 = 2*Tbs(0,2); -const float HK77 = 2*Tbs(0,0); -const float HK78 = -2*HK55*vd - 2*HK58*vn + 2*HK62*ve; -const float HK79 = HK21*HK76 - HK25*HK75 - HK29*Tbs(0,0); -const float HK80 = HK31*HK77 - HK35*HK76 - HK37*Tbs(0,1); -const float HK81 = HK39*HK75 - HK40*HK77 - HK41*Tbs(0,2); -const float HK82 = -2*HK64*ve + 2*HK68; -const float HK83 = -2*HK70*vd + 2*HK72; -const float HK84 = -2*HK73*vn + 2*HK74; -const float HK85 = HK78*P(0,0) + HK79*P(0,4) + HK80*P(0,5) + HK81*P(0,6) + HK82*P(0,1) + HK83*P(0,2) + HK84*P(0,3); -const float HK86 = HK78*P(0,6) + HK79*P(4,6) + HK80*P(5,6) + HK81*P(6,6) + HK82*P(1,6) + HK83*P(2,6) + HK84*P(3,6); -const float HK87 = HK78*P(0,5) + HK79*P(4,5) + HK80*P(5,5) + HK81*P(5,6) + HK82*P(1,5) + HK83*P(2,5) + HK84*P(3,5); -const float HK88 = HK78*P(0,4) + HK79*P(4,4) + HK80*P(4,5) + HK81*P(4,6) + HK82*P(1,4) + HK83*P(2,4) + HK84*P(3,4); -const float HK89 = HK78*P(0,3) + HK79*P(3,4) + HK80*P(3,5) + HK81*P(3,6) + HK82*P(1,3) + HK83*P(2,3) + HK84*P(3,3); -const float HK90 = HK78*P(0,2) + HK79*P(2,4) + HK80*P(2,5) + HK81*P(2,6) + HK82*P(1,2) + HK83*P(2,2) + HK84*P(2,3); -const float HK91 = HK78*P(0,1) + HK79*P(1,4) + HK80*P(1,5) + HK81*P(1,6) + HK82*P(1,1) + HK83*P(1,2) + HK84*P(1,3); -const float HK92 = HK8/(HK45*HK78*HK85 + HK45*HK79*HK88 + HK45*HK80*HK87 + HK45*HK81*HK86 + HK45*HK82*HK91 + HK45*HK83*HK90 + HK45*HK84*HK89 + R_LOS); +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 = (q1)*(q1); +const float HK19 = (q2)*(q2); +const float HK20 = -HK19; +const float HK21 = (q0)*(q0); +const float HK22 = (q3)*(q3); +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 = 1.0F/((range)*(range)); +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,0)*q2 + Tbs(0,1)*q1 + Tbs(0,2)*q0; +const float HK52 = Tbs(0,0)*q3 + Tbs(0,1)*q0 - Tbs(0,2)*q1; +const float HK53 = Tbs(0,0)*q0 - Tbs(0,1)*q3 + Tbs(0,2)*q2; +const float HK54 = HK51*vd + HK52*ve + HK53*vn; +const float HK55 = Tbs(0,0)*q1 + Tbs(0,1)*q2 + Tbs(0,2)*q3; +const float HK56 = HK52*vd + HK55*vn; +const float HK57 = HK51*vn + HK55*ve; +const float HK58 = HK53*ve + HK55*vd; +const float HK59 = 2*Tbs(0,1); +const float HK60 = 2*Tbs(0,2); +const float HK61 = HK12*HK60 + HK24*Tbs(0,0); +const float HK62 = 2*Tbs(0,0); +const float HK63 = HK26*HK62 + HK32*Tbs(0,1); +const float HK64 = HK34*HK59 + HK36*Tbs(0,2); +const float HK65 = 2*HK54; +const float HK66 = -2*HK51*ve + 2*HK56; +const float HK67 = -2*HK53*vd + 2*HK57; +const float HK68 = -2*HK52*vn + 2*HK58; +const float HK69 = -HK16*HK59 + HK61; +const float HK70 = -HK30*HK60 + HK63; +const float HK71 = -HK35*HK62 + HK64; +const float HK72 = HK65*P(0,0) + HK66*P(0,1) + HK67*P(0,2) + HK68*P(0,3) + HK69*P(0,4) + HK70*P(0,5) + HK71*P(0,6); +const float HK73 = HK65*P(0,6) + HK66*P(1,6) + HK67*P(2,6) + HK68*P(3,6) + HK69*P(4,6) + HK70*P(5,6) + HK71*P(6,6); +const float HK74 = HK65*P(0,5) + HK66*P(1,5) + HK67*P(2,5) + HK68*P(3,5) + HK69*P(4,5) + HK70*P(5,5) + HK71*P(5,6); +const float HK75 = HK65*P(0,4) + HK66*P(1,4) + HK67*P(2,4) + HK68*P(3,4) + HK69*P(4,4) + HK70*P(4,5) + HK71*P(4,6); +const float HK76 = HK65*P(0,2) + HK66*P(1,2) + HK67*P(2,2) + HK68*P(2,3) + HK69*P(2,4) + HK70*P(2,5) + HK71*P(2,6); +const float HK77 = HK65*P(0,3) + HK66*P(1,3) + HK67*P(2,3) + HK68*P(3,3) + HK69*P(3,4) + HK70*P(3,5) + HK71*P(3,6); +const float HK78 = HK65*P(0,1) + HK66*P(1,1) + HK67*P(1,2) + HK68*P(1,3) + HK69*P(1,4) + HK70*P(1,5) + HK71*P(1,6); +const float HK79 = HK4/(HK43*HK65*HK72 + HK43*HK66*HK78 + HK43*HK67*HK76 + HK43*HK68*HK77 + HK43*HK69*HK75 + HK43*HK70*HK74 + HK43*HK71*HK73 + 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>() = -HK30*HK8; -Hfusion.at<5>() = -HK38*HK8; -Hfusion.at<6>() = -HK42*HK8; +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) = HK44*HK52; -Kfusion(1) = HK51*HK52; -Kfusion(2) = HK50*HK52; -Kfusion(3) = HK49*HK52; -Kfusion(4) = HK48*HK52; -Kfusion(5) = HK47*HK52; -Kfusion(6) = HK46*HK52; -Kfusion(7) = HK52*(2*HK14*P(1,7) + 2*HK17*P(2,7) + 2*HK18*P(3,7) - HK30*P(4,7) - HK38*P(5,7) - HK42*P(6,7) - HK43*P(0,7)); -Kfusion(8) = HK52*(2*HK14*P(1,8) + 2*HK17*P(2,8) + 2*HK18*P(3,8) - HK30*P(4,8) - HK38*P(5,8) - HK42*P(6,8) - HK43*P(0,8)); -Kfusion(9) = HK52*(2*HK14*P(1,9) + 2*HK17*P(2,9) + 2*HK18*P(3,9) - HK30*P(4,9) - HK38*P(5,9) - HK42*P(6,9) - HK43*P(0,9)); -Kfusion(10) = HK52*(2*HK14*P(1,10) + 2*HK17*P(2,10) + 2*HK18*P(3,10) - HK30*P(4,10) - HK38*P(5,10) - HK42*P(6,10) - HK43*P(0,10)); -Kfusion(11) = HK52*(2*HK14*P(1,11) + 2*HK17*P(2,11) + 2*HK18*P(3,11) - HK30*P(4,11) - HK38*P(5,11) - HK42*P(6,11) - HK43*P(0,11)); -Kfusion(12) = HK52*(2*HK14*P(1,12) + 2*HK17*P(2,12) + 2*HK18*P(3,12) - HK30*P(4,12) - HK38*P(5,12) - HK42*P(6,12) - HK43*P(0,12)); -Kfusion(13) = HK52*(2*HK14*P(1,13) + 2*HK17*P(2,13) + 2*HK18*P(3,13) - HK30*P(4,13) - HK38*P(5,13) - HK42*P(6,13) - HK43*P(0,13)); -Kfusion(14) = HK52*(2*HK14*P(1,14) + 2*HK17*P(2,14) + 2*HK18*P(3,14) - HK30*P(4,14) - HK38*P(5,14) - HK42*P(6,14) - HK43*P(0,14)); -Kfusion(15) = HK52*(2*HK14*P(1,15) + 2*HK17*P(2,15) + 2*HK18*P(3,15) - HK30*P(4,15) - HK38*P(5,15) - HK42*P(6,15) - HK43*P(0,15)); -Kfusion(16) = HK52*(2*HK14*P(1,16) + 2*HK17*P(2,16) + 2*HK18*P(3,16) - HK30*P(4,16) - HK38*P(5,16) - HK42*P(6,16) - HK43*P(0,16)); -Kfusion(17) = HK52*(2*HK14*P(1,17) + 2*HK17*P(2,17) + 2*HK18*P(3,17) - HK30*P(4,17) - HK38*P(5,17) - HK42*P(6,17) - HK43*P(0,17)); -Kfusion(18) = HK52*(2*HK14*P(1,18) + 2*HK17*P(2,18) + 2*HK18*P(3,18) - HK30*P(4,18) - HK38*P(5,18) - HK42*P(6,18) - HK43*P(0,18)); -Kfusion(19) = HK52*(2*HK14*P(1,19) + 2*HK17*P(2,19) + 2*HK18*P(3,19) - HK30*P(4,19) - HK38*P(5,19) - HK42*P(6,19) - HK43*P(0,19)); -Kfusion(20) = HK52*(2*HK14*P(1,20) + 2*HK17*P(2,20) + 2*HK18*P(3,20) - HK30*P(4,20) - HK38*P(5,20) - HK42*P(6,20) - HK43*P(0,20)); -Kfusion(21) = HK52*(2*HK14*P(1,21) + 2*HK17*P(2,21) + 2*HK18*P(3,21) - HK30*P(4,21) - HK38*P(5,21) - HK42*P(6,21) - HK43*P(0,21)); -Kfusion(22) = HK52*(2*HK14*P(1,22) + 2*HK17*P(2,22) + 2*HK18*P(3,22) - HK30*P(4,22) - HK38*P(5,22) - HK42*P(6,22) - HK43*P(0,22)); -Kfusion(23) = HK52*(2*HK14*P(1,23) + 2*HK17*P(2,23) + 2*HK18*P(3,23) - HK30*P(4,23) - HK38*P(5,23) - HK42*P(6,23) - HK43*P(0,23)); +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; +Kfusion(7) = HK50*(HK25*P(4,7) + HK33*P(5,7) + HK37*P(6,7) + HK38*P(0,7) + HK39*P(1,7) + HK40*P(2,7) + HK41*P(3,7)); +Kfusion(8) = HK50*(HK25*P(4,8) + HK33*P(5,8) + HK37*P(6,8) + HK38*P(0,8) + HK39*P(1,8) + HK40*P(2,8) + HK41*P(3,8)); +Kfusion(9) = HK50*(HK25*P(4,9) + HK33*P(5,9) + HK37*P(6,9) + HK38*P(0,9) + HK39*P(1,9) + HK40*P(2,9) + HK41*P(3,9)); +Kfusion(10) = HK50*(HK25*P(4,10) + HK33*P(5,10) + HK37*P(6,10) + HK38*P(0,10) + HK39*P(1,10) + HK40*P(2,10) + HK41*P(3,10)); +Kfusion(11) = HK50*(HK25*P(4,11) + HK33*P(5,11) + HK37*P(6,11) + HK38*P(0,11) + HK39*P(1,11) + HK40*P(2,11) + HK41*P(3,11)); +Kfusion(12) = HK50*(HK25*P(4,12) + HK33*P(5,12) + HK37*P(6,12) + HK38*P(0,12) + HK39*P(1,12) + HK40*P(2,12) + HK41*P(3,12)); +Kfusion(13) = HK50*(HK25*P(4,13) + HK33*P(5,13) + HK37*P(6,13) + HK38*P(0,13) + HK39*P(1,13) + HK40*P(2,13) + HK41*P(3,13)); +Kfusion(14) = HK50*(HK25*P(4,14) + HK33*P(5,14) + HK37*P(6,14) + HK38*P(0,14) + HK39*P(1,14) + HK40*P(2,14) + HK41*P(3,14)); +Kfusion(15) = HK50*(HK25*P(4,15) + HK33*P(5,15) + HK37*P(6,15) + HK38*P(0,15) + HK39*P(1,15) + HK40*P(2,15) + HK41*P(3,15)); +Kfusion(16) = HK50*(HK25*P(4,16) + HK33*P(5,16) + HK37*P(6,16) + HK38*P(0,16) + HK39*P(1,16) + HK40*P(2,16) + HK41*P(3,16)); +Kfusion(17) = HK50*(HK25*P(4,17) + HK33*P(5,17) + HK37*P(6,17) + HK38*P(0,17) + HK39*P(1,17) + HK40*P(2,17) + HK41*P(3,17)); +Kfusion(18) = HK50*(HK25*P(4,18) + HK33*P(5,18) + HK37*P(6,18) + HK38*P(0,18) + HK39*P(1,18) + HK40*P(2,18) + HK41*P(3,18)); +Kfusion(19) = HK50*(HK25*P(4,19) + HK33*P(5,19) + HK37*P(6,19) + HK38*P(0,19) + HK39*P(1,19) + HK40*P(2,19) + HK41*P(3,19)); +Kfusion(20) = HK50*(HK25*P(4,20) + HK33*P(5,20) + HK37*P(6,20) + HK38*P(0,20) + HK39*P(1,20) + HK40*P(2,20) + HK41*P(3,20)); +Kfusion(21) = HK50*(HK25*P(4,21) + HK33*P(5,21) + HK37*P(6,21) + HK38*P(0,21) + HK39*P(1,21) + HK40*P(2,21) + HK41*P(3,21)); +Kfusion(22) = HK50*(HK25*P(4,22) + HK33*P(5,22) + HK37*P(6,22) + HK38*P(0,22) + HK39*P(1,22) + HK40*P(2,22) + HK41*P(3,22)); +Kfusion(23) = HK50*(HK25*P(4,23) + HK33*P(5,23) + HK37*P(6,23) + HK38*P(0,23) + HK39*P(1,23) + HK40*P(2,23) + HK41*P(3,23)); // Observation Jacobians - axis 1 -Hfusion.at<0>() = -HK9*(-HK55*vd - HK58*vn + HK62*ve); -Hfusion.at<1>() = -HK9*(-HK64*ve + HK68); -Hfusion.at<2>() = -HK9*(-HK70*vd + HK72); -Hfusion.at<3>() = -HK9*(-HK73*vn + HK74); -Hfusion.at<4>() = -HK8*(2*HK21*Tbs(0,2) - HK25*HK75 - HK29*Tbs(0,0)); -Hfusion.at<5>() = -HK8*(2*HK31*Tbs(0,0) - HK35*HK76 - HK37*Tbs(0,1)); -Hfusion.at<6>() = -HK8*(2*HK39*Tbs(0,1) - HK40*HK77 - HK41*Tbs(0,2)); +Hfusion.at<0>() = -HK5*HK54; +Hfusion.at<1>() = -HK5*(-HK51*ve + HK56); +Hfusion.at<2>() = -HK5*(-HK53*vd + HK57); +Hfusion.at<3>() = -HK5*(-HK52*vn + HK58); +Hfusion.at<4>() = -HK4*(-HK16*HK59 + HK61); +Hfusion.at<5>() = -HK4*(-HK30*HK60 + HK63); +Hfusion.at<6>() = -HK4*(-HK35*HK62 + HK64); // Kalman gains - axis 1 -Kfusion(0) = -HK85*HK92; -Kfusion(1) = -HK91*HK92; -Kfusion(2) = -HK90*HK92; -Kfusion(3) = -HK89*HK92; -Kfusion(4) = -HK88*HK92; -Kfusion(5) = -HK87*HK92; -Kfusion(6) = -HK86*HK92; -Kfusion(7) = -HK92*(HK78*P(0,7) + HK79*P(4,7) + HK80*P(5,7) + HK81*P(6,7) + HK82*P(1,7) + HK83*P(2,7) + HK84*P(3,7)); -Kfusion(8) = -HK92*(HK78*P(0,8) + HK79*P(4,8) + HK80*P(5,8) + HK81*P(6,8) + HK82*P(1,8) + HK83*P(2,8) + HK84*P(3,8)); -Kfusion(9) = -HK92*(HK78*P(0,9) + HK79*P(4,9) + HK80*P(5,9) + HK81*P(6,9) + HK82*P(1,9) + HK83*P(2,9) + HK84*P(3,9)); -Kfusion(10) = -HK92*(HK78*P(0,10) + HK79*P(4,10) + HK80*P(5,10) + HK81*P(6,10) + HK82*P(1,10) + HK83*P(2,10) + HK84*P(3,10)); -Kfusion(11) = -HK92*(HK78*P(0,11) + HK79*P(4,11) + HK80*P(5,11) + HK81*P(6,11) + HK82*P(1,11) + HK83*P(2,11) + HK84*P(3,11)); -Kfusion(12) = -HK92*(HK78*P(0,12) + HK79*P(4,12) + HK80*P(5,12) + HK81*P(6,12) + HK82*P(1,12) + HK83*P(2,12) + HK84*P(3,12)); -Kfusion(13) = -HK92*(HK78*P(0,13) + HK79*P(4,13) + HK80*P(5,13) + HK81*P(6,13) + HK82*P(1,13) + HK83*P(2,13) + HK84*P(3,13)); -Kfusion(14) = -HK92*(HK78*P(0,14) + HK79*P(4,14) + HK80*P(5,14) + HK81*P(6,14) + HK82*P(1,14) + HK83*P(2,14) + HK84*P(3,14)); -Kfusion(15) = -HK92*(HK78*P(0,15) + HK79*P(4,15) + HK80*P(5,15) + HK81*P(6,15) + HK82*P(1,15) + HK83*P(2,15) + HK84*P(3,15)); -Kfusion(16) = -HK92*(HK78*P(0,16) + HK79*P(4,16) + HK80*P(5,16) + HK81*P(6,16) + HK82*P(1,16) + HK83*P(2,16) + HK84*P(3,16)); -Kfusion(17) = -HK92*(HK78*P(0,17) + HK79*P(4,17) + HK80*P(5,17) + HK81*P(6,17) + HK82*P(1,17) + HK83*P(2,17) + HK84*P(3,17)); -Kfusion(18) = -HK92*(HK78*P(0,18) + HK79*P(4,18) + HK80*P(5,18) + HK81*P(6,18) + HK82*P(1,18) + HK83*P(2,18) + HK84*P(3,18)); -Kfusion(19) = -HK92*(HK78*P(0,19) + HK79*P(4,19) + HK80*P(5,19) + HK81*P(6,19) + HK82*P(1,19) + HK83*P(2,19) + HK84*P(3,19)); -Kfusion(20) = -HK92*(HK78*P(0,20) + HK79*P(4,20) + HK80*P(5,20) + HK81*P(6,20) + HK82*P(1,20) + HK83*P(2,20) + HK84*P(3,20)); -Kfusion(21) = -HK92*(HK78*P(0,21) + HK79*P(4,21) + HK80*P(5,21) + HK81*P(6,21) + HK82*P(1,21) + HK83*P(2,21) + HK84*P(3,21)); -Kfusion(22) = -HK92*(HK78*P(0,22) + HK79*P(4,22) + HK80*P(5,22) + HK81*P(6,22) + HK82*P(1,22) + HK83*P(2,22) + HK84*P(3,22)); -Kfusion(23) = -HK92*(HK78*P(0,23) + HK79*P(4,23) + HK80*P(5,23) + HK81*P(6,23) + HK82*P(1,23) + HK83*P(2,23) + HK84*P(3,23)); +Kfusion(0) = -HK72*HK79; +Kfusion(1) = -HK78*HK79; +Kfusion(2) = -HK76*HK79; +Kfusion(3) = -HK77*HK79; +Kfusion(4) = -HK75*HK79; +Kfusion(5) = -HK74*HK79; +Kfusion(6) = -HK73*HK79; +Kfusion(7) = -HK79*(HK65*P(0,7) + HK66*P(1,7) + HK67*P(2,7) + HK68*P(3,7) + HK69*P(4,7) + HK70*P(5,7) + HK71*P(6,7)); +Kfusion(8) = -HK79*(HK65*P(0,8) + HK66*P(1,8) + HK67*P(2,8) + HK68*P(3,8) + HK69*P(4,8) + HK70*P(5,8) + HK71*P(6,8)); +Kfusion(9) = -HK79*(HK65*P(0,9) + HK66*P(1,9) + HK67*P(2,9) + HK68*P(3,9) + HK69*P(4,9) + HK70*P(5,9) + HK71*P(6,9)); +Kfusion(10) = -HK79*(HK65*P(0,10) + HK66*P(1,10) + HK67*P(2,10) + HK68*P(3,10) + HK69*P(4,10) + HK70*P(5,10) + HK71*P(6,10)); +Kfusion(11) = -HK79*(HK65*P(0,11) + HK66*P(1,11) + HK67*P(2,11) + HK68*P(3,11) + HK69*P(4,11) + HK70*P(5,11) + HK71*P(6,11)); +Kfusion(12) = -HK79*(HK65*P(0,12) + HK66*P(1,12) + HK67*P(2,12) + HK68*P(3,12) + HK69*P(4,12) + HK70*P(5,12) + HK71*P(6,12)); +Kfusion(13) = -HK79*(HK65*P(0,13) + HK66*P(1,13) + HK67*P(2,13) + HK68*P(3,13) + HK69*P(4,13) + HK70*P(5,13) + HK71*P(6,13)); +Kfusion(14) = -HK79*(HK65*P(0,14) + HK66*P(1,14) + HK67*P(2,14) + HK68*P(3,14) + HK69*P(4,14) + HK70*P(5,14) + HK71*P(6,14)); +Kfusion(15) = -HK79*(HK65*P(0,15) + HK66*P(1,15) + HK67*P(2,15) + HK68*P(3,15) + HK69*P(4,15) + HK70*P(5,15) + HK71*P(6,15)); +Kfusion(16) = -HK79*(HK65*P(0,16) + HK66*P(1,16) + HK67*P(2,16) + HK68*P(3,16) + HK69*P(4,16) + HK70*P(5,16) + HK71*P(6,16)); +Kfusion(17) = -HK79*(HK65*P(0,17) + HK66*P(1,17) + HK67*P(2,17) + HK68*P(3,17) + HK69*P(4,17) + HK70*P(5,17) + HK71*P(6,17)); +Kfusion(18) = -HK79*(HK65*P(0,18) + HK66*P(1,18) + HK67*P(2,18) + HK68*P(3,18) + HK69*P(4,18) + HK70*P(5,18) + HK71*P(6,18)); +Kfusion(19) = -HK79*(HK65*P(0,19) + HK66*P(1,19) + HK67*P(2,19) + HK68*P(3,19) + HK69*P(4,19) + HK70*P(5,19) + HK71*P(6,19)); +Kfusion(20) = -HK79*(HK65*P(0,20) + HK66*P(1,20) + HK67*P(2,20) + HK68*P(3,20) + HK69*P(4,20) + HK70*P(5,20) + HK71*P(6,20)); +Kfusion(21) = -HK79*(HK65*P(0,21) + HK66*P(1,21) + HK67*P(2,21) + HK68*P(3,21) + HK69*P(4,21) + HK70*P(5,21) + HK71*P(6,21)); +Kfusion(22) = -HK79*(HK65*P(0,22) + HK66*P(1,22) + HK67*P(2,22) + HK68*P(3,22) + HK69*P(4,22) + HK70*P(5,22) + HK71*P(6,22)); +Kfusion(23) = -HK79*(HK65*P(0,23) + HK66*P(1,23) + HK67*P(2,23) + HK68*P(3,23) + HK69*P(4,23) + HK70*P(5,23) + HK71*P(6,23)); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/gps_yaw_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/gps_yaw_generated.cpp index 31f99f7e55..db9e35f772 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/gps_yaw_generated.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/gps_yaw_generated.cpp @@ -1,63 +1,67 @@ // 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*HK0; -const float HK5 = 2*(q3)*(q3) - 1; -const float HK6 = -HK1*(HK5 + 2*(q1)*(q1)) + HK4*(HK2 + HK3); -const float HK7 = 2*HK1; -const float HK8 = HK0*(HK5 + 2*(q2)*(q2)) + HK7*(HK2 - HK3); -const float HK9 = 1.0F/(HK8); -const float HK10 = q3*(-HK0 + HK1*HK6*HK9); -const float HK11 = 1.0F/((HK8)*(HK8)); -const float HK12 = HK11*(HK6)*(HK6) + 1; -const float HK13 = 2*HK9/HK12; -const float HK14 = HK0*q2; -const float HK15 = HK1*q1; -const float HK16 = HK6*HK9; -const float HK17 = HK1*HK16*q2 + HK14 - 2*HK15; -const float HK18 = HK0*q1 + HK16*(-2*HK14 + HK15); -const float HK19 = -HK0*q0 + HK16*(HK1*q0 + HK4*q3) + HK7*q3; -const float HK20 = HK10*P(0,0) - HK17*P(0,1) - HK18*P(0,2) + HK19*P(0,3); -const float HK21 = 4*HK11/(HK12)*(HK12); -const float HK22 = HK10*P(0,1) - HK17*P(1,1) - HK18*P(1,2) + HK19*P(1,3); -const float HK23 = HK10*P(0,2) - HK17*P(1,2) - HK18*P(2,2) + HK19*P(2,3); -const float HK24 = HK10*P(0,3) - HK17*P(1,3) - HK18*P(2,3) + HK19*P(3,3); -const float HK25 = HK13/(HK10*HK20*HK21 - HK17*HK21*HK22 - HK18*HK21*HK23 + HK19*HK21*HK24 + R_YAW); +const float HK0 = sinf(ant_yaw); +const float HK1 = q0*q3; +const float HK2 = q1*q2; +const float HK3 = cosf(ant_yaw); +const float HK4 = (q1)*(q1); +const float HK5 = (q2)*(q2); +const float HK6 = (q0)*(q0) - (q3)*(q3); +const float HK7 = 2*HK0*(HK1 - HK2) - HK3*(HK4 - HK5 + HK6); +const float HK8 = 1.0F/(HK7); +const float HK9 = HK3*q0; +const float HK10 = HK0*q3; +const float HK11 = HK0*(-HK4 + HK5 + HK6) + 2*HK3*(HK1 + HK2); +const float HK12 = HK11*HK8; +const float HK13 = HK0*q0 + HK3*q3; +const float HK14 = HK8*(HK12*(-HK10 + HK9) + HK13); +const float HK15 = (HK11)*(HK11)/(HK7)*(HK7) + 1; +const float HK16 = 2/HK15; +const float HK17 = -1/HK7; +const float HK18 = HK0*q2 + HK3*q1; +const float HK19 = HK0*q1 - HK3*q2; +const float HK20 = HK17*(HK11*HK17*HK18 + HK19); +const float HK21 = HK17*(HK11*HK17*HK19 - HK18); +const float HK22 = HK10 + HK12*HK13 - HK9; +const float HK23 = -HK14*P(0,0) - HK20*P(0,1) - HK21*P(0,2) + HK22*HK8*P(0,3); +const float HK24 = -HK14*P(0,1) - HK20*P(1,1) - HK21*P(1,2) + HK22*HK8*P(1,3); +const float HK25 = 1.0F/((HK15)*(HK15)); +const float HK26 = 4*HK25; +const float HK27 = -HK14*P(0,2) - HK20*P(1,2) - HK21*P(2,2) + HK22*HK8*P(2,3); +const float HK28 = -HK14*P(0,3) - HK20*P(1,3) - HK21*P(2,3) + HK22*HK8*P(3,3); +const float HK29 = HK16/(-HK14*HK23*HK26 - HK20*HK24*HK26 - HK21*HK26*HK27 + 4*HK22*HK25*HK28*HK8 + R_YAW); // Observation Jacobians -Hfusion.at<0>() = HK10*HK13; -Hfusion.at<1>() = -HK13*HK17; -Hfusion.at<2>() = -HK13*HK18; -Hfusion.at<3>() = HK13*HK19; +Hfusion.at<0>() = -HK14*HK16; +Hfusion.at<1>() = -HK16*HK20; +Hfusion.at<2>() = -HK16*HK21; +Hfusion.at<3>() = HK16*HK22*HK8; // Kalman gains -Kfusion(0) = HK20*HK25; -Kfusion(1) = HK22*HK25; -Kfusion(2) = HK23*HK25; -Kfusion(3) = HK24*HK25; -Kfusion(4) = HK25*(HK10*P(0,4) - HK17*P(1,4) - HK18*P(2,4) + HK19*P(3,4)); -Kfusion(5) = HK25*(HK10*P(0,5) - HK17*P(1,5) - HK18*P(2,5) + HK19*P(3,5)); -Kfusion(6) = HK25*(HK10*P(0,6) - HK17*P(1,6) - HK18*P(2,6) + HK19*P(3,6)); -Kfusion(7) = HK25*(HK10*P(0,7) - HK17*P(1,7) - HK18*P(2,7) + HK19*P(3,7)); -Kfusion(8) = HK25*(HK10*P(0,8) - HK17*P(1,8) - HK18*P(2,8) + HK19*P(3,8)); -Kfusion(9) = HK25*(HK10*P(0,9) - HK17*P(1,9) - HK18*P(2,9) + HK19*P(3,9)); -Kfusion(10) = HK25*(HK10*P(0,10) - HK17*P(1,10) - HK18*P(2,10) + HK19*P(3,10)); -Kfusion(11) = HK25*(HK10*P(0,11) - HK17*P(1,11) - HK18*P(2,11) + HK19*P(3,11)); -Kfusion(12) = HK25*(HK10*P(0,12) - HK17*P(1,12) - HK18*P(2,12) + HK19*P(3,12)); -Kfusion(13) = HK25*(HK10*P(0,13) - HK17*P(1,13) - HK18*P(2,13) + HK19*P(3,13)); -Kfusion(14) = HK25*(HK10*P(0,14) - HK17*P(1,14) - HK18*P(2,14) + HK19*P(3,14)); -Kfusion(15) = HK25*(HK10*P(0,15) - HK17*P(1,15) - HK18*P(2,15) + HK19*P(3,15)); -Kfusion(16) = HK25*(HK10*P(0,16) - HK17*P(1,16) - HK18*P(2,16) + HK19*P(3,16)); -Kfusion(17) = HK25*(HK10*P(0,17) - HK17*P(1,17) - HK18*P(2,17) + HK19*P(3,17)); -Kfusion(18) = HK25*(HK10*P(0,18) - HK17*P(1,18) - HK18*P(2,18) + HK19*P(3,18)); -Kfusion(19) = HK25*(HK10*P(0,19) - HK17*P(1,19) - HK18*P(2,19) + HK19*P(3,19)); -Kfusion(20) = HK25*(HK10*P(0,20) - HK17*P(1,20) - HK18*P(2,20) + HK19*P(3,20)); -Kfusion(21) = HK25*(HK10*P(0,21) - HK17*P(1,21) - HK18*P(2,21) + HK19*P(3,21)); -Kfusion(22) = HK25*(HK10*P(0,22) - HK17*P(1,22) - HK18*P(2,22) + HK19*P(3,22)); -Kfusion(23) = HK25*(HK10*P(0,23) - HK17*P(1,23) - HK18*P(2,23) + HK19*P(3,23)); +Kfusion(0) = HK23*HK29; +Kfusion(1) = HK24*HK29; +Kfusion(2) = HK27*HK29; +Kfusion(3) = HK28*HK29; +Kfusion(4) = HK29*(-HK14*P(0,4) - HK20*P(1,4) - HK21*P(2,4) + HK22*HK8*P(3,4)); +Kfusion(5) = HK29*(-HK14*P(0,5) - HK20*P(1,5) - HK21*P(2,5) + HK22*HK8*P(3,5)); +Kfusion(6) = HK29*(-HK14*P(0,6) - HK20*P(1,6) - HK21*P(2,6) + HK22*HK8*P(3,6)); +Kfusion(7) = HK29*(-HK14*P(0,7) - HK20*P(1,7) - HK21*P(2,7) + HK22*HK8*P(3,7)); +Kfusion(8) = HK29*(-HK14*P(0,8) - HK20*P(1,8) - HK21*P(2,8) + HK22*HK8*P(3,8)); +Kfusion(9) = HK29*(-HK14*P(0,9) - HK20*P(1,9) - HK21*P(2,9) + HK22*HK8*P(3,9)); +Kfusion(10) = HK29*(-HK14*P(0,10) - HK20*P(1,10) - HK21*P(2,10) + HK22*HK8*P(3,10)); +Kfusion(11) = HK29*(-HK14*P(0,11) - HK20*P(1,11) - HK21*P(2,11) + HK22*HK8*P(3,11)); +Kfusion(12) = HK29*(-HK14*P(0,12) - HK20*P(1,12) - HK21*P(2,12) + HK22*HK8*P(3,12)); +Kfusion(13) = HK29*(-HK14*P(0,13) - HK20*P(1,13) - HK21*P(2,13) + HK22*HK8*P(3,13)); +Kfusion(14) = HK29*(-HK14*P(0,14) - HK20*P(1,14) - HK21*P(2,14) + HK22*HK8*P(3,14)); +Kfusion(15) = HK29*(-HK14*P(0,15) - HK20*P(1,15) - HK21*P(2,15) + HK22*HK8*P(3,15)); +Kfusion(16) = HK29*(-HK14*P(0,16) - HK20*P(1,16) - HK21*P(2,16) + HK22*HK8*P(3,16)); +Kfusion(17) = HK29*(-HK14*P(0,17) - HK20*P(1,17) - HK21*P(2,17) + HK22*HK8*P(3,17)); +Kfusion(18) = HK29*(-HK14*P(0,18) - HK20*P(1,18) - HK21*P(2,18) + HK22*HK8*P(3,18)); +Kfusion(19) = HK29*(-HK14*P(0,19) - HK20*P(1,19) - HK21*P(2,19) + HK22*HK8*P(3,19)); +Kfusion(20) = HK29*(-HK14*P(0,20) - HK20*P(1,20) - HK21*P(2,20) + HK22*HK8*P(3,20)); +Kfusion(21) = HK29*(-HK14*P(0,21) - HK20*P(1,21) - HK21*P(2,21) + HK22*HK8*P(3,21)); +Kfusion(22) = HK29*(-HK14*P(0,22) - HK20*P(1,22) - HK21*P(2,22) + HK22*HK8*P(3,22)); +Kfusion(23) = HK29*(-HK14*P(0,23) - HK20*P(1,23) - HK21*P(2,23) + HK22*HK8*P(3,23)); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp index 1268c8a2ff..d75236640e 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp @@ -1,180 +1,177 @@ // axis 0 -const float HK0 = q2*vd - q3*ve; -const float HK1 = q2*ve + q3*vd; -const float HK2 = 2*vn; -const float HK3 = HK2*q2 + q0*vd - q1*ve; -const float HK4 = -HK2*q3 + q0*ve + q1*vd; -const float HK5 = 2*(q2)*(q2) + 2*(q3)*(q3) - 1; -const float HK6 = q0*q3 + q1*q2; -const float HK7 = q0*q2 - q1*q3; -const float HK8 = 2*HK1; -const float HK9 = 2*HK6; +const float HK0 = q0*vn - q2*vd + q3*ve; +const float HK1 = q1*vn + q2*ve + q3*vd; +const float HK2 = q0*vd - q1*ve + q2*vn; +const float HK3 = q0*ve + q1*vd - q3*vn; +const float HK4 = (q0)*(q0) + (q1)*(q1) - (q2)*(q2) - (q3)*(q3); +const float HK5 = q0*q3 + q1*q2; +const float HK6 = q0*q2 - q1*q3; +const float HK7 = 2*HK5; +const float HK8 = 2*HK6; +const float HK9 = 2*HK1; const float HK10 = 2*HK0; -const float HK11 = 2*HK7; -const float HK12 = 2*HK4; -const float HK13 = 2*HK3; -const float HK14 = HK10*P(0,0) + HK11*P(0,6) - HK12*P(0,3) + HK13*P(0,2) + HK5*P(0,4) - HK8*P(0,1) - HK9*P(0,5); -const float HK15 = HK10*P(0,5) + HK11*P(5,6) - HK12*P(3,5) + HK13*P(2,5) + HK5*P(4,5) - HK8*P(1,5) - HK9*P(5,5); -const float HK16 = HK10*P(0,1) + HK11*P(1,6) - HK12*P(1,3) + HK13*P(1,2) + HK5*P(1,4) - HK8*P(1,1) - HK9*P(1,5); -const float HK17 = HK10*P(0,6) + HK11*P(6,6) - HK12*P(3,6) + HK13*P(2,6) + HK5*P(4,6) - HK8*P(1,6) - HK9*P(5,6); -const float HK18 = HK10*P(0,4) + HK11*P(4,6) - HK12*P(3,4) + HK13*P(2,4) + HK5*P(4,4) - HK8*P(1,4) - HK9*P(4,5); -const float HK19 = HK10*P(0,3) + HK11*P(3,6) - HK12*P(3,3) + HK13*P(2,3) + HK5*P(3,4) - HK8*P(1,3) - HK9*P(3,5); -const float HK20 = HK10*P(0,2) + HK11*P(2,6) - HK12*P(2,3) + HK13*P(2,2) + HK5*P(2,4) - HK8*P(1,2) - HK9*P(2,5); -const float HK21 = 1.0F/(HK10*HK14 + HK11*HK17 - HK12*HK19 + HK13*HK20 - HK15*HK9 - HK16*HK8 + HK18*HK5 + R_VEL); - - -H_VEL(0) = -2*HK0; -H_VEL(1) = 2*HK1; -H_VEL(2) = -2*HK3; -H_VEL(3) = 2*HK4; -H_VEL(4) = -HK5; -H_VEL(5) = 2*HK6; -H_VEL(6) = -2*HK7; - - -Kfusion(0) = -HK14*HK21; -Kfusion(1) = -HK16*HK21; -Kfusion(2) = -HK20*HK21; -Kfusion(3) = -HK19*HK21; -Kfusion(4) = -HK18*HK21; -Kfusion(5) = -HK15*HK21; -Kfusion(6) = -HK17*HK21; -Kfusion(7) = -HK21*(HK10*P(0,7) + HK11*P(6,7) - HK12*P(3,7) + HK13*P(2,7) + HK5*P(4,7) - HK8*P(1,7) - HK9*P(5,7)); -Kfusion(8) = -HK21*(HK10*P(0,8) + HK11*P(6,8) - HK12*P(3,8) + HK13*P(2,8) + HK5*P(4,8) - HK8*P(1,8) - HK9*P(5,8)); -Kfusion(9) = -HK21*(HK10*P(0,9) + HK11*P(6,9) - HK12*P(3,9) + HK13*P(2,9) + HK5*P(4,9) - HK8*P(1,9) - HK9*P(5,9)); -Kfusion(10) = -HK21*(HK10*P(0,10) + HK11*P(6,10) - HK12*P(3,10) + HK13*P(2,10) + HK5*P(4,10) - HK8*P(1,10) - HK9*P(5,10)); -Kfusion(11) = -HK21*(HK10*P(0,11) + HK11*P(6,11) - HK12*P(3,11) + HK13*P(2,11) + HK5*P(4,11) - HK8*P(1,11) - HK9*P(5,11)); -Kfusion(12) = -HK21*(HK10*P(0,12) + HK11*P(6,12) - HK12*P(3,12) + HK13*P(2,12) + HK5*P(4,12) - HK8*P(1,12) - HK9*P(5,12)); -Kfusion(13) = -HK21*(HK10*P(0,13) + HK11*P(6,13) - HK12*P(3,13) + HK13*P(2,13) + HK5*P(4,13) - HK8*P(1,13) - HK9*P(5,13)); -Kfusion(14) = -HK21*(HK10*P(0,14) + HK11*P(6,14) - HK12*P(3,14) + HK13*P(2,14) + HK5*P(4,14) - HK8*P(1,14) - HK9*P(5,14)); -Kfusion(15) = -HK21*(HK10*P(0,15) + HK11*P(6,15) - HK12*P(3,15) + HK13*P(2,15) + HK5*P(4,15) - HK8*P(1,15) - HK9*P(5,15)); -Kfusion(16) = -HK21*(HK10*P(0,16) + HK11*P(6,16) - HK12*P(3,16) + HK13*P(2,16) + HK5*P(4,16) - HK8*P(1,16) - HK9*P(5,16)); -Kfusion(17) = -HK21*(HK10*P(0,17) + HK11*P(6,17) - HK12*P(3,17) + HK13*P(2,17) + HK5*P(4,17) - HK8*P(1,17) - HK9*P(5,17)); -Kfusion(18) = -HK21*(HK10*P(0,18) + HK11*P(6,18) - HK12*P(3,18) + HK13*P(2,18) + HK5*P(4,18) - HK8*P(1,18) - HK9*P(5,18)); -Kfusion(19) = -HK21*(HK10*P(0,19) + HK11*P(6,19) - HK12*P(3,19) + HK13*P(2,19) + HK5*P(4,19) - HK8*P(1,19) - HK9*P(5,19)); -Kfusion(20) = -HK21*(HK10*P(0,20) + HK11*P(6,20) - HK12*P(3,20) + HK13*P(2,20) + HK5*P(4,20) - HK8*P(1,20) - HK9*P(5,20)); -Kfusion(21) = -HK21*(HK10*P(0,21) + HK11*P(6,21) - HK12*P(3,21) + HK13*P(2,21) + HK5*P(4,21) - HK8*P(1,21) - HK9*P(5,21)); -Kfusion(22) = -HK21*(HK10*P(0,22) + HK11*P(6,22) - HK12*P(3,22) + HK13*P(2,22) + HK5*P(4,22) - HK8*P(1,22) - HK9*P(5,22)); -Kfusion(23) = -HK21*(HK10*P(0,23) + HK11*P(6,23) - HK12*P(3,23) + HK13*P(2,23) + HK5*P(4,23) - HK8*P(1,23) - HK9*P(5,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 = HK1*q3 + q0*vn - q2*vd; -const float HK5 = q0*q3 - q1*q2; -const float HK6 = 2*(q1)*(q1) + 2*(q3)*(q3) - 1; -const float HK7 = q0*q1 + q2*q3; -const float HK8 = 2*HK3; -const float HK9 = 2*HK7; -const float HK10 = 2*HK0; -const float HK11 = 2*HK5; -const float HK12 = 2*HK2; -const float HK13 = 2*HK4; -const float HK14 = HK10*P(0,0) - HK11*P(0,4) + HK12*P(0,1) - HK13*P(0,3) - HK6*P(0,5) + HK8*P(0,2) + HK9*P(0,6); -const float HK15 = HK10*P(0,6) - HK11*P(4,6) + HK12*P(1,6) - HK13*P(3,6) - HK6*P(5,6) + HK8*P(2,6) + HK9*P(6,6); -const float HK16 = HK10*P(0,2) - HK11*P(2,4) + HK12*P(1,2) - HK13*P(2,3) - HK6*P(2,5) + HK8*P(2,2) + HK9*P(2,6); -const float HK17 = HK10*P(0,4) - HK11*P(4,4) + HK12*P(1,4) - HK13*P(3,4) - HK6*P(4,5) + HK8*P(2,4) + HK9*P(4,6); -const float HK18 = HK10*P(0,1) - HK11*P(1,4) + HK12*P(1,1) - HK13*P(1,3) - HK6*P(1,5) + HK8*P(1,2) + HK9*P(1,6); -const float HK19 = HK10*P(0,5) - HK11*P(4,5) + HK12*P(1,5) - HK13*P(3,5) - HK6*P(5,5) + HK8*P(2,5) + HK9*P(5,6); -const float HK20 = HK10*P(0,3) - HK11*P(3,4) + HK12*P(1,3) - HK13*P(3,3) - HK6*P(3,5) + HK8*P(2,3) + HK9*P(3,6); -const float HK21 = 1.0F/(HK10*HK14 - HK11*HK17 + HK12*HK18 - HK13*HK20 + HK15*HK9 + HK16*HK8 - HK19*HK6 + R_VEL); +const float HK11 = 2*HK2; +const float HK12 = 2*HK3; +const float HK13 = HK10*P(0,0) - HK11*P(0,2) + HK12*P(0,3) + HK4*P(0,4) + HK7*P(0,5) - HK8*P(0,6) + HK9*P(0,1); +const float HK14 = HK10*P(0,5) - HK11*P(2,5) + HK12*P(3,5) + HK4*P(4,5) + HK7*P(5,5) - HK8*P(5,6) + HK9*P(1,5); +const float HK15 = HK10*P(0,6) - HK11*P(2,6) + HK12*P(3,6) + HK4*P(4,6) + HK7*P(5,6) - HK8*P(6,6) + HK9*P(1,6); +const float HK16 = HK10*P(0,1) - HK11*P(1,2) + HK12*P(1,3) + HK4*P(1,4) + HK7*P(1,5) - HK8*P(1,6) + HK9*P(1,1); +const float HK17 = HK10*P(0,2) - HK11*P(2,2) + HK12*P(2,3) + HK4*P(2,4) + HK7*P(2,5) - HK8*P(2,6) + HK9*P(1,2); +const float HK18 = HK10*P(0,3) - HK11*P(2,3) + HK12*P(3,3) + HK4*P(3,4) + HK7*P(3,5) - HK8*P(3,6) + HK9*P(1,3); +const float HK19 = HK10*P(0,4) - HK11*P(2,4) + HK12*P(3,4) + HK4*P(4,4) + HK7*P(4,5) - HK8*P(4,6) + HK9*P(1,4); +const float HK20 = 1.0F/(HK10*HK13 - HK11*HK17 + HK12*HK18 + HK14*HK7 - HK15*HK8 + HK16*HK9 + HK19*HK4 + R_VEL); H_VEL(0) = 2*HK0; -H_VEL(1) = 2*HK2; -H_VEL(2) = 2*HK3; -H_VEL(3) = -2*HK4; -H_VEL(4) = -2*HK5; -H_VEL(5) = -HK6; -H_VEL(6) = 2*HK7; +H_VEL(1) = 2*HK1; +H_VEL(2) = -2*HK2; +H_VEL(3) = 2*HK3; +H_VEL(4) = HK4; +H_VEL(5) = 2*HK5; +H_VEL(6) = -2*HK6; -Kfusion(0) = HK14*HK21; -Kfusion(1) = HK18*HK21; -Kfusion(2) = HK16*HK21; -Kfusion(3) = HK20*HK21; -Kfusion(4) = HK17*HK21; -Kfusion(5) = HK19*HK21; -Kfusion(6) = HK15*HK21; -Kfusion(7) = HK21*(HK10*P(0,7) - HK11*P(4,7) + HK12*P(1,7) - HK13*P(3,7) - HK6*P(5,7) + HK8*P(2,7) + HK9*P(6,7)); -Kfusion(8) = HK21*(HK10*P(0,8) - HK11*P(4,8) + HK12*P(1,8) - HK13*P(3,8) - HK6*P(5,8) + HK8*P(2,8) + HK9*P(6,8)); -Kfusion(9) = HK21*(HK10*P(0,9) - HK11*P(4,9) + HK12*P(1,9) - HK13*P(3,9) - HK6*P(5,9) + HK8*P(2,9) + HK9*P(6,9)); -Kfusion(10) = HK21*(HK10*P(0,10) - HK11*P(4,10) + HK12*P(1,10) - HK13*P(3,10) - HK6*P(5,10) + HK8*P(2,10) + HK9*P(6,10)); -Kfusion(11) = HK21*(HK10*P(0,11) - HK11*P(4,11) + HK12*P(1,11) - HK13*P(3,11) - HK6*P(5,11) + HK8*P(2,11) + HK9*P(6,11)); -Kfusion(12) = HK21*(HK10*P(0,12) - HK11*P(4,12) + HK12*P(1,12) - HK13*P(3,12) - HK6*P(5,12) + HK8*P(2,12) + HK9*P(6,12)); -Kfusion(13) = HK21*(HK10*P(0,13) - HK11*P(4,13) + HK12*P(1,13) - HK13*P(3,13) - HK6*P(5,13) + HK8*P(2,13) + HK9*P(6,13)); -Kfusion(14) = HK21*(HK10*P(0,14) - HK11*P(4,14) + HK12*P(1,14) - HK13*P(3,14) - HK6*P(5,14) + HK8*P(2,14) + HK9*P(6,14)); -Kfusion(15) = HK21*(HK10*P(0,15) - HK11*P(4,15) + HK12*P(1,15) - HK13*P(3,15) - HK6*P(5,15) + HK8*P(2,15) + HK9*P(6,15)); -Kfusion(16) = HK21*(HK10*P(0,16) - HK11*P(4,16) + HK12*P(1,16) - HK13*P(3,16) - HK6*P(5,16) + HK8*P(2,16) + HK9*P(6,16)); -Kfusion(17) = HK21*(HK10*P(0,17) - HK11*P(4,17) + HK12*P(1,17) - HK13*P(3,17) - HK6*P(5,17) + HK8*P(2,17) + HK9*P(6,17)); -Kfusion(18) = HK21*(HK10*P(0,18) - HK11*P(4,18) + HK12*P(1,18) - HK13*P(3,18) - HK6*P(5,18) + HK8*P(2,18) + HK9*P(6,18)); -Kfusion(19) = HK21*(HK10*P(0,19) - HK11*P(4,19) + HK12*P(1,19) - HK13*P(3,19) - HK6*P(5,19) + HK8*P(2,19) + HK9*P(6,19)); -Kfusion(20) = HK21*(HK10*P(0,20) - HK11*P(4,20) + HK12*P(1,20) - HK13*P(3,20) - HK6*P(5,20) + HK8*P(2,20) + HK9*P(6,20)); -Kfusion(21) = HK21*(HK10*P(0,21) - HK11*P(4,21) + HK12*P(1,21) - HK13*P(3,21) - HK6*P(5,21) + HK8*P(2,21) + HK9*P(6,21)); -Kfusion(22) = HK21*(HK10*P(0,22) - HK11*P(4,22) + HK12*P(1,22) - HK13*P(3,22) - HK6*P(5,22) + HK8*P(2,22) + HK9*P(6,22)); -Kfusion(23) = HK21*(HK10*P(0,23) - HK11*P(4,23) + HK12*P(1,23) - HK13*P(3,23) - HK6*P(5,23) + HK8*P(2,23) + HK9*P(6,23)); +Kfusion(0) = HK13*HK20; +Kfusion(1) = HK16*HK20; +Kfusion(2) = HK17*HK20; +Kfusion(3) = HK18*HK20; +Kfusion(4) = HK19*HK20; +Kfusion(5) = HK14*HK20; +Kfusion(6) = HK15*HK20; +Kfusion(7) = HK20*(HK10*P(0,7) - HK11*P(2,7) + HK12*P(3,7) + HK4*P(4,7) + HK7*P(5,7) - HK8*P(6,7) + HK9*P(1,7)); +Kfusion(8) = HK20*(HK10*P(0,8) - HK11*P(2,8) + HK12*P(3,8) + HK4*P(4,8) + HK7*P(5,8) - HK8*P(6,8) + HK9*P(1,8)); +Kfusion(9) = HK20*(HK10*P(0,9) - HK11*P(2,9) + HK12*P(3,9) + HK4*P(4,9) + HK7*P(5,9) - HK8*P(6,9) + HK9*P(1,9)); +Kfusion(10) = HK20*(HK10*P(0,10) - HK11*P(2,10) + HK12*P(3,10) + HK4*P(4,10) + HK7*P(5,10) - HK8*P(6,10) + HK9*P(1,10)); +Kfusion(11) = HK20*(HK10*P(0,11) - HK11*P(2,11) + HK12*P(3,11) + HK4*P(4,11) + HK7*P(5,11) - HK8*P(6,11) + HK9*P(1,11)); +Kfusion(12) = HK20*(HK10*P(0,12) - HK11*P(2,12) + HK12*P(3,12) + HK4*P(4,12) + HK7*P(5,12) - HK8*P(6,12) + HK9*P(1,12)); +Kfusion(13) = HK20*(HK10*P(0,13) - HK11*P(2,13) + HK12*P(3,13) + HK4*P(4,13) + HK7*P(5,13) - HK8*P(6,13) + HK9*P(1,13)); +Kfusion(14) = HK20*(HK10*P(0,14) - HK11*P(2,14) + HK12*P(3,14) + HK4*P(4,14) + HK7*P(5,14) - HK8*P(6,14) + HK9*P(1,14)); +Kfusion(15) = HK20*(HK10*P(0,15) - HK11*P(2,15) + HK12*P(3,15) + HK4*P(4,15) + HK7*P(5,15) - HK8*P(6,15) + HK9*P(1,15)); +Kfusion(16) = HK20*(HK10*P(0,16) - HK11*P(2,16) + HK12*P(3,16) + HK4*P(4,16) + HK7*P(5,16) - HK8*P(6,16) + HK9*P(1,16)); +Kfusion(17) = HK20*(HK10*P(0,17) - HK11*P(2,17) + HK12*P(3,17) + HK4*P(4,17) + HK7*P(5,17) - HK8*P(6,17) + HK9*P(1,17)); +Kfusion(18) = HK20*(HK10*P(0,18) - HK11*P(2,18) + HK12*P(3,18) + HK4*P(4,18) + HK7*P(5,18) - HK8*P(6,18) + HK9*P(1,18)); +Kfusion(19) = HK20*(HK10*P(0,19) - HK11*P(2,19) + HK12*P(3,19) + HK4*P(4,19) + HK7*P(5,19) - HK8*P(6,19) + HK9*P(1,19)); +Kfusion(20) = HK20*(HK10*P(0,20) - HK11*P(2,20) + HK12*P(3,20) + HK4*P(4,20) + HK7*P(5,20) - HK8*P(6,20) + HK9*P(1,20)); +Kfusion(21) = HK20*(HK10*P(0,21) - HK11*P(2,21) + HK12*P(3,21) + HK4*P(4,21) + HK7*P(5,21) - HK8*P(6,21) + HK9*P(1,21)); +Kfusion(22) = HK20*(HK10*P(0,22) - HK11*P(2,22) + HK12*P(3,22) + HK4*P(4,22) + HK7*P(5,22) - HK8*P(6,22) + HK9*P(1,22)); +Kfusion(23) = HK20*(HK10*P(0,23) - HK11*P(2,23) + HK12*P(3,23) + HK4*P(4,23) + HK7*P(5,23) - HK8*P(6,23) + HK9*P(1,23)); + + +// axis 1 +const float HK0 = q0*ve + q1*vd - q3*vn; +const float HK1 = q0*vd - q1*ve + q2*vn; +const float HK2 = q1*vn + q2*ve + q3*vd; +const float HK3 = q0*vn - q2*vd + q3*ve; +const float HK4 = q0*q3 - q1*q2; +const float HK5 = (q0)*(q0) - (q1)*(q1) + (q2)*(q2) - (q3)*(q3); +const float HK6 = q0*q1 + q2*q3; +const float HK7 = 2*HK6; +const float HK8 = 2*HK4; +const float HK9 = 2*HK2; +const float HK10 = 2*HK0; +const float HK11 = 2*HK1; +const float HK12 = 2*HK3; +const float HK13 = HK10*P(0,0) + HK11*P(0,1) - HK12*P(0,3) + HK5*P(0,5) + HK7*P(0,6) - HK8*P(0,4) + HK9*P(0,2); +const float HK14 = HK10*P(0,6) + HK11*P(1,6) - HK12*P(3,6) + HK5*P(5,6) + HK7*P(6,6) - HK8*P(4,6) + HK9*P(2,6); +const float HK15 = HK10*P(0,4) + HK11*P(1,4) - HK12*P(3,4) + HK5*P(4,5) + HK7*P(4,6) - HK8*P(4,4) + HK9*P(2,4); +const float HK16 = HK10*P(0,2) + HK11*P(1,2) - HK12*P(2,3) + HK5*P(2,5) + HK7*P(2,6) - HK8*P(2,4) + HK9*P(2,2); +const float HK17 = HK10*P(0,1) + HK11*P(1,1) - HK12*P(1,3) + HK5*P(1,5) + HK7*P(1,6) - HK8*P(1,4) + HK9*P(1,2); +const float HK18 = HK10*P(0,3) + HK11*P(1,3) - HK12*P(3,3) + HK5*P(3,5) + HK7*P(3,6) - HK8*P(3,4) + HK9*P(2,3); +const float HK19 = HK10*P(0,5) + HK11*P(1,5) - HK12*P(3,5) + HK5*P(5,5) + HK7*P(5,6) - HK8*P(4,5) + HK9*P(2,5); +const float HK20 = 1.0F/(HK10*HK13 + HK11*HK17 - HK12*HK18 + HK14*HK7 - HK15*HK8 + HK16*HK9 + HK19*HK5 + R_VEL); + + +H_VEL(0) = 2*HK0; +H_VEL(1) = 2*HK1; +H_VEL(2) = 2*HK2; +H_VEL(3) = -2*HK3; +H_VEL(4) = -2*HK4; +H_VEL(5) = HK5; +H_VEL(6) = 2*HK6; + + +Kfusion(0) = HK13*HK20; +Kfusion(1) = HK17*HK20; +Kfusion(2) = HK16*HK20; +Kfusion(3) = HK18*HK20; +Kfusion(4) = HK15*HK20; +Kfusion(5) = HK19*HK20; +Kfusion(6) = HK14*HK20; +Kfusion(7) = HK20*(HK10*P(0,7) + HK11*P(1,7) - HK12*P(3,7) + HK5*P(5,7) + HK7*P(6,7) - HK8*P(4,7) + HK9*P(2,7)); +Kfusion(8) = HK20*(HK10*P(0,8) + HK11*P(1,8) - HK12*P(3,8) + HK5*P(5,8) + HK7*P(6,8) - HK8*P(4,8) + HK9*P(2,8)); +Kfusion(9) = HK20*(HK10*P(0,9) + HK11*P(1,9) - HK12*P(3,9) + HK5*P(5,9) + HK7*P(6,9) - HK8*P(4,9) + HK9*P(2,9)); +Kfusion(10) = HK20*(HK10*P(0,10) + HK11*P(1,10) - HK12*P(3,10) + HK5*P(5,10) + HK7*P(6,10) - HK8*P(4,10) + HK9*P(2,10)); +Kfusion(11) = HK20*(HK10*P(0,11) + HK11*P(1,11) - HK12*P(3,11) + HK5*P(5,11) + HK7*P(6,11) - HK8*P(4,11) + HK9*P(2,11)); +Kfusion(12) = HK20*(HK10*P(0,12) + HK11*P(1,12) - HK12*P(3,12) + HK5*P(5,12) + HK7*P(6,12) - HK8*P(4,12) + HK9*P(2,12)); +Kfusion(13) = HK20*(HK10*P(0,13) + HK11*P(1,13) - HK12*P(3,13) + HK5*P(5,13) + HK7*P(6,13) - HK8*P(4,13) + HK9*P(2,13)); +Kfusion(14) = HK20*(HK10*P(0,14) + HK11*P(1,14) - HK12*P(3,14) + HK5*P(5,14) + HK7*P(6,14) - HK8*P(4,14) + HK9*P(2,14)); +Kfusion(15) = HK20*(HK10*P(0,15) + HK11*P(1,15) - HK12*P(3,15) + HK5*P(5,15) + HK7*P(6,15) - HK8*P(4,15) + HK9*P(2,15)); +Kfusion(16) = HK20*(HK10*P(0,16) + HK11*P(1,16) - HK12*P(3,16) + HK5*P(5,16) + HK7*P(6,16) - HK8*P(4,16) + HK9*P(2,16)); +Kfusion(17) = HK20*(HK10*P(0,17) + HK11*P(1,17) - HK12*P(3,17) + HK5*P(5,17) + HK7*P(6,17) - HK8*P(4,17) + HK9*P(2,17)); +Kfusion(18) = HK20*(HK10*P(0,18) + HK11*P(1,18) - HK12*P(3,18) + HK5*P(5,18) + HK7*P(6,18) - HK8*P(4,18) + HK9*P(2,18)); +Kfusion(19) = HK20*(HK10*P(0,19) + HK11*P(1,19) - HK12*P(3,19) + HK5*P(5,19) + HK7*P(6,19) - HK8*P(4,19) + HK9*P(2,19)); +Kfusion(20) = HK20*(HK10*P(0,20) + HK11*P(1,20) - HK12*P(3,20) + HK5*P(5,20) + HK7*P(6,20) - HK8*P(4,20) + HK9*P(2,20)); +Kfusion(21) = HK20*(HK10*P(0,21) + HK11*P(1,21) - HK12*P(3,21) + HK5*P(5,21) + HK7*P(6,21) - HK8*P(4,21) + HK9*P(2,21)); +Kfusion(22) = HK20*(HK10*P(0,22) + HK11*P(1,22) - HK12*P(3,22) + HK5*P(5,22) + HK7*P(6,22) - HK8*P(4,22) + HK9*P(2,22)); +Kfusion(23) = HK20*(HK10*P(0,23) + HK11*P(1,23) - HK12*P(3,23) + HK5*P(5,23) + HK7*P(6,23) - HK8*P(4,23) + HK9*P(2,23)); // axis 2 -const float HK0 = q1*ve - q2*vn; -const float HK1 = 2*vd; -const float HK2 = HK1*q1 + q0*ve - q3*vn; -const float HK3 = -HK1*q2 + q0*vn + q3*ve; -const float HK4 = q1*vn + q2*ve; -const float HK5 = q0*q2 + q1*q3; -const float HK6 = q0*q1 - q2*q3; -const float HK7 = 2*(q1)*(q1) + 2*(q2)*(q2) - 1; -const float HK8 = 2*HK4; -const float HK9 = 2*HK5; +const float HK0 = q0*vd - q1*ve + q2*vn; +const float HK1 = q0*ve + q1*vd - q3*vn; +const float HK2 = q0*vn - q2*vd + q3*ve; +const float HK3 = q1*vn + q2*ve + q3*vd; +const float HK4 = q0*q2 + q1*q3; +const float HK5 = q0*q1 - q2*q3; +const float HK6 = (q0)*(q0) - (q1)*(q1) - (q2)*(q2) + (q3)*(q3); +const float HK7 = 2*HK4; +const float HK8 = 2*HK5; +const float HK9 = 2*HK3; const float HK10 = 2*HK0; -const float HK11 = 2*HK6; -const float HK12 = 2*HK3; -const float HK13 = 2*HK2; -const float HK14 = HK10*P(0,0) + HK11*P(0,5) - HK12*P(0,2) + HK13*P(0,1) + HK7*P(0,6) - HK8*P(0,3) - HK9*P(0,4); -const float HK15 = HK10*P(0,4) + HK11*P(4,5) - HK12*P(2,4) + HK13*P(1,4) + HK7*P(4,6) - HK8*P(3,4) - HK9*P(4,4); -const float HK16 = HK10*P(0,3) + HK11*P(3,5) - HK12*P(2,3) + HK13*P(1,3) + HK7*P(3,6) - HK8*P(3,3) - HK9*P(3,4); -const float HK17 = HK10*P(0,5) + HK11*P(5,5) - HK12*P(2,5) + HK13*P(1,5) + HK7*P(5,6) - HK8*P(3,5) - HK9*P(4,5); -const float HK18 = HK10*P(0,6) + HK11*P(5,6) - HK12*P(2,6) + HK13*P(1,6) + HK7*P(6,6) - HK8*P(3,6) - HK9*P(4,6); -const float HK19 = HK10*P(0,2) + HK11*P(2,5) - HK12*P(2,2) + HK13*P(1,2) + HK7*P(2,6) - HK8*P(2,3) - HK9*P(2,4); -const float HK20 = HK10*P(0,1) + HK11*P(1,5) - HK12*P(1,2) + HK13*P(1,1) + HK7*P(1,6) - HK8*P(1,3) - HK9*P(1,4); -const float HK21 = 1.0F/(HK10*HK14 + HK11*HK17 - HK12*HK19 + HK13*HK20 - HK15*HK9 - HK16*HK8 + HK18*HK7 + R_VEL); +const float HK11 = 2*HK1; +const float HK12 = 2*HK2; +const float HK13 = HK10*P(0,0) - HK11*P(0,1) + HK12*P(0,2) + HK6*P(0,6) + HK7*P(0,4) - HK8*P(0,5) + HK9*P(0,3); +const float HK14 = HK10*P(0,4) - HK11*P(1,4) + HK12*P(2,4) + HK6*P(4,6) + HK7*P(4,4) - HK8*P(4,5) + HK9*P(3,4); +const float HK15 = HK10*P(0,5) - HK11*P(1,5) + HK12*P(2,5) + HK6*P(5,6) + HK7*P(4,5) - HK8*P(5,5) + HK9*P(3,5); +const float HK16 = HK10*P(0,3) - HK11*P(1,3) + HK12*P(2,3) + HK6*P(3,6) + HK7*P(3,4) - HK8*P(3,5) + HK9*P(3,3); +const float HK17 = HK10*P(0,1) - HK11*P(1,1) + HK12*P(1,2) + HK6*P(1,6) + HK7*P(1,4) - HK8*P(1,5) + HK9*P(1,3); +const float HK18 = HK10*P(0,2) - HK11*P(1,2) + HK12*P(2,2) + HK6*P(2,6) + HK7*P(2,4) - HK8*P(2,5) + HK9*P(2,3); +const float HK19 = HK10*P(0,6) - HK11*P(1,6) + HK12*P(2,6) + HK6*P(6,6) + HK7*P(4,6) - HK8*P(5,6) + HK9*P(3,6); +const float HK20 = 1.0F/(HK10*HK13 - HK11*HK17 + HK12*HK18 + HK14*HK7 - HK15*HK8 + HK16*HK9 + HK19*HK6 + R_VEL); -H_VEL(0) = -2*HK0; -H_VEL(1) = -2*HK2; -H_VEL(2) = 2*HK3; -H_VEL(3) = 2*HK4; -H_VEL(4) = 2*HK5; -H_VEL(5) = -2*HK6; -H_VEL(6) = -HK7; +H_VEL(0) = 2*HK0; +H_VEL(1) = -2*HK1; +H_VEL(2) = 2*HK2; +H_VEL(3) = 2*HK3; +H_VEL(4) = 2*HK4; +H_VEL(5) = -2*HK5; +H_VEL(6) = HK6; -Kfusion(0) = -HK14*HK21; -Kfusion(1) = -HK20*HK21; -Kfusion(2) = -HK19*HK21; -Kfusion(3) = -HK16*HK21; -Kfusion(4) = -HK15*HK21; -Kfusion(5) = -HK17*HK21; -Kfusion(6) = -HK18*HK21; -Kfusion(7) = -HK21*(HK10*P(0,7) + HK11*P(5,7) - HK12*P(2,7) + HK13*P(1,7) + HK7*P(6,7) - HK8*P(3,7) - HK9*P(4,7)); -Kfusion(8) = -HK21*(HK10*P(0,8) + HK11*P(5,8) - HK12*P(2,8) + HK13*P(1,8) + HK7*P(6,8) - HK8*P(3,8) - HK9*P(4,8)); -Kfusion(9) = -HK21*(HK10*P(0,9) + HK11*P(5,9) - HK12*P(2,9) + HK13*P(1,9) + HK7*P(6,9) - HK8*P(3,9) - HK9*P(4,9)); -Kfusion(10) = -HK21*(HK10*P(0,10) + HK11*P(5,10) - HK12*P(2,10) + HK13*P(1,10) + HK7*P(6,10) - HK8*P(3,10) - HK9*P(4,10)); -Kfusion(11) = -HK21*(HK10*P(0,11) + HK11*P(5,11) - HK12*P(2,11) + HK13*P(1,11) + HK7*P(6,11) - HK8*P(3,11) - HK9*P(4,11)); -Kfusion(12) = -HK21*(HK10*P(0,12) + HK11*P(5,12) - HK12*P(2,12) + HK13*P(1,12) + HK7*P(6,12) - HK8*P(3,12) - HK9*P(4,12)); -Kfusion(13) = -HK21*(HK10*P(0,13) + HK11*P(5,13) - HK12*P(2,13) + HK13*P(1,13) + HK7*P(6,13) - HK8*P(3,13) - HK9*P(4,13)); -Kfusion(14) = -HK21*(HK10*P(0,14) + HK11*P(5,14) - HK12*P(2,14) + HK13*P(1,14) + HK7*P(6,14) - HK8*P(3,14) - HK9*P(4,14)); -Kfusion(15) = -HK21*(HK10*P(0,15) + HK11*P(5,15) - HK12*P(2,15) + HK13*P(1,15) + HK7*P(6,15) - HK8*P(3,15) - HK9*P(4,15)); -Kfusion(16) = -HK21*(HK10*P(0,16) + HK11*P(5,16) - HK12*P(2,16) + HK13*P(1,16) + HK7*P(6,16) - HK8*P(3,16) - HK9*P(4,16)); -Kfusion(17) = -HK21*(HK10*P(0,17) + HK11*P(5,17) - HK12*P(2,17) + HK13*P(1,17) + HK7*P(6,17) - HK8*P(3,17) - HK9*P(4,17)); -Kfusion(18) = -HK21*(HK10*P(0,18) + HK11*P(5,18) - HK12*P(2,18) + HK13*P(1,18) + HK7*P(6,18) - HK8*P(3,18) - HK9*P(4,18)); -Kfusion(19) = -HK21*(HK10*P(0,19) + HK11*P(5,19) - HK12*P(2,19) + HK13*P(1,19) + HK7*P(6,19) - HK8*P(3,19) - HK9*P(4,19)); -Kfusion(20) = -HK21*(HK10*P(0,20) + HK11*P(5,20) - HK12*P(2,20) + HK13*P(1,20) + HK7*P(6,20) - HK8*P(3,20) - HK9*P(4,20)); -Kfusion(21) = -HK21*(HK10*P(0,21) + HK11*P(5,21) - HK12*P(2,21) + HK13*P(1,21) + HK7*P(6,21) - HK8*P(3,21) - HK9*P(4,21)); -Kfusion(22) = -HK21*(HK10*P(0,22) + HK11*P(5,22) - HK12*P(2,22) + HK13*P(1,22) + HK7*P(6,22) - HK8*P(3,22) - HK9*P(4,22)); -Kfusion(23) = -HK21*(HK10*P(0,23) + HK11*P(5,23) - HK12*P(2,23) + HK13*P(1,23) + HK7*P(6,23) - HK8*P(3,23) - HK9*P(4,23)); +Kfusion(0) = HK13*HK20; +Kfusion(1) = HK17*HK20; +Kfusion(2) = HK18*HK20; +Kfusion(3) = HK16*HK20; +Kfusion(4) = HK14*HK20; +Kfusion(5) = HK15*HK20; +Kfusion(6) = HK19*HK20; +Kfusion(7) = HK20*(HK10*P(0,7) - HK11*P(1,7) + HK12*P(2,7) + HK6*P(6,7) + HK7*P(4,7) - HK8*P(5,7) + HK9*P(3,7)); +Kfusion(8) = HK20*(HK10*P(0,8) - HK11*P(1,8) + HK12*P(2,8) + HK6*P(6,8) + HK7*P(4,8) - HK8*P(5,8) + HK9*P(3,8)); +Kfusion(9) = HK20*(HK10*P(0,9) - HK11*P(1,9) + HK12*P(2,9) + HK6*P(6,9) + HK7*P(4,9) - HK8*P(5,9) + HK9*P(3,9)); +Kfusion(10) = HK20*(HK10*P(0,10) - HK11*P(1,10) + HK12*P(2,10) + HK6*P(6,10) + HK7*P(4,10) - HK8*P(5,10) + HK9*P(3,10)); +Kfusion(11) = HK20*(HK10*P(0,11) - HK11*P(1,11) + HK12*P(2,11) + HK6*P(6,11) + HK7*P(4,11) - HK8*P(5,11) + HK9*P(3,11)); +Kfusion(12) = HK20*(HK10*P(0,12) - HK11*P(1,12) + HK12*P(2,12) + HK6*P(6,12) + HK7*P(4,12) - HK8*P(5,12) + HK9*P(3,12)); +Kfusion(13) = HK20*(HK10*P(0,13) - HK11*P(1,13) + HK12*P(2,13) + HK6*P(6,13) + HK7*P(4,13) - HK8*P(5,13) + HK9*P(3,13)); +Kfusion(14) = HK20*(HK10*P(0,14) - HK11*P(1,14) + HK12*P(2,14) + HK6*P(6,14) + HK7*P(4,14) - HK8*P(5,14) + HK9*P(3,14)); +Kfusion(15) = HK20*(HK10*P(0,15) - HK11*P(1,15) + HK12*P(2,15) + HK6*P(6,15) + HK7*P(4,15) - HK8*P(5,15) + HK9*P(3,15)); +Kfusion(16) = HK20*(HK10*P(0,16) - HK11*P(1,16) + HK12*P(2,16) + HK6*P(6,16) + HK7*P(4,16) - HK8*P(5,16) + HK9*P(3,16)); +Kfusion(17) = HK20*(HK10*P(0,17) - HK11*P(1,17) + HK12*P(2,17) + HK6*P(6,17) + HK7*P(4,17) - HK8*P(5,17) + HK9*P(3,17)); +Kfusion(18) = HK20*(HK10*P(0,18) - HK11*P(1,18) + HK12*P(2,18) + HK6*P(6,18) + HK7*P(4,18) - HK8*P(5,18) + HK9*P(3,18)); +Kfusion(19) = HK20*(HK10*P(0,19) - HK11*P(1,19) + HK12*P(2,19) + HK6*P(6,19) + HK7*P(4,19) - HK8*P(5,19) + HK9*P(3,19)); +Kfusion(20) = HK20*(HK10*P(0,20) - HK11*P(1,20) + HK12*P(2,20) + HK6*P(6,20) + HK7*P(4,20) - HK8*P(5,20) + HK9*P(3,20)); +Kfusion(21) = HK20*(HK10*P(0,21) - HK11*P(1,21) + HK12*P(2,21) + HK6*P(6,21) + HK7*P(4,21) - HK8*P(5,21) + HK9*P(3,21)); +Kfusion(22) = HK20*(HK10*P(0,22) - HK11*P(1,22) + HK12*P(2,22) + HK6*P(6,22) + HK7*P(4,22) - HK8*P(5,22) + HK9*P(3,22)); +Kfusion(23) = HK20*(HK10*P(0,23) - HK11*P(1,23) + HK12*P(2,23) + HK6*P(6,23) + HK7*P(4,23) - HK8*P(5,23) + HK9*P(3,23)); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp index 22713eab1f..457c92a310 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp @@ -1,196 +1,187 @@ // Sub Expressions -const float HK0 = q2*vd; -const float HK1 = HK0 - q3*ve; -const float HK2 = q2*ve; -const float HK3 = q3*vd; -const float HK4 = HK2 + HK3; -const float HK5 = q0*vd; -const float HK6 = q1*ve; -const float HK7 = q2*vn; -const float HK8 = HK5 - HK6 + 2*HK7; -const float HK9 = q0*ve; -const float HK10 = q1*vd; -const float HK11 = q3*vn; -const float HK12 = HK10 - 2*HK11 + HK9; -const float HK13 = 2*(q2)*(q2); -const float HK14 = 2*(q3)*(q3) - 1; -const float HK15 = HK13 + HK14; -const float HK16 = q0*q3; -const float HK17 = HK16 + q1*q2; -const float HK18 = q0*q2; -const float HK19 = HK18 - q1*q3; -const float HK20 = 2*HK4; -const float HK21 = 2*HK17; -const float HK22 = 2*HK1; -const float HK23 = 2*HK19; -const float HK24 = 2*HK12; -const float HK25 = 2*HK8; -const float HK26 = HK15*P(0,4) - HK20*P(0,1) - HK21*P(0,5) + HK22*P(0,0) + HK23*P(0,6) - HK24*P(0,3) + HK25*P(0,2); -const float HK27 = HK15*P(4,5) - HK20*P(1,5) - HK21*P(5,5) + HK22*P(0,5) + HK23*P(5,6) - HK24*P(3,5) + HK25*P(2,5); -const float HK28 = HK15*P(1,4) - HK20*P(1,1) - HK21*P(1,5) + HK22*P(0,1) + HK23*P(1,6) - HK24*P(1,3) + HK25*P(1,2); -const float HK29 = HK15*P(4,6) - HK20*P(1,6) - HK21*P(5,6) + HK22*P(0,6) + HK23*P(6,6) - HK24*P(3,6) + HK25*P(2,6); -const float HK30 = HK15*P(4,4) - HK20*P(1,4) - HK21*P(4,5) + HK22*P(0,4) + HK23*P(4,6) - HK24*P(3,4) + HK25*P(2,4); -const float HK31 = HK15*P(3,4) - HK20*P(1,3) - HK21*P(3,5) + HK22*P(0,3) + HK23*P(3,6) - HK24*P(3,3) + HK25*P(2,3); -const float HK32 = HK15*P(2,4) - HK20*P(1,2) - HK21*P(2,5) + HK22*P(0,2) + HK23*P(2,6) - HK24*P(2,3) + HK25*P(2,2); -const float HK33 = 1.0F/(HK15*HK30 - HK20*HK28 - HK21*HK27 + HK22*HK26 + HK23*HK29 - HK24*HK31 + HK25*HK32 + R_VEL); -const float HK34 = -HK11; -const float HK35 = HK10 + HK34; -const float HK36 = HK5 - 2*HK6 + HK7; -const float HK37 = q1*vn; -const float HK38 = HK3 + HK37; -const float HK39 = q0*vn; -const float HK40 = q3*ve; -const float HK41 = -HK0 + HK39 + 2*HK40; -const float HK42 = HK16 - q1*q2; -const float HK43 = 2*(q1)*(q1); -const float HK44 = HK14 + HK43; -const float HK45 = q0*q1; -const float HK46 = HK45 + q2*q3; -const float HK47 = 2*HK38; -const float HK48 = 2*HK46; -const float HK49 = 2*HK35; -const float HK50 = 2*HK42; -const float HK51 = 2*HK36; -const float HK52 = 2*HK41; -const float HK53 = -HK44*P(0,5) + HK47*P(0,2) + HK48*P(0,6) + HK49*P(0,0) - HK50*P(0,4) + HK51*P(0,1) - HK52*P(0,3); -const float HK54 = -HK44*P(5,6) + HK47*P(2,6) + HK48*P(6,6) + HK49*P(0,6) - HK50*P(4,6) + HK51*P(1,6) - HK52*P(3,6); -const float HK55 = -HK44*P(2,5) + HK47*P(2,2) + HK48*P(2,6) + HK49*P(0,2) - HK50*P(2,4) + HK51*P(1,2) - HK52*P(2,3); -const float HK56 = -HK44*P(4,5) + HK47*P(2,4) + HK48*P(4,6) + HK49*P(0,4) - HK50*P(4,4) + HK51*P(1,4) - HK52*P(3,4); -const float HK57 = -HK44*P(1,5) + HK47*P(1,2) + HK48*P(1,6) + HK49*P(0,1) - HK50*P(1,4) + HK51*P(1,1) - HK52*P(1,3); -const float HK58 = -HK44*P(5,5) + HK47*P(2,5) + HK48*P(5,6) + HK49*P(0,5) - HK50*P(4,5) + HK51*P(1,5) - HK52*P(3,5); -const float HK59 = -HK44*P(3,5) + HK47*P(2,3) + HK48*P(3,6) + HK49*P(0,3) - HK50*P(3,4) + HK51*P(1,3) - HK52*P(3,3); -const float HK60 = 1.0F/(-HK44*HK58 + HK47*HK55 + HK48*HK54 + HK49*HK53 - HK50*HK56 + HK51*HK57 - HK52*HK59 + R_VEL); -const float HK61 = HK6 - q2*vn; -const float HK62 = 2*HK10 + HK34 + HK9; -const float HK63 = -2*HK0 + HK39 + HK40; -const float HK64 = HK2 + HK37; -const float HK65 = HK18 + q1*q3; -const float HK66 = HK45 - q2*q3; -const float HK67 = HK13 + HK43 - 1; -const float HK68 = 2*HK64; -const float HK69 = 2*HK65; -const float HK70 = 2*HK61; -const float HK71 = 2*HK66; -const float HK72 = 2*HK63; -const float HK73 = 2*HK62; -const float HK74 = HK67*P(0,6) - HK68*P(0,3) - HK69*P(0,4) + HK70*P(0,0) + HK71*P(0,5) - HK72*P(0,2) + HK73*P(0,1); -const float HK75 = HK67*P(4,6) - HK68*P(3,4) - HK69*P(4,4) + HK70*P(0,4) + HK71*P(4,5) - HK72*P(2,4) + HK73*P(1,4); -const float HK76 = HK67*P(3,6) - HK68*P(3,3) - HK69*P(3,4) + HK70*P(0,3) + HK71*P(3,5) - HK72*P(2,3) + HK73*P(1,3); -const float HK77 = HK67*P(5,6) - HK68*P(3,5) - HK69*P(4,5) + HK70*P(0,5) + HK71*P(5,5) - HK72*P(2,5) + HK73*P(1,5); -const float HK78 = HK67*P(6,6) - HK68*P(3,6) - HK69*P(4,6) + HK70*P(0,6) + HK71*P(5,6) - HK72*P(2,6) + HK73*P(1,6); -const float HK79 = HK67*P(2,6) - HK68*P(2,3) - HK69*P(2,4) + HK70*P(0,2) + HK71*P(2,5) - HK72*P(2,2) + HK73*P(1,2); -const float HK80 = HK67*P(1,6) - HK68*P(1,3) - HK69*P(1,4) + HK70*P(0,1) + HK71*P(1,5) - HK72*P(1,2) + HK73*P(1,1); -const float HK81 = 1.0F/(HK67*HK78 - HK68*HK76 - HK69*HK75 + HK70*HK74 + HK71*HK77 - HK72*HK79 + HK73*HK80 + R_VEL); +const float HK0 = q0*vn - q2*vd + q3*ve; +const float HK1 = 2*HK0; +const float HK2 = q1*vn + q2*ve + q3*vd; +const float HK3 = 2*HK2; +const float HK4 = q0*vd - q1*ve + q2*vn; +const float HK5 = q0*ve + q1*vd - q3*vn; +const float HK6 = 2*HK5; +const float HK7 = (q1)*(q1); +const float HK8 = (q2)*(q2); +const float HK9 = -HK8; +const float HK10 = (q0)*(q0); +const float HK11 = (q3)*(q3); +const float HK12 = HK10 - HK11; +const float HK13 = HK12 + HK7 + HK9; +const float HK14 = q0*q3; +const float HK15 = HK14 + q1*q2; +const float HK16 = q0*q2; +const float HK17 = HK16 - q1*q3; +const float HK18 = 2*HK15; +const float HK19 = 2*HK17; +const float HK20 = 2*HK2; +const float HK21 = 2*HK0; +const float HK22 = 2*HK4; +const float HK23 = HK22*P(0,2); +const float HK24 = 2*HK5; +const float HK25 = HK24*P(0,3); +const float HK26 = HK13*P(0,4) + HK18*P(0,5) - HK19*P(0,6) + HK20*P(0,1) + HK21*P(0,0) - HK23 + HK25; +const float HK27 = HK13*P(4,5) + HK18*P(5,5) - HK19*P(5,6) + HK20*P(1,5) + HK21*P(0,5) - HK22*P(2,5) + HK24*P(3,5); +const float HK28 = HK13*P(4,6) + HK18*P(5,6) - HK19*P(6,6) + HK20*P(1,6) + HK21*P(0,6) - HK22*P(2,6) + HK24*P(3,6); +const float HK29 = HK22*P(1,2); +const float HK30 = HK24*P(1,3); +const float HK31 = HK13*P(1,4) + HK18*P(1,5) - HK19*P(1,6) + HK20*P(1,1) + HK21*P(0,1) - HK29 + HK30; +const float HK32 = HK20*P(1,2); +const float HK33 = HK21*P(0,2); +const float HK34 = HK13*P(2,4) + HK18*P(2,5) - HK19*P(2,6) - HK22*P(2,2) + HK24*P(2,3) + HK32 + HK33; +const float HK35 = HK20*P(1,3); +const float HK36 = HK21*P(0,3); +const float HK37 = HK13*P(3,4) + HK18*P(3,5) - HK19*P(3,6) - HK22*P(2,3) + HK24*P(3,3) + HK35 + HK36; +const float HK38 = HK13*P(4,4) + HK18*P(4,5) - HK19*P(4,6) + HK20*P(1,4) + HK21*P(0,4) - HK22*P(2,4) + HK24*P(3,4); +const float HK39 = 1.0F/(HK13*HK38 + HK18*HK27 - HK19*HK28 + HK20*HK31 + HK21*HK26 - HK22*HK34 + HK24*HK37 + R_VEL); +const float HK40 = 2*HK4; +const float HK41 = HK14 - q1*q2; +const float HK42 = -HK7; +const float HK43 = HK12 + HK42 + HK8; +const float HK44 = q0*q1; +const float HK45 = HK44 + q2*q3; +const float HK46 = 2*HK45; +const float HK47 = 2*HK41; +const float HK48 = HK22*P(0,1); +const float HK49 = HK20*P(0,2) + HK24*P(0,0) - HK36 + HK43*P(0,5) + HK46*P(0,6) - HK47*P(0,4) + HK48; +const float HK50 = HK20*P(2,6) - HK21*P(3,6) + HK22*P(1,6) + HK24*P(0,6) + HK43*P(5,6) + HK46*P(6,6) - HK47*P(4,6); +const float HK51 = HK20*P(2,4) - HK21*P(3,4) + HK22*P(1,4) + HK24*P(0,4) + HK43*P(4,5) + HK46*P(4,6) - HK47*P(4,4); +const float HK52 = HK21*P(2,3); +const float HK53 = HK20*P(2,2) + HK24*P(0,2) + HK29 + HK43*P(2,5) + HK46*P(2,6) - HK47*P(2,4) - HK52; +const float HK54 = HK24*P(0,1); +const float HK55 = -HK21*P(1,3) + HK22*P(1,1) + HK32 + HK43*P(1,5) + HK46*P(1,6) - HK47*P(1,4) + HK54; +const float HK56 = HK20*P(2,3); +const float HK57 = -HK21*P(3,3) + HK22*P(1,3) + HK25 + HK43*P(3,5) + HK46*P(3,6) - HK47*P(3,4) + HK56; +const float HK58 = HK20*P(2,5) - HK21*P(3,5) + HK22*P(1,5) + HK24*P(0,5) + HK43*P(5,5) + HK46*P(5,6) - HK47*P(4,5); +const float HK59 = 1.0F/(HK20*HK53 - HK21*HK57 + HK22*HK55 + HK24*HK49 + HK43*HK58 + HK46*HK50 - HK47*HK51 + R_VEL); +const float HK60 = HK16 + q1*q3; +const float HK61 = HK44 - q2*q3; +const float HK62 = HK10 + HK11 + HK42 + HK9; +const float HK63 = 2*HK60; +const float HK64 = 2*HK61; +const float HK65 = HK20*P(0,3) + HK22*P(0,0) + HK33 - HK54 + HK62*P(0,6) + HK63*P(0,4) - HK64*P(0,5); +const float HK66 = HK20*P(3,4) + HK21*P(2,4) + HK22*P(0,4) - HK24*P(1,4) + HK62*P(4,6) + HK63*P(4,4) - HK64*P(4,5); +const float HK67 = HK20*P(3,5) + HK21*P(2,5) + HK22*P(0,5) - HK24*P(1,5) + HK62*P(5,6) + HK63*P(4,5) - HK64*P(5,5); +const float HK68 = HK20*P(3,3) + HK22*P(0,3) - HK30 + HK52 + HK62*P(3,6) + HK63*P(3,4) - HK64*P(3,5); +const float HK69 = HK21*P(1,2) - HK24*P(1,1) + HK35 + HK48 + HK62*P(1,6) + HK63*P(1,4) - HK64*P(1,5); +const float HK70 = HK21*P(2,2) + HK23 - HK24*P(1,2) + HK56 + HK62*P(2,6) + HK63*P(2,4) - HK64*P(2,5); +const float HK71 = HK20*P(3,6) + HK21*P(2,6) + HK22*P(0,6) - HK24*P(1,6) + HK62*P(6,6) + HK63*P(4,6) - HK64*P(5,6); +const float HK72 = 1.0F/(HK20*HK68 + HK21*HK70 + HK22*HK65 - HK24*HK69 + HK62*HK71 + HK63*HK66 - HK64*HK67 + R_VEL); // Observation Jacobians - axis 0 -Hfusion.at<0>() = -2*HK1; -Hfusion.at<1>() = 2*HK4; -Hfusion.at<2>() = -2*HK8; -Hfusion.at<3>() = 2*HK12; -Hfusion.at<4>() = -HK15; -Hfusion.at<5>() = 2*HK17; -Hfusion.at<6>() = -2*HK19; +Hfusion.at<0>() = HK1; +Hfusion.at<1>() = HK3; +Hfusion.at<2>() = -2*HK4; +Hfusion.at<3>() = HK6; +Hfusion.at<4>() = HK13; +Hfusion.at<5>() = 2*HK15; +Hfusion.at<6>() = -2*HK17; // Kalman gains - axis 0 -Kfusion(0) = -HK26*HK33; -Kfusion(1) = -HK28*HK33; -Kfusion(2) = -HK32*HK33; -Kfusion(3) = -HK31*HK33; -Kfusion(4) = -HK30*HK33; -Kfusion(5) = -HK27*HK33; -Kfusion(6) = -HK29*HK33; -Kfusion(7) = -HK33*(HK15*P(4,7) - HK20*P(1,7) - HK21*P(5,7) + HK22*P(0,7) + HK23*P(6,7) - HK24*P(3,7) + HK25*P(2,7)); -Kfusion(8) = -HK33*(HK15*P(4,8) - HK20*P(1,8) - HK21*P(5,8) + HK22*P(0,8) + HK23*P(6,8) - HK24*P(3,8) + HK25*P(2,8)); -Kfusion(9) = -HK33*(HK15*P(4,9) - HK20*P(1,9) - HK21*P(5,9) + HK22*P(0,9) + HK23*P(6,9) - HK24*P(3,9) + HK25*P(2,9)); -Kfusion(10) = -HK33*(HK15*P(4,10) - HK20*P(1,10) - HK21*P(5,10) + HK22*P(0,10) + HK23*P(6,10) - HK24*P(3,10) + HK25*P(2,10)); -Kfusion(11) = -HK33*(HK15*P(4,11) - HK20*P(1,11) - HK21*P(5,11) + HK22*P(0,11) + HK23*P(6,11) - HK24*P(3,11) + HK25*P(2,11)); -Kfusion(12) = -HK33*(HK15*P(4,12) - HK20*P(1,12) - HK21*P(5,12) + HK22*P(0,12) + HK23*P(6,12) - HK24*P(3,12) + HK25*P(2,12)); -Kfusion(13) = -HK33*(HK15*P(4,13) - HK20*P(1,13) - HK21*P(5,13) + HK22*P(0,13) + HK23*P(6,13) - HK24*P(3,13) + HK25*P(2,13)); -Kfusion(14) = -HK33*(HK15*P(4,14) - HK20*P(1,14) - HK21*P(5,14) + HK22*P(0,14) + HK23*P(6,14) - HK24*P(3,14) + HK25*P(2,14)); -Kfusion(15) = -HK33*(HK15*P(4,15) - HK20*P(1,15) - HK21*P(5,15) + HK22*P(0,15) + HK23*P(6,15) - HK24*P(3,15) + HK25*P(2,15)); -Kfusion(16) = -HK33*(HK15*P(4,16) - HK20*P(1,16) - HK21*P(5,16) + HK22*P(0,16) + HK23*P(6,16) - HK24*P(3,16) + HK25*P(2,16)); -Kfusion(17) = -HK33*(HK15*P(4,17) - HK20*P(1,17) - HK21*P(5,17) + HK22*P(0,17) + HK23*P(6,17) - HK24*P(3,17) + HK25*P(2,17)); -Kfusion(18) = -HK33*(HK15*P(4,18) - HK20*P(1,18) - HK21*P(5,18) + HK22*P(0,18) + HK23*P(6,18) - HK24*P(3,18) + HK25*P(2,18)); -Kfusion(19) = -HK33*(HK15*P(4,19) - HK20*P(1,19) - HK21*P(5,19) + HK22*P(0,19) + HK23*P(6,19) - HK24*P(3,19) + HK25*P(2,19)); -Kfusion(20) = -HK33*(HK15*P(4,20) - HK20*P(1,20) - HK21*P(5,20) + HK22*P(0,20) + HK23*P(6,20) - HK24*P(3,20) + HK25*P(2,20)); -Kfusion(21) = -HK33*(HK15*P(4,21) - HK20*P(1,21) - HK21*P(5,21) + HK22*P(0,21) + HK23*P(6,21) - HK24*P(3,21) + HK25*P(2,21)); -Kfusion(22) = -HK33*(HK15*P(4,22) - HK20*P(1,22) - HK21*P(5,22) + HK22*P(0,22) + HK23*P(6,22) - HK24*P(3,22) + HK25*P(2,22)); -Kfusion(23) = -HK33*(HK15*P(4,23) - HK20*P(1,23) - HK21*P(5,23) + HK22*P(0,23) + HK23*P(6,23) - HK24*P(3,23) + HK25*P(2,23)); +Kfusion(0) = HK26*HK39; +Kfusion(1) = HK31*HK39; +Kfusion(2) = HK34*HK39; +Kfusion(3) = HK37*HK39; +Kfusion(4) = HK38*HK39; +Kfusion(5) = HK27*HK39; +Kfusion(6) = HK28*HK39; +Kfusion(7) = HK39*(HK13*P(4,7) + HK18*P(5,7) - HK19*P(6,7) + HK20*P(1,7) + HK21*P(0,7) - HK22*P(2,7) + HK24*P(3,7)); +Kfusion(8) = HK39*(HK13*P(4,8) + HK18*P(5,8) - HK19*P(6,8) + HK20*P(1,8) + HK21*P(0,8) - HK22*P(2,8) + HK24*P(3,8)); +Kfusion(9) = HK39*(HK13*P(4,9) + HK18*P(5,9) - HK19*P(6,9) + HK20*P(1,9) + HK21*P(0,9) - HK22*P(2,9) + HK24*P(3,9)); +Kfusion(10) = HK39*(HK13*P(4,10) + HK18*P(5,10) - HK19*P(6,10) + HK20*P(1,10) + HK21*P(0,10) - HK22*P(2,10) + HK24*P(3,10)); +Kfusion(11) = HK39*(HK13*P(4,11) + HK18*P(5,11) - HK19*P(6,11) + HK20*P(1,11) + HK21*P(0,11) - HK22*P(2,11) + HK24*P(3,11)); +Kfusion(12) = HK39*(HK13*P(4,12) + HK18*P(5,12) - HK19*P(6,12) + HK20*P(1,12) + HK21*P(0,12) - HK22*P(2,12) + HK24*P(3,12)); +Kfusion(13) = HK39*(HK13*P(4,13) + HK18*P(5,13) - HK19*P(6,13) + HK20*P(1,13) + HK21*P(0,13) - HK22*P(2,13) + HK24*P(3,13)); +Kfusion(14) = HK39*(HK13*P(4,14) + HK18*P(5,14) - HK19*P(6,14) + HK20*P(1,14) + HK21*P(0,14) - HK22*P(2,14) + HK24*P(3,14)); +Kfusion(15) = HK39*(HK13*P(4,15) + HK18*P(5,15) - HK19*P(6,15) + HK20*P(1,15) + HK21*P(0,15) - HK22*P(2,15) + HK24*P(3,15)); +Kfusion(16) = HK39*(HK13*P(4,16) + HK18*P(5,16) - HK19*P(6,16) + HK20*P(1,16) + HK21*P(0,16) - HK22*P(2,16) + HK24*P(3,16)); +Kfusion(17) = HK39*(HK13*P(4,17) + HK18*P(5,17) - HK19*P(6,17) + HK20*P(1,17) + HK21*P(0,17) - HK22*P(2,17) + HK24*P(3,17)); +Kfusion(18) = HK39*(HK13*P(4,18) + HK18*P(5,18) - HK19*P(6,18) + HK20*P(1,18) + HK21*P(0,18) - HK22*P(2,18) + HK24*P(3,18)); +Kfusion(19) = HK39*(HK13*P(4,19) + HK18*P(5,19) - HK19*P(6,19) + HK20*P(1,19) + HK21*P(0,19) - HK22*P(2,19) + HK24*P(3,19)); +Kfusion(20) = HK39*(HK13*P(4,20) + HK18*P(5,20) - HK19*P(6,20) + HK20*P(1,20) + HK21*P(0,20) - HK22*P(2,20) + HK24*P(3,20)); +Kfusion(21) = HK39*(HK13*P(4,21) + HK18*P(5,21) - HK19*P(6,21) + HK20*P(1,21) + HK21*P(0,21) - HK22*P(2,21) + HK24*P(3,21)); +Kfusion(22) = HK39*(HK13*P(4,22) + HK18*P(5,22) - HK19*P(6,22) + HK20*P(1,22) + HK21*P(0,22) - HK22*P(2,22) + HK24*P(3,22)); +Kfusion(23) = HK39*(HK13*P(4,23) + HK18*P(5,23) - HK19*P(6,23) + HK20*P(1,23) + HK21*P(0,23) - HK22*P(2,23) + HK24*P(3,23)); // Observation Jacobians - axis 1 -Hfusion.at<0>() = 2*HK35; -Hfusion.at<1>() = 2*HK36; -Hfusion.at<2>() = 2*HK38; -Hfusion.at<3>() = -2*HK41; -Hfusion.at<4>() = -2*HK42; -Hfusion.at<5>() = -HK44; -Hfusion.at<6>() = 2*HK46; +Hfusion.at<0>() = HK6; +Hfusion.at<1>() = HK40; +Hfusion.at<2>() = HK3; +Hfusion.at<3>() = -2*HK0; +Hfusion.at<4>() = -2*HK41; +Hfusion.at<5>() = HK43; +Hfusion.at<6>() = 2*HK45; // Kalman gains - axis 1 -Kfusion(0) = HK53*HK60; -Kfusion(1) = HK57*HK60; -Kfusion(2) = HK55*HK60; -Kfusion(3) = HK59*HK60; -Kfusion(4) = HK56*HK60; -Kfusion(5) = HK58*HK60; -Kfusion(6) = HK54*HK60; -Kfusion(7) = HK60*(-HK44*P(5,7) + HK47*P(2,7) + HK48*P(6,7) + HK49*P(0,7) - HK50*P(4,7) + HK51*P(1,7) - HK52*P(3,7)); -Kfusion(8) = HK60*(-HK44*P(5,8) + HK47*P(2,8) + HK48*P(6,8) + HK49*P(0,8) - HK50*P(4,8) + HK51*P(1,8) - HK52*P(3,8)); -Kfusion(9) = HK60*(-HK44*P(5,9) + HK47*P(2,9) + HK48*P(6,9) + HK49*P(0,9) - HK50*P(4,9) + HK51*P(1,9) - HK52*P(3,9)); -Kfusion(10) = HK60*(-HK44*P(5,10) + HK47*P(2,10) + HK48*P(6,10) + HK49*P(0,10) - HK50*P(4,10) + HK51*P(1,10) - HK52*P(3,10)); -Kfusion(11) = HK60*(-HK44*P(5,11) + HK47*P(2,11) + HK48*P(6,11) + HK49*P(0,11) - HK50*P(4,11) + HK51*P(1,11) - HK52*P(3,11)); -Kfusion(12) = HK60*(-HK44*P(5,12) + HK47*P(2,12) + HK48*P(6,12) + HK49*P(0,12) - HK50*P(4,12) + HK51*P(1,12) - HK52*P(3,12)); -Kfusion(13) = HK60*(-HK44*P(5,13) + HK47*P(2,13) + HK48*P(6,13) + HK49*P(0,13) - HK50*P(4,13) + HK51*P(1,13) - HK52*P(3,13)); -Kfusion(14) = HK60*(-HK44*P(5,14) + HK47*P(2,14) + HK48*P(6,14) + HK49*P(0,14) - HK50*P(4,14) + HK51*P(1,14) - HK52*P(3,14)); -Kfusion(15) = HK60*(-HK44*P(5,15) + HK47*P(2,15) + HK48*P(6,15) + HK49*P(0,15) - HK50*P(4,15) + HK51*P(1,15) - HK52*P(3,15)); -Kfusion(16) = HK60*(-HK44*P(5,16) + HK47*P(2,16) + HK48*P(6,16) + HK49*P(0,16) - HK50*P(4,16) + HK51*P(1,16) - HK52*P(3,16)); -Kfusion(17) = HK60*(-HK44*P(5,17) + HK47*P(2,17) + HK48*P(6,17) + HK49*P(0,17) - HK50*P(4,17) + HK51*P(1,17) - HK52*P(3,17)); -Kfusion(18) = HK60*(-HK44*P(5,18) + HK47*P(2,18) + HK48*P(6,18) + HK49*P(0,18) - HK50*P(4,18) + HK51*P(1,18) - HK52*P(3,18)); -Kfusion(19) = HK60*(-HK44*P(5,19) + HK47*P(2,19) + HK48*P(6,19) + HK49*P(0,19) - HK50*P(4,19) + HK51*P(1,19) - HK52*P(3,19)); -Kfusion(20) = HK60*(-HK44*P(5,20) + HK47*P(2,20) + HK48*P(6,20) + HK49*P(0,20) - HK50*P(4,20) + HK51*P(1,20) - HK52*P(3,20)); -Kfusion(21) = HK60*(-HK44*P(5,21) + HK47*P(2,21) + HK48*P(6,21) + HK49*P(0,21) - HK50*P(4,21) + HK51*P(1,21) - HK52*P(3,21)); -Kfusion(22) = HK60*(-HK44*P(5,22) + HK47*P(2,22) + HK48*P(6,22) + HK49*P(0,22) - HK50*P(4,22) + HK51*P(1,22) - HK52*P(3,22)); -Kfusion(23) = HK60*(-HK44*P(5,23) + HK47*P(2,23) + HK48*P(6,23) + HK49*P(0,23) - HK50*P(4,23) + HK51*P(1,23) - HK52*P(3,23)); +Kfusion(0) = HK49*HK59; +Kfusion(1) = HK55*HK59; +Kfusion(2) = HK53*HK59; +Kfusion(3) = HK57*HK59; +Kfusion(4) = HK51*HK59; +Kfusion(5) = HK58*HK59; +Kfusion(6) = HK50*HK59; +Kfusion(7) = HK59*(HK20*P(2,7) - HK21*P(3,7) + HK22*P(1,7) + HK24*P(0,7) + HK43*P(5,7) + HK46*P(6,7) - HK47*P(4,7)); +Kfusion(8) = HK59*(HK20*P(2,8) - HK21*P(3,8) + HK22*P(1,8) + HK24*P(0,8) + HK43*P(5,8) + HK46*P(6,8) - HK47*P(4,8)); +Kfusion(9) = HK59*(HK20*P(2,9) - HK21*P(3,9) + HK22*P(1,9) + HK24*P(0,9) + HK43*P(5,9) + HK46*P(6,9) - HK47*P(4,9)); +Kfusion(10) = HK59*(HK20*P(2,10) - HK21*P(3,10) + HK22*P(1,10) + HK24*P(0,10) + HK43*P(5,10) + HK46*P(6,10) - HK47*P(4,10)); +Kfusion(11) = HK59*(HK20*P(2,11) - HK21*P(3,11) + HK22*P(1,11) + HK24*P(0,11) + HK43*P(5,11) + HK46*P(6,11) - HK47*P(4,11)); +Kfusion(12) = HK59*(HK20*P(2,12) - HK21*P(3,12) + HK22*P(1,12) + HK24*P(0,12) + HK43*P(5,12) + HK46*P(6,12) - HK47*P(4,12)); +Kfusion(13) = HK59*(HK20*P(2,13) - HK21*P(3,13) + HK22*P(1,13) + HK24*P(0,13) + HK43*P(5,13) + HK46*P(6,13) - HK47*P(4,13)); +Kfusion(14) = HK59*(HK20*P(2,14) - HK21*P(3,14) + HK22*P(1,14) + HK24*P(0,14) + HK43*P(5,14) + HK46*P(6,14) - HK47*P(4,14)); +Kfusion(15) = HK59*(HK20*P(2,15) - HK21*P(3,15) + HK22*P(1,15) + HK24*P(0,15) + HK43*P(5,15) + HK46*P(6,15) - HK47*P(4,15)); +Kfusion(16) = HK59*(HK20*P(2,16) - HK21*P(3,16) + HK22*P(1,16) + HK24*P(0,16) + HK43*P(5,16) + HK46*P(6,16) - HK47*P(4,16)); +Kfusion(17) = HK59*(HK20*P(2,17) - HK21*P(3,17) + HK22*P(1,17) + HK24*P(0,17) + HK43*P(5,17) + HK46*P(6,17) - HK47*P(4,17)); +Kfusion(18) = HK59*(HK20*P(2,18) - HK21*P(3,18) + HK22*P(1,18) + HK24*P(0,18) + HK43*P(5,18) + HK46*P(6,18) - HK47*P(4,18)); +Kfusion(19) = HK59*(HK20*P(2,19) - HK21*P(3,19) + HK22*P(1,19) + HK24*P(0,19) + HK43*P(5,19) + HK46*P(6,19) - HK47*P(4,19)); +Kfusion(20) = HK59*(HK20*P(2,20) - HK21*P(3,20) + HK22*P(1,20) + HK24*P(0,20) + HK43*P(5,20) + HK46*P(6,20) - HK47*P(4,20)); +Kfusion(21) = HK59*(HK20*P(2,21) - HK21*P(3,21) + HK22*P(1,21) + HK24*P(0,21) + HK43*P(5,21) + HK46*P(6,21) - HK47*P(4,21)); +Kfusion(22) = HK59*(HK20*P(2,22) - HK21*P(3,22) + HK22*P(1,22) + HK24*P(0,22) + HK43*P(5,22) + HK46*P(6,22) - HK47*P(4,22)); +Kfusion(23) = HK59*(HK20*P(2,23) - HK21*P(3,23) + HK22*P(1,23) + HK24*P(0,23) + HK43*P(5,23) + HK46*P(6,23) - HK47*P(4,23)); // Observation Jacobians - axis 2 -Hfusion.at<0>() = -2*HK61; -Hfusion.at<1>() = -2*HK62; -Hfusion.at<2>() = 2*HK63; -Hfusion.at<3>() = 2*HK64; -Hfusion.at<4>() = 2*HK65; -Hfusion.at<5>() = -2*HK66; -Hfusion.at<6>() = -HK67; +Hfusion.at<0>() = HK40; +Hfusion.at<1>() = -2*HK5; +Hfusion.at<2>() = HK1; +Hfusion.at<3>() = HK3; +Hfusion.at<4>() = 2*HK60; +Hfusion.at<5>() = -2*HK61; +Hfusion.at<6>() = HK62; // Kalman gains - axis 2 -Kfusion(0) = -HK74*HK81; -Kfusion(1) = -HK80*HK81; -Kfusion(2) = -HK79*HK81; -Kfusion(3) = -HK76*HK81; -Kfusion(4) = -HK75*HK81; -Kfusion(5) = -HK77*HK81; -Kfusion(6) = -HK78*HK81; -Kfusion(7) = -HK81*(HK67*P(6,7) - HK68*P(3,7) - HK69*P(4,7) + HK70*P(0,7) + HK71*P(5,7) - HK72*P(2,7) + HK73*P(1,7)); -Kfusion(8) = -HK81*(HK67*P(6,8) - HK68*P(3,8) - HK69*P(4,8) + HK70*P(0,8) + HK71*P(5,8) - HK72*P(2,8) + HK73*P(1,8)); -Kfusion(9) = -HK81*(HK67*P(6,9) - HK68*P(3,9) - HK69*P(4,9) + HK70*P(0,9) + HK71*P(5,9) - HK72*P(2,9) + HK73*P(1,9)); -Kfusion(10) = -HK81*(HK67*P(6,10) - HK68*P(3,10) - HK69*P(4,10) + HK70*P(0,10) + HK71*P(5,10) - HK72*P(2,10) + HK73*P(1,10)); -Kfusion(11) = -HK81*(HK67*P(6,11) - HK68*P(3,11) - HK69*P(4,11) + HK70*P(0,11) + HK71*P(5,11) - HK72*P(2,11) + HK73*P(1,11)); -Kfusion(12) = -HK81*(HK67*P(6,12) - HK68*P(3,12) - HK69*P(4,12) + HK70*P(0,12) + HK71*P(5,12) - HK72*P(2,12) + HK73*P(1,12)); -Kfusion(13) = -HK81*(HK67*P(6,13) - HK68*P(3,13) - HK69*P(4,13) + HK70*P(0,13) + HK71*P(5,13) - HK72*P(2,13) + HK73*P(1,13)); -Kfusion(14) = -HK81*(HK67*P(6,14) - HK68*P(3,14) - HK69*P(4,14) + HK70*P(0,14) + HK71*P(5,14) - HK72*P(2,14) + HK73*P(1,14)); -Kfusion(15) = -HK81*(HK67*P(6,15) - HK68*P(3,15) - HK69*P(4,15) + HK70*P(0,15) + HK71*P(5,15) - HK72*P(2,15) + HK73*P(1,15)); -Kfusion(16) = -HK81*(HK67*P(6,16) - HK68*P(3,16) - HK69*P(4,16) + HK70*P(0,16) + HK71*P(5,16) - HK72*P(2,16) + HK73*P(1,16)); -Kfusion(17) = -HK81*(HK67*P(6,17) - HK68*P(3,17) - HK69*P(4,17) + HK70*P(0,17) + HK71*P(5,17) - HK72*P(2,17) + HK73*P(1,17)); -Kfusion(18) = -HK81*(HK67*P(6,18) - HK68*P(3,18) - HK69*P(4,18) + HK70*P(0,18) + HK71*P(5,18) - HK72*P(2,18) + HK73*P(1,18)); -Kfusion(19) = -HK81*(HK67*P(6,19) - HK68*P(3,19) - HK69*P(4,19) + HK70*P(0,19) + HK71*P(5,19) - HK72*P(2,19) + HK73*P(1,19)); -Kfusion(20) = -HK81*(HK67*P(6,20) - HK68*P(3,20) - HK69*P(4,20) + HK70*P(0,20) + HK71*P(5,20) - HK72*P(2,20) + HK73*P(1,20)); -Kfusion(21) = -HK81*(HK67*P(6,21) - HK68*P(3,21) - HK69*P(4,21) + HK70*P(0,21) + HK71*P(5,21) - HK72*P(2,21) + HK73*P(1,21)); -Kfusion(22) = -HK81*(HK67*P(6,22) - HK68*P(3,22) - HK69*P(4,22) + HK70*P(0,22) + HK71*P(5,22) - HK72*P(2,22) + HK73*P(1,22)); -Kfusion(23) = -HK81*(HK67*P(6,23) - HK68*P(3,23) - HK69*P(4,23) + HK70*P(0,23) + HK71*P(5,23) - HK72*P(2,23) + HK73*P(1,23)); +Kfusion(0) = HK65*HK72; +Kfusion(1) = HK69*HK72; +Kfusion(2) = HK70*HK72; +Kfusion(3) = HK68*HK72; +Kfusion(4) = HK66*HK72; +Kfusion(5) = HK67*HK72; +Kfusion(6) = HK71*HK72; +Kfusion(7) = HK72*(HK20*P(3,7) + HK21*P(2,7) + HK22*P(0,7) - HK24*P(1,7) + HK62*P(6,7) + HK63*P(4,7) - HK64*P(5,7)); +Kfusion(8) = HK72*(HK20*P(3,8) + HK21*P(2,8) + HK22*P(0,8) - HK24*P(1,8) + HK62*P(6,8) + HK63*P(4,8) - HK64*P(5,8)); +Kfusion(9) = HK72*(HK20*P(3,9) + HK21*P(2,9) + HK22*P(0,9) - HK24*P(1,9) + HK62*P(6,9) + HK63*P(4,9) - HK64*P(5,9)); +Kfusion(10) = HK72*(HK20*P(3,10) + HK21*P(2,10) + HK22*P(0,10) - HK24*P(1,10) + HK62*P(6,10) + HK63*P(4,10) - HK64*P(5,10)); +Kfusion(11) = HK72*(HK20*P(3,11) + HK21*P(2,11) + HK22*P(0,11) - HK24*P(1,11) + HK62*P(6,11) + HK63*P(4,11) - HK64*P(5,11)); +Kfusion(12) = HK72*(HK20*P(3,12) + HK21*P(2,12) + HK22*P(0,12) - HK24*P(1,12) + HK62*P(6,12) + HK63*P(4,12) - HK64*P(5,12)); +Kfusion(13) = HK72*(HK20*P(3,13) + HK21*P(2,13) + HK22*P(0,13) - HK24*P(1,13) + HK62*P(6,13) + HK63*P(4,13) - HK64*P(5,13)); +Kfusion(14) = HK72*(HK20*P(3,14) + HK21*P(2,14) + HK22*P(0,14) - HK24*P(1,14) + HK62*P(6,14) + HK63*P(4,14) - HK64*P(5,14)); +Kfusion(15) = HK72*(HK20*P(3,15) + HK21*P(2,15) + HK22*P(0,15) - HK24*P(1,15) + HK62*P(6,15) + HK63*P(4,15) - HK64*P(5,15)); +Kfusion(16) = HK72*(HK20*P(3,16) + HK21*P(2,16) + HK22*P(0,16) - HK24*P(1,16) + HK62*P(6,16) + HK63*P(4,16) - HK64*P(5,16)); +Kfusion(17) = HK72*(HK20*P(3,17) + HK21*P(2,17) + HK22*P(0,17) - HK24*P(1,17) + HK62*P(6,17) + HK63*P(4,17) - HK64*P(5,17)); +Kfusion(18) = HK72*(HK20*P(3,18) + HK21*P(2,18) + HK22*P(0,18) - HK24*P(1,18) + HK62*P(6,18) + HK63*P(4,18) - HK64*P(5,18)); +Kfusion(19) = HK72*(HK20*P(3,19) + HK21*P(2,19) + HK22*P(0,19) - HK24*P(1,19) + HK62*P(6,19) + HK63*P(4,19) - HK64*P(5,19)); +Kfusion(20) = HK72*(HK20*P(3,20) + HK21*P(2,20) + HK22*P(0,20) - HK24*P(1,20) + HK62*P(6,20) + HK63*P(4,20) - HK64*P(5,20)); +Kfusion(21) = HK72*(HK20*P(3,21) + HK21*P(2,21) + HK22*P(0,21) - HK24*P(1,21) + HK62*P(6,21) + HK63*P(4,21) - HK64*P(5,21)); +Kfusion(22) = HK72*(HK20*P(3,22) + HK21*P(2,22) + HK22*P(0,22) - HK24*P(1,22) + HK62*P(6,22) + HK63*P(4,22) - HK64*P(5,22)); +Kfusion(23) = HK72*(HK20*P(3,23) + HK21*P(2,23) + HK22*P(0,23) - HK24*P(1,23) + HK62*P(6,23) + HK63*P(4,23) - HK64*P(5,23)); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_generated.cpp index ee99c11832..47f5d1b55b 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_generated.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_generated.cpp @@ -1,19 +1,20 @@ // 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*(q2)*(q2) - 2*(q3)*(q3) + 1; +const float SA0 = 2*q3; +const float SA1 = 2*q2; +const float SA2 = SA0*q0 + SA1*q1; +const float SA3 = (q0)*(q0) + (q1)*(q1) - (q2)*(q2) - (q3)*(q3); const float SA4 = 1.0F/((SA3)*(SA3)); const float SA5 = 1.0F/((SA2)*(SA2)*SA4 + 1); const float SA6 = 1.0F/(SA3); -const float SA7 = 2*SA5*SA6; -const float SA8 = 4*SA2*SA4; +const float SA7 = SA2*SA4; +const float SA8 = 2*SA7; +const float SA9 = 2*SA6; -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<0>() = SA5*(2*SA6*q3 - SA8*q0); +H_YAW.at<1>() = SA5*(2*SA6*q2 - SA8*q1); +H_YAW.at<2>() = SA5*(SA1*SA7 + SA9*q1); +H_YAW.at<3>() = SA5*(SA0*SA7 + SA9*q0); // calculate 321 yaw observation matrix - option B @@ -21,51 +22,54 @@ const float SB0 = 2*q0; const float SB1 = 2*q1; const float SB2 = SB0*q3 + SB1*q2; const float SB3 = 1.0F/((SB2)*(SB2)); -const float SB4 = -2*(q2)*(q2) - 2*(q3)*(q3) + 1; +const float SB4 = (q0)*(q0) + (q1)*(q1) - (q2)*(q2) - (q3)*(q3); const float SB5 = 1.0F/(SB3*(SB4)*(SB4) + 1); -const float SB6 = SB3*SB4; -const float SB7 = 2*SB5*SB6; -const float SB8 = 4/SB2; +const float SB6 = 1.0F/(SB2); +const float SB7 = SB3*SB4; +const float SB8 = 2*SB7; +const float SB9 = 2*SB6; -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<0>() = -SB5*(SB0*SB6 - SB8*q3); +H_YAW.at<1>() = -SB5*(SB1*SB6 - SB8*q2); +H_YAW.at<2>() = -SB5*(-SB1*SB7 - SB9*q2); +H_YAW.at<3>() = -SB5*(-SB0*SB7 - SB9*q3); // calculate 312 yaw observation matrix - option A -const float SA0 = 2*q0; +const float SA0 = 2*q3; const float SA1 = 2*q2; -const float SA2 = SA0*q3 - SA1*q1; -const float SA3 = -2*(q1)*(q1) - 2*(q3)*(q3) + 1; +const float SA2 = SA0*q0 - SA1*q1; +const float SA3 = (q0)*(q0) - (q1)*(q1) + (q2)*(q2) - (q3)*(q3); const float SA4 = 1.0F/((SA3)*(SA3)); const float SA5 = 1.0F/((SA2)*(SA2)*SA4 + 1); const float SA6 = 1.0F/(SA3); -const float SA7 = 2*SA5*SA6; -const float SA8 = 4*SA2*SA4; +const float SA7 = SA2*SA4; +const float SA8 = 2*SA7; +const float SA9 = 2*SA6; -H_YAW.at<0>() = SA7*q3; +H_YAW.at<0>() = SA5*(2*SA6*q3 - SA8*q0); 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<2>() = SA5*(-SA1*SA7 - SA9*q1); +H_YAW.at<3>() = SA5*(SA0*SA7 + SA9*q0); // calculate 312 yaw observation matrix - option B const float SB0 = 2*q0; const float SB1 = -SB0*q3 + 2*q1*q2; const float SB2 = 1.0F/((SB1)*(SB1)); -const float SB3 = 2*(q1)*(q1) + 2*(q3)*(q3) - 1; +const float SB3 = -(q0)*(q0) + (q1)*(q1) - (q2)*(q2) + (q3)*(q3); const float SB4 = 1.0F/(SB2*(SB3)*(SB3) + 1); -const float SB5 = SB2*SB3; -const float SB6 = 2*SB4*SB5; -const float SB7 = 4/SB1; +const float SB5 = 1.0F/(SB1); +const float SB6 = 2*q1; +const float SB7 = SB2*SB3; +const float SB8 = 2*SB5; -H_YAW.at<0>() = -SB6*q3; -H_YAW.at<1>() = -SB4*(-2*SB5*q2 + SB7*q1); -H_YAW.at<2>() = SB6*q1; -H_YAW.at<3>() = -SB4*(SB0*SB5 + SB7*q3); +H_YAW.at<0>() = -SB4*(-SB0*SB5 + 2*SB2*SB3*q3); +H_YAW.at<1>() = -SB4*(SB5*SB6 - 2*SB7*q2); +H_YAW.at<2>() = -SB4*(-SB6*SB7 - SB8*q2); +H_YAW.at<3>() = -SB4*(SB0*SB7 + SB8*q3); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/main.py b/src/modules/ekf2/EKF/python/ekf_derivation/main.py index 2bb0d3b719..307b18e970 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/main.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/main.py @@ -6,16 +6,12 @@ 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): +def quat2RotSimplified(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 @@ -25,6 +21,18 @@ def quat2Rot(q): return Rot +def quat2RotUnSimplified(q): + q0 = q[0] + q1 = q[1] + q2 = q[2] + q3 = q[3] + + Rot = Matrix([[q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)], + [2*(q1*q2 + q0*q3), q0**2 - q1**2 + q2**2 - q3**2, 2*(q2*q3 - q0*q1)], + [2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0**2 - q1**2 - q2**2 + q3**2]]) + + return Rot + def create_cov_matrix(i, j): if j >= i: return Symbol("P(" + str(i) + "," + str(j) + ")", real=True) @@ -535,7 +543,7 @@ def generate_code(): # 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_earth = quat2RotSimplified(q) R_to_body = R_to_earth.T # velocity in NED local frame (north, east, down) @@ -611,6 +619,11 @@ def generate_code(): cov_code_generator.close() + # Use legacy quaternion to rotation matrix conversion for observaton equation as it gives + # simpler equations + R_to_earth = quat2RotUnSimplified(q) + R_to_body = R_to_earth.T + # derive autocode for observation methods print('Generating heading observation code ...') yaw_observation(P,state,R_to_earth)