From c988f233892315a1399080c5c963654722790abf Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 12 Jul 2021 09:03:50 +1000 Subject: [PATCH] AP_NavEKF3: convert EKF3 derivation to ftype --- .../derivation/generated/acc_bf_generated.cpp | 132 +++--- .../generated/covariance_generated.cpp | 380 +++++++++--------- .../derivation/generated/yaw_generated.cpp | 80 ++-- 3 files changed, 296 insertions(+), 296 deletions(-) diff --git a/libraries/AP_NavEKF3/derivation/generated/acc_bf_generated.cpp b/libraries/AP_NavEKF3/derivation/generated/acc_bf_generated.cpp index 7611e16006..21b18f298c 100644 --- a/libraries/AP_NavEKF3/derivation/generated/acc_bf_generated.cpp +++ b/libraries/AP_NavEKF3/derivation/generated/acc_bf_generated.cpp @@ -1,38 +1,38 @@ // Axis 0 equations // Sub Expressions -const float HK0 = vn - vwn; -const float HK1 = ve - vwe; -const float HK2 = HK0*q0 + HK1*q3 - q2*vd; -const float HK3 = 2*Kaccx; -const float HK4 = HK0*q1 + HK1*q2 + q3*vd; -const float HK5 = HK0*q2 - HK1*q1 + q0*vd; -const float HK6 = -HK0*q3 + HK1*q0 + q1*vd; -const float HK7 = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2); -const float HK8 = HK7*Kaccx; -const float HK9 = q0*q3 + q1*q2; -const float HK10 = HK3*HK9; -const float HK11 = q0*q2 - q1*q3; -const float HK12 = 2*HK9; -const float HK13 = 2*HK11; -const float HK14 = 2*HK4; -const float HK15 = 2*HK2; -const float HK16 = 2*HK5; -const float HK17 = 2*HK6; -const float HK18 = -HK12*P[0][23] + HK12*P[0][5] - HK13*P[0][6] + HK14*P[0][1] + HK15*P[0][0] - HK16*P[0][2] + HK17*P[0][3] - HK7*P[0][22] + HK7*P[0][4]; -const float HK19 = HK12*P[5][23]; -const float HK20 = -HK12*P[23][23] - HK13*P[6][23] + HK14*P[1][23] + HK15*P[0][23] - HK16*P[2][23] + HK17*P[3][23] + HK19 - HK7*P[22][23] + HK7*P[4][23]; -const float HK21 = powf(Kaccx, 2); -const float HK22 = HK12*HK21; -const float HK23 = HK12*P[5][5] - HK13*P[5][6] + HK14*P[1][5] + HK15*P[0][5] - HK16*P[2][5] + HK17*P[3][5] - HK19 + HK7*P[4][5] - HK7*P[5][22]; -const float HK24 = HK12*P[5][6] - HK12*P[6][23] - HK13*P[6][6] + HK14*P[1][6] + HK15*P[0][6] - HK16*P[2][6] + HK17*P[3][6] + HK7*P[4][6] - HK7*P[6][22]; -const float HK25 = HK7*P[4][22]; -const float HK26 = -HK12*P[4][23] + HK12*P[4][5] - HK13*P[4][6] + HK14*P[1][4] + HK15*P[0][4] - HK16*P[2][4] + HK17*P[3][4] - HK25 + HK7*P[4][4]; -const float HK27 = HK21*HK7; -const float HK28 = -HK12*P[22][23] + HK12*P[5][22] - HK13*P[6][22] + HK14*P[1][22] + HK15*P[0][22] - HK16*P[2][22] + HK17*P[3][22] + HK25 - HK7*P[22][22]; -const float HK29 = -HK12*P[1][23] + HK12*P[1][5] - HK13*P[1][6] + HK14*P[1][1] + HK15*P[0][1] - HK16*P[1][2] + HK17*P[1][3] - HK7*P[1][22] + HK7*P[1][4]; -const float HK30 = -HK12*P[2][23] + HK12*P[2][5] - HK13*P[2][6] + HK14*P[1][2] + HK15*P[0][2] - HK16*P[2][2] + HK17*P[2][3] - HK7*P[2][22] + HK7*P[2][4]; -const float HK31 = -HK12*P[3][23] + HK12*P[3][5] - HK13*P[3][6] + HK14*P[1][3] + HK15*P[0][3] - HK16*P[2][3] + HK17*P[3][3] - HK7*P[3][22] + HK7*P[3][4]; -const float HK32 = Kaccx/(-HK13*HK21*HK24 + HK14*HK21*HK29 + HK15*HK18*HK21 - HK16*HK21*HK30 + HK17*HK21*HK31 - HK20*HK22 + HK22*HK23 + HK26*HK27 - HK27*HK28 + R_ACC); +const ftype HK0 = vn - vwn; +const ftype HK1 = ve - vwe; +const ftype HK2 = HK0*q0 + HK1*q3 - q2*vd; +const ftype HK3 = 2*Kaccx; +const ftype HK4 = HK0*q1 + HK1*q2 + q3*vd; +const ftype HK5 = HK0*q2 - HK1*q1 + q0*vd; +const ftype HK6 = -HK0*q3 + HK1*q0 + q1*vd; +const ftype HK7 = sq(q0) + sq(q1) - sq(q2) - sq(q3); +const ftype HK8 = HK7*Kaccx; +const ftype HK9 = q0*q3 + q1*q2; +const ftype HK10 = HK3*HK9; +const ftype HK11 = q0*q2 - q1*q3; +const ftype HK12 = 2*HK9; +const ftype HK13 = 2*HK11; +const ftype HK14 = 2*HK4; +const ftype HK15 = 2*HK2; +const ftype HK16 = 2*HK5; +const ftype HK17 = 2*HK6; +const ftype HK18 = -HK12*P[0][23] + HK12*P[0][5] - HK13*P[0][6] + HK14*P[0][1] + HK15*P[0][0] - HK16*P[0][2] + HK17*P[0][3] - HK7*P[0][22] + HK7*P[0][4]; +const ftype HK19 = HK12*P[5][23]; +const ftype HK20 = -HK12*P[23][23] - HK13*P[6][23] + HK14*P[1][23] + HK15*P[0][23] - HK16*P[2][23] + HK17*P[3][23] + HK19 - HK7*P[22][23] + HK7*P[4][23]; +const ftype HK21 = sq(Kaccx); +const ftype HK22 = HK12*HK21; +const ftype HK23 = HK12*P[5][5] - HK13*P[5][6] + HK14*P[1][5] + HK15*P[0][5] - HK16*P[2][5] + HK17*P[3][5] - HK19 + HK7*P[4][5] - HK7*P[5][22]; +const ftype HK24 = HK12*P[5][6] - HK12*P[6][23] - HK13*P[6][6] + HK14*P[1][6] + HK15*P[0][6] - HK16*P[2][6] + HK17*P[3][6] + HK7*P[4][6] - HK7*P[6][22]; +const ftype HK25 = HK7*P[4][22]; +const ftype HK26 = -HK12*P[4][23] + HK12*P[4][5] - HK13*P[4][6] + HK14*P[1][4] + HK15*P[0][4] - HK16*P[2][4] + HK17*P[3][4] - HK25 + HK7*P[4][4]; +const ftype HK27 = HK21*HK7; +const ftype HK28 = -HK12*P[22][23] + HK12*P[5][22] - HK13*P[6][22] + HK14*P[1][22] + HK15*P[0][22] - HK16*P[2][22] + HK17*P[3][22] + HK25 - HK7*P[22][22]; +const ftype HK29 = -HK12*P[1][23] + HK12*P[1][5] - HK13*P[1][6] + HK14*P[1][1] + HK15*P[0][1] - HK16*P[1][2] + HK17*P[1][3] - HK7*P[1][22] + HK7*P[1][4]; +const ftype HK30 = -HK12*P[2][23] + HK12*P[2][5] - HK13*P[2][6] + HK14*P[1][2] + HK15*P[0][2] - HK16*P[2][2] + HK17*P[2][3] - HK7*P[2][22] + HK7*P[2][4]; +const ftype HK31 = -HK12*P[3][23] + HK12*P[3][5] - HK13*P[3][6] + HK14*P[1][3] + HK15*P[0][3] - HK16*P[2][3] + HK17*P[3][3] - HK7*P[3][22] + HK7*P[3][4]; +const ftype HK32 = Kaccx/(-HK13*HK21*HK24 + HK14*HK21*HK29 + HK15*HK18*HK21 - HK16*HK21*HK30 + HK17*HK21*HK31 - HK20*HK22 + HK22*HK23 + HK26*HK27 - HK27*HK28 + R_ACC); // Observation Jacobians @@ -91,39 +91,39 @@ Kfusion[23] = -HK20*HK32; // 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 = powf(q0, 2) - powf(q1, 2) + powf(q2, 2) - powf(q3, 2); -const float HK10 = HK9*Kaccy; -const float HK11 = q0*q1 + q2*q3; -const float HK12 = 2*HK11; -const float HK13 = 2*HK7; -const float HK14 = 2*HK5; -const float HK15 = 2*HK2; -const float HK16 = 2*HK4; -const float HK17 = 2*HK6; -const float HK18 = HK12*P[0][6] + HK13*P[0][22] - HK13*P[0][4] + HK14*P[0][2] + HK15*P[0][0] + HK16*P[0][1] - HK17*P[0][3] - HK9*P[0][23] + HK9*P[0][5]; -const float HK19 = powf(Kaccy, 2); -const float HK20 = HK12*P[6][6] - HK13*P[4][6] + HK13*P[6][22] + HK14*P[2][6] + HK15*P[0][6] + HK16*P[1][6] - HK17*P[3][6] + HK9*P[5][6] - HK9*P[6][23]; -const float HK21 = HK13*P[4][22]; -const float HK22 = HK12*P[6][22] + HK13*P[22][22] + HK14*P[2][22] + HK15*P[0][22] + HK16*P[1][22] - HK17*P[3][22] - HK21 - HK9*P[22][23] + HK9*P[5][22]; -const float HK23 = HK13*HK19; -const float HK24 = HK12*P[4][6] - HK13*P[4][4] + HK14*P[2][4] + HK15*P[0][4] + HK16*P[1][4] - HK17*P[3][4] + HK21 - HK9*P[4][23] + HK9*P[4][5]; -const float HK25 = HK9*P[5][23]; -const float HK26 = HK12*P[5][6] - HK13*P[4][5] + HK13*P[5][22] + HK14*P[2][5] + HK15*P[0][5] + HK16*P[1][5] - HK17*P[3][5] - HK25 + HK9*P[5][5]; -const float HK27 = HK19*HK9; -const float HK28 = HK12*P[6][23] + HK13*P[22][23] - HK13*P[4][23] + HK14*P[2][23] + HK15*P[0][23] + HK16*P[1][23] - HK17*P[3][23] + HK25 - HK9*P[23][23]; -const float HK29 = HK12*P[2][6] + HK13*P[2][22] - HK13*P[2][4] + HK14*P[2][2] + HK15*P[0][2] + HK16*P[1][2] - HK17*P[2][3] - HK9*P[2][23] + HK9*P[2][5]; -const float HK30 = HK12*P[1][6] + HK13*P[1][22] - HK13*P[1][4] + HK14*P[1][2] + HK15*P[0][1] + HK16*P[1][1] - HK17*P[1][3] - HK9*P[1][23] + HK9*P[1][5]; -const float HK31 = HK12*P[3][6] + HK13*P[3][22] - HK13*P[3][4] + HK14*P[2][3] + HK15*P[0][3] + HK16*P[1][3] - HK17*P[3][3] - HK9*P[3][23] + HK9*P[3][5]; -const float HK32 = Kaccy/(HK12*HK19*HK20 + HK14*HK19*HK29 + HK15*HK18*HK19 + HK16*HK19*HK30 - HK17*HK19*HK31 + HK22*HK23 - HK23*HK24 + HK26*HK27 - HK27*HK28 + R_ACC); +const ftype HK0 = ve - vwe; +const ftype HK1 = vn - vwn; +const ftype HK2 = HK0*q0 - HK1*q3 + q1*vd; +const ftype HK3 = 2*Kaccy; +const ftype HK4 = -HK0*q1 + HK1*q2 + q0*vd; +const ftype HK5 = HK0*q2 + HK1*q1 + q3*vd; +const ftype HK6 = HK0*q3 + HK1*q0 - q2*vd; +const ftype HK7 = q0*q3 - q1*q2; +const ftype HK8 = HK3*HK7; +const ftype HK9 = sq(q0) - sq(q1) + sq(q2) - sq(q3); +const ftype HK10 = HK9*Kaccy; +const ftype HK11 = q0*q1 + q2*q3; +const ftype HK12 = 2*HK11; +const ftype HK13 = 2*HK7; +const ftype HK14 = 2*HK5; +const ftype HK15 = 2*HK2; +const ftype HK16 = 2*HK4; +const ftype HK17 = 2*HK6; +const ftype HK18 = HK12*P[0][6] + HK13*P[0][22] - HK13*P[0][4] + HK14*P[0][2] + HK15*P[0][0] + HK16*P[0][1] - HK17*P[0][3] - HK9*P[0][23] + HK9*P[0][5]; +const ftype HK19 = sq(Kaccy); +const ftype HK20 = HK12*P[6][6] - HK13*P[4][6] + HK13*P[6][22] + HK14*P[2][6] + HK15*P[0][6] + HK16*P[1][6] - HK17*P[3][6] + HK9*P[5][6] - HK9*P[6][23]; +const ftype HK21 = HK13*P[4][22]; +const ftype HK22 = HK12*P[6][22] + HK13*P[22][22] + HK14*P[2][22] + HK15*P[0][22] + HK16*P[1][22] - HK17*P[3][22] - HK21 - HK9*P[22][23] + HK9*P[5][22]; +const ftype HK23 = HK13*HK19; +const ftype HK24 = HK12*P[4][6] - HK13*P[4][4] + HK14*P[2][4] + HK15*P[0][4] + HK16*P[1][4] - HK17*P[3][4] + HK21 - HK9*P[4][23] + HK9*P[4][5]; +const ftype HK25 = HK9*P[5][23]; +const ftype HK26 = HK12*P[5][6] - HK13*P[4][5] + HK13*P[5][22] + HK14*P[2][5] + HK15*P[0][5] + HK16*P[1][5] - HK17*P[3][5] - HK25 + HK9*P[5][5]; +const ftype HK27 = HK19*HK9; +const ftype HK28 = HK12*P[6][23] + HK13*P[22][23] - HK13*P[4][23] + HK14*P[2][23] + HK15*P[0][23] + HK16*P[1][23] - HK17*P[3][23] + HK25 - HK9*P[23][23]; +const ftype HK29 = HK12*P[2][6] + HK13*P[2][22] - HK13*P[2][4] + HK14*P[2][2] + HK15*P[0][2] + HK16*P[1][2] - HK17*P[2][3] - HK9*P[2][23] + HK9*P[2][5]; +const ftype HK30 = HK12*P[1][6] + HK13*P[1][22] - HK13*P[1][4] + HK14*P[1][2] + HK15*P[0][1] + HK16*P[1][1] - HK17*P[1][3] - HK9*P[1][23] + HK9*P[1][5]; +const ftype HK31 = HK12*P[3][6] + HK13*P[3][22] - HK13*P[3][4] + HK14*P[2][3] + HK15*P[0][3] + HK16*P[1][3] - HK17*P[3][3] - HK9*P[3][23] + HK9*P[3][5]; +const ftype HK32 = Kaccy/(HK12*HK19*HK20 + HK14*HK19*HK29 + HK15*HK18*HK19 + HK16*HK19*HK30 - HK17*HK19*HK31 + HK22*HK23 - HK23*HK24 + HK26*HK27 - HK27*HK28 + R_ACC); // Observation Jacobians diff --git a/libraries/AP_NavEKF3/derivation/generated/covariance_generated.cpp b/libraries/AP_NavEKF3/derivation/generated/covariance_generated.cpp index 16c850472a..8535fb4f47 100644 --- a/libraries/AP_NavEKF3/derivation/generated/covariance_generated.cpp +++ b/libraries/AP_NavEKF3/derivation/generated/covariance_generated.cpp @@ -1,191 +1,191 @@ // Equations for covariance matrix prediction, without process noise! -const float PS0 = powf(q1, 2); -const float PS1 = 0.25F*daxVar; -const float PS2 = powf(q2, 2); -const float PS3 = 0.25F*dayVar; -const float PS4 = powf(q3, 2); -const float PS5 = 0.25F*dazVar; -const float PS6 = 0.5F*q1; -const float PS7 = 0.5F*q2; -const float PS8 = PS7*P[10][11]; -const float PS9 = 0.5F*q3; -const float PS10 = PS9*P[10][12]; -const float PS11 = 0.5F*dax - 0.5F*dax_b; -const float PS12 = 0.5F*day - 0.5F*day_b; -const float PS13 = 0.5F*daz - 0.5F*daz_b; -const float PS14 = PS10 - PS11*P[1][10] - PS12*P[2][10] - PS13*P[3][10] + PS6*P[10][10] + PS8 + P[0][10]; -const float PS15 = PS6*P[10][11]; -const float PS16 = PS9*P[11][12]; -const float PS17 = -PS11*P[1][11] - PS12*P[2][11] - PS13*P[3][11] + PS15 + PS16 + PS7*P[11][11] + P[0][11]; -const float PS18 = PS6*P[10][12]; -const float PS19 = PS7*P[11][12]; -const float PS20 = -PS11*P[1][12] - PS12*P[2][12] - PS13*P[3][12] + PS18 + PS19 + PS9*P[12][12] + P[0][12]; -const float PS21 = PS12*P[1][2]; -const float PS22 = -PS13*P[1][3]; -const float PS23 = -PS11*P[1][1] - PS21 + PS22 + PS6*P[1][10] + PS7*P[1][11] + PS9*P[1][12] + P[0][1]; -const float PS24 = -PS11*P[1][2]; -const float PS25 = PS13*P[2][3]; -const float PS26 = -PS12*P[2][2] + PS24 - PS25 + PS6*P[2][10] + PS7*P[2][11] + PS9*P[2][12] + P[0][2]; -const float PS27 = PS11*P[1][3]; -const float PS28 = -PS12*P[2][3]; -const float PS29 = -PS13*P[3][3] - PS27 + PS28 + PS6*P[3][10] + PS7*P[3][11] + PS9*P[3][12] + P[0][3]; -const float PS30 = PS11*P[0][1]; -const float PS31 = PS12*P[0][2]; -const float PS32 = PS13*P[0][3]; -const float PS33 = -PS30 - PS31 - PS32 + PS6*P[0][10] + PS7*P[0][11] + PS9*P[0][12] + P[0][0]; -const float PS34 = 0.5F*q0; -const float PS35 = q2*q3; -const float PS36 = q0*q1; -const float PS37 = q1*q3; -const float PS38 = q0*q2; -const float PS39 = q1*q2; -const float PS40 = q0*q3; -const float PS41 = -PS2; -const float PS42 = powf(q0, 2); -const float PS43 = -PS4 + PS42; -const float PS44 = PS0 + PS41 + PS43; -const float PS45 = -PS11*P[1][13] - PS12*P[2][13] - PS13*P[3][13] + PS6*P[10][13] + PS7*P[11][13] + PS9*P[12][13] + P[0][13]; -const float PS46 = PS37 + PS38; -const float PS47 = -PS11*P[1][15] - PS12*P[2][15] - PS13*P[3][15] + PS6*P[10][15] + PS7*P[11][15] + PS9*P[12][15] + P[0][15]; -const float PS48 = 2*PS47; -const float PS49 = dvy - dvy_b; -const float PS50 = dvx - dvx_b; -const float PS51 = dvz - dvz_b; -const float PS52 = PS49*q0 + PS50*q3 - PS51*q1; -const float PS53 = 2*PS29; -const float PS54 = -PS39 + PS40; -const float PS55 = -PS11*P[1][14] - PS12*P[2][14] - PS13*P[3][14] + PS6*P[10][14] + PS7*P[11][14] + PS9*P[12][14] + P[0][14]; -const float PS56 = 2*PS55; -const float PS57 = -PS49*q3 + PS50*q0 + PS51*q2; -const float PS58 = 2*PS33; -const float PS59 = PS49*q1 - PS50*q2 + PS51*q0; -const float PS60 = 2*PS59; -const float PS61 = PS49*q2 + PS50*q1 + PS51*q3; -const float PS62 = 2*PS61; -const float PS63 = -PS11*P[1][4] - PS12*P[2][4] - PS13*P[3][4] + PS6*P[4][10] + PS7*P[4][11] + PS9*P[4][12] + P[0][4]; -const float PS64 = -PS0; -const float PS65 = PS2 + PS43 + PS64; -const float PS66 = PS39 + PS40; -const float PS67 = 2*PS45; -const float PS68 = -PS35 + PS36; -const float PS69 = -PS11*P[1][5] - PS12*P[2][5] - PS13*P[3][5] + PS6*P[5][10] + PS7*P[5][11] + PS9*P[5][12] + P[0][5]; -const float PS70 = PS4 + PS41 + PS42 + PS64; -const float PS71 = PS35 + PS36; -const float PS72 = 2*PS57; -const float PS73 = -PS37 + PS38; -const float PS74 = 2*PS52; -const float PS75 = -PS11*P[1][6] - PS12*P[2][6] - PS13*P[3][6] + PS6*P[6][10] + PS7*P[6][11] + PS9*P[6][12] + P[0][6]; -const float PS76 = -PS34*P[10][11]; -const float PS77 = PS11*P[0][11] - PS12*P[3][11] + PS13*P[2][11] - PS19 + PS76 + PS9*P[11][11] + P[1][11]; -const float PS78 = PS13*P[0][2]; -const float PS79 = PS12*P[0][3]; -const float PS80 = PS11*P[0][0] - PS34*P[0][10] - PS7*P[0][12] + PS78 - PS79 + PS9*P[0][11] + P[0][1]; -const float PS81 = PS11*P[0][2]; -const float PS82 = PS13*P[2][2] + PS28 - PS34*P[2][10] - PS7*P[2][12] + PS81 + PS9*P[2][11] + P[1][2]; -const float PS83 = PS9*P[10][11]; -const float PS84 = PS7*P[10][12]; -const float PS85 = PS11*P[0][10] - PS12*P[3][10] + PS13*P[2][10] - PS34*P[10][10] + PS83 - PS84 + P[1][10]; -const float PS86 = -PS34*P[10][12]; -const float PS87 = PS11*P[0][12] - PS12*P[3][12] + PS13*P[2][12] + PS16 - PS7*P[12][12] + PS86 + P[1][12]; -const float PS88 = PS11*P[0][3]; -const float PS89 = -PS12*P[3][3] + PS25 - PS34*P[3][10] - PS7*P[3][12] + PS88 + PS9*P[3][11] + P[1][3]; -const float PS90 = PS13*P[1][2]; -const float PS91 = PS12*P[1][3]; -const float PS92 = PS30 - PS34*P[1][10] - PS7*P[1][12] + PS9*P[1][11] + PS90 - PS91 + P[1][1]; -const float PS93 = PS11*P[0][13] - PS12*P[3][13] + PS13*P[2][13] - PS34*P[10][13] - PS7*P[12][13] + PS9*P[11][13] + P[1][13]; -const float PS94 = PS11*P[0][15] - PS12*P[3][15] + PS13*P[2][15] - PS34*P[10][15] - PS7*P[12][15] + PS9*P[11][15] + P[1][15]; -const float PS95 = 2*PS94; -const float PS96 = PS11*P[0][14] - PS12*P[3][14] + PS13*P[2][14] - PS34*P[10][14] - PS7*P[12][14] + PS9*P[11][14] + P[1][14]; -const float PS97 = 2*PS96; -const float PS98 = PS11*P[0][4] - PS12*P[3][4] + PS13*P[2][4] - PS34*P[4][10] - PS7*P[4][12] + PS9*P[4][11] + P[1][4]; -const float PS99 = 2*PS93; -const float PS100 = PS11*P[0][5] - PS12*P[3][5] + PS13*P[2][5] - PS34*P[5][10] - PS7*P[5][12] + PS9*P[5][11] + P[1][5]; -const float PS101 = PS11*P[0][6] - PS12*P[3][6] + PS13*P[2][6] - PS34*P[6][10] - PS7*P[6][12] + PS9*P[6][11] + P[1][6]; -const float PS102 = -PS34*P[11][12]; -const float PS103 = -PS10 + PS102 + PS11*P[3][12] + PS12*P[0][12] - PS13*P[1][12] + PS6*P[12][12] + P[2][12]; -const float PS104 = PS11*P[3][3] + PS22 - PS34*P[3][11] + PS6*P[3][12] + PS79 - PS9*P[3][10] + P[2][3]; -const float PS105 = PS13*P[0][1]; -const float PS106 = -PS105 + PS12*P[0][0] - PS34*P[0][11] + PS6*P[0][12] + PS88 - PS9*P[0][10] + P[0][2]; -const float PS107 = PS6*P[11][12]; -const float PS108 = PS107 + PS11*P[3][11] + PS12*P[0][11] - PS13*P[1][11] - PS34*P[11][11] - PS83 + P[2][11]; -const float PS109 = PS11*P[3][10] + PS12*P[0][10] - PS13*P[1][10] + PS18 + PS76 - PS9*P[10][10] + P[2][10]; -const float PS110 = PS12*P[0][1]; -const float PS111 = PS110 - PS13*P[1][1] + PS27 - PS34*P[1][11] + PS6*P[1][12] - PS9*P[1][10] + P[1][2]; -const float PS112 = PS11*P[2][3]; -const float PS113 = PS112 + PS31 - PS34*P[2][11] + PS6*P[2][12] - PS9*P[2][10] - PS90 + P[2][2]; -const float PS114 = PS11*P[3][13] + PS12*P[0][13] - PS13*P[1][13] - PS34*P[11][13] + PS6*P[12][13] - PS9*P[10][13] + P[2][13]; -const float PS115 = PS11*P[3][15] + PS12*P[0][15] - PS13*P[1][15] - PS34*P[11][15] + PS6*P[12][15] - PS9*P[10][15] + P[2][15]; -const float PS116 = 2*PS115; -const float PS117 = PS11*P[3][14] + PS12*P[0][14] - PS13*P[1][14] - PS34*P[11][14] + PS6*P[12][14] - PS9*P[10][14] + P[2][14]; -const float PS118 = 2*PS117; -const float PS119 = PS11*P[3][4] + PS12*P[0][4] - PS13*P[1][4] - PS34*P[4][11] + PS6*P[4][12] - PS9*P[4][10] + P[2][4]; -const float PS120 = 2*PS114; -const float PS121 = PS11*P[3][5] + PS12*P[0][5] - PS13*P[1][5] - PS34*P[5][11] + PS6*P[5][12] - PS9*P[5][10] + P[2][5]; -const float PS122 = PS11*P[3][6] + PS12*P[0][6] - PS13*P[1][6] - PS34*P[6][11] + PS6*P[6][12] - PS9*P[6][10] + P[2][6]; -const float PS123 = -PS11*P[2][10] + PS12*P[1][10] + PS13*P[0][10] - PS15 + PS7*P[10][10] + PS86 + P[3][10]; -const float PS124 = PS105 + PS12*P[1][1] + PS24 - PS34*P[1][12] - PS6*P[1][11] + PS7*P[1][10] + P[1][3]; -const float PS125 = PS110 + PS13*P[0][0] - PS34*P[0][12] - PS6*P[0][11] + PS7*P[0][10] - PS81 + P[0][3]; -const float PS126 = -PS107 - PS11*P[2][12] + PS12*P[1][12] + PS13*P[0][12] - PS34*P[12][12] + PS84 + P[3][12]; -const float PS127 = PS102 - PS11*P[2][11] + PS12*P[1][11] + PS13*P[0][11] - PS6*P[11][11] + PS8 + P[3][11]; -const float PS128 = -PS11*P[2][2] + PS21 - PS34*P[2][12] - PS6*P[2][11] + PS7*P[2][10] + PS78 + P[2][3]; -const float PS129 = -PS112 + PS32 - PS34*P[3][12] - PS6*P[3][11] + PS7*P[3][10] + PS91 + P[3][3]; -const float PS130 = -PS11*P[2][13] + PS12*P[1][13] + PS13*P[0][13] - PS34*P[12][13] - PS6*P[11][13] + PS7*P[10][13] + P[3][13]; -const float PS131 = -PS11*P[2][15] + PS12*P[1][15] + PS13*P[0][15] - PS34*P[12][15] - PS6*P[11][15] + PS7*P[10][15] + P[3][15]; -const float PS132 = 2*PS131; -const float PS133 = -PS11*P[2][14] + PS12*P[1][14] + PS13*P[0][14] - PS34*P[12][14] - PS6*P[11][14] + PS7*P[10][14] + P[3][14]; -const float PS134 = 2*PS133; -const float PS135 = -PS11*P[2][4] + PS12*P[1][4] + PS13*P[0][4] - PS34*P[4][12] - PS6*P[4][11] + PS7*P[4][10] + P[3][4]; -const float PS136 = 2*PS130; -const float PS137 = -PS11*P[2][5] + PS12*P[1][5] + PS13*P[0][5] - PS34*P[5][12] - PS6*P[5][11] + PS7*P[5][10] + P[3][5]; -const float PS138 = -PS11*P[2][6] + PS12*P[1][6] + PS13*P[0][6] - PS34*P[6][12] - PS6*P[6][11] + PS7*P[6][10] + P[3][6]; -const float PS139 = 2*PS46; -const float PS140 = 2*PS54; -const float PS141 = -PS139*P[13][15] + PS140*P[13][14] - PS44*P[13][13] + PS60*P[2][13] + PS62*P[1][13] + PS72*P[0][13] - PS74*P[3][13] + P[4][13]; -const float PS142 = -PS139*P[15][15] + PS140*P[14][15] - PS44*P[13][15] + PS60*P[2][15] + PS62*P[1][15] + PS72*P[0][15] - PS74*P[3][15] + P[4][15]; -const float PS143 = PS62*P[1][3]; -const float PS144 = PS72*P[0][3]; -const float PS145 = -PS139*P[3][15] + PS140*P[3][14] + PS143 + PS144 - PS44*P[3][13] + PS60*P[2][3] - PS74*P[3][3] + P[3][4]; -const float PS146 = -PS139*P[14][15] + PS140*P[14][14] - PS44*P[13][14] + PS60*P[2][14] + PS62*P[1][14] + PS72*P[0][14] - PS74*P[3][14] + P[4][14]; -const float PS147 = PS60*P[0][2]; -const float PS148 = PS74*P[0][3]; -const float PS149 = -PS139*P[0][15] + PS140*P[0][14] + PS147 - PS148 - PS44*P[0][13] + PS62*P[0][1] + PS72*P[0][0] + P[0][4]; -const float PS150 = PS62*P[1][2]; -const float PS151 = PS72*P[0][2]; -const float PS152 = -PS139*P[2][15] + PS140*P[2][14] + PS150 + PS151 - PS44*P[2][13] + PS60*P[2][2] - PS74*P[2][3] + P[2][4]; -const float PS153 = PS60*P[1][2]; -const float PS154 = PS74*P[1][3]; -const float PS155 = -PS139*P[1][15] + PS140*P[1][14] + PS153 - PS154 - PS44*P[1][13] + PS62*P[1][1] + PS72*P[0][1] + P[1][4]; -const float PS156 = 4*dvyVar; -const float PS157 = 4*dvzVar; -const float PS158 = -PS139*P[4][15] + PS140*P[4][14] - PS44*P[4][13] + PS60*P[2][4] + PS62*P[1][4] + PS72*P[0][4] - PS74*P[3][4] + P[4][4]; -const float PS159 = 2*PS141; -const float PS160 = 2*PS68; -const float PS161 = PS65*dvyVar; -const float PS162 = 2*PS66; -const float PS163 = PS44*dvxVar; -const float PS164 = -PS139*P[5][15] + PS140*P[5][14] - PS44*P[5][13] + PS60*P[2][5] + PS62*P[1][5] + PS72*P[0][5] - PS74*P[3][5] + P[4][5]; -const float PS165 = 2*PS71; -const float PS166 = 2*PS73; -const float PS167 = PS70*dvzVar; -const float PS168 = -PS139*P[6][15] + PS140*P[6][14] - PS44*P[6][13] + PS60*P[2][6] + PS62*P[1][6] + PS72*P[0][6] - PS74*P[3][6] + P[4][6]; -const float PS169 = PS160*P[14][15] - PS162*P[13][14] - PS60*P[1][14] + PS62*P[2][14] - PS65*P[14][14] + PS72*P[3][14] + PS74*P[0][14] + P[5][14]; -const float PS170 = PS160*P[13][15] - PS162*P[13][13] - PS60*P[1][13] + PS62*P[2][13] - PS65*P[13][14] + PS72*P[3][13] + PS74*P[0][13] + P[5][13]; -const float PS171 = PS74*P[0][1]; -const float PS172 = PS150 + PS160*P[1][15] - PS162*P[1][13] + PS171 - PS60*P[1][1] - PS65*P[1][14] + PS72*P[1][3] + P[1][5]; -const float PS173 = PS160*P[15][15] - PS162*P[13][15] - PS60*P[1][15] + PS62*P[2][15] - PS65*P[14][15] + PS72*P[3][15] + PS74*P[0][15] + P[5][15]; -const float PS174 = PS62*P[2][3]; -const float PS175 = PS148 + PS160*P[3][15] - PS162*P[3][13] + PS174 - PS60*P[1][3] - PS65*P[3][14] + PS72*P[3][3] + P[3][5]; -const float PS176 = PS60*P[0][1]; -const float PS177 = PS144 + PS160*P[0][15] - PS162*P[0][13] - PS176 + PS62*P[0][2] - PS65*P[0][14] + PS74*P[0][0] + P[0][5]; -const float PS178 = PS72*P[2][3]; -const float PS179 = -PS153 + PS160*P[2][15] - PS162*P[2][13] + PS178 + PS62*P[2][2] - PS65*P[2][14] + PS74*P[0][2] + P[2][5]; -const float PS180 = 4*dvxVar; -const float PS181 = PS160*P[5][15] - PS162*P[5][13] - PS60*P[1][5] + PS62*P[2][5] - PS65*P[5][14] + PS72*P[3][5] + PS74*P[0][5] + P[5][5]; -const float PS182 = PS160*P[6][15] - PS162*P[6][13] - PS60*P[1][6] + PS62*P[2][6] - PS65*P[6][14] + PS72*P[3][6] + PS74*P[0][6] + P[5][6]; -const float PS183 = -PS165*P[14][15] + PS166*P[13][15] + PS60*P[0][15] + PS62*P[3][15] - PS70*P[15][15] - PS72*P[2][15] + PS74*P[1][15] + P[6][15]; -const float PS184 = -PS165*P[14][14] + PS166*P[13][14] + PS60*P[0][14] + PS62*P[3][14] - PS70*P[14][15] - PS72*P[2][14] + PS74*P[1][14] + P[6][14]; -const float PS185 = -PS165*P[13][14] + PS166*P[13][13] + PS60*P[0][13] + PS62*P[3][13] - PS70*P[13][15] - PS72*P[2][13] + PS74*P[1][13] + P[6][13]; -const float PS186 = -PS165*P[6][14] + PS166*P[6][13] + PS60*P[0][6] + PS62*P[3][6] - PS70*P[6][15] - PS72*P[2][6] + PS74*P[1][6] + P[6][6]; +const ftype PS0 = sq(q1); +const ftype PS1 = 0.25F*daxVar; +const ftype PS2 = sq(q2); +const ftype PS3 = 0.25F*dayVar; +const ftype PS4 = sq(q3); +const ftype PS5 = 0.25F*dazVar; +const ftype PS6 = 0.5F*q1; +const ftype PS7 = 0.5F*q2; +const ftype PS8 = PS7*P[10][11]; +const ftype PS9 = 0.5F*q3; +const ftype PS10 = PS9*P[10][12]; +const ftype PS11 = 0.5F*dax - 0.5F*dax_b; +const ftype PS12 = 0.5F*day - 0.5F*day_b; +const ftype PS13 = 0.5F*daz - 0.5F*daz_b; +const ftype PS14 = PS10 - PS11*P[1][10] - PS12*P[2][10] - PS13*P[3][10] + PS6*P[10][10] + PS8 + P[0][10]; +const ftype PS15 = PS6*P[10][11]; +const ftype PS16 = PS9*P[11][12]; +const ftype PS17 = -PS11*P[1][11] - PS12*P[2][11] - PS13*P[3][11] + PS15 + PS16 + PS7*P[11][11] + P[0][11]; +const ftype PS18 = PS6*P[10][12]; +const ftype PS19 = PS7*P[11][12]; +const ftype PS20 = -PS11*P[1][12] - PS12*P[2][12] - PS13*P[3][12] + PS18 + PS19 + PS9*P[12][12] + P[0][12]; +const ftype PS21 = PS12*P[1][2]; +const ftype PS22 = -PS13*P[1][3]; +const ftype PS23 = -PS11*P[1][1] - PS21 + PS22 + PS6*P[1][10] + PS7*P[1][11] + PS9*P[1][12] + P[0][1]; +const ftype PS24 = -PS11*P[1][2]; +const ftype PS25 = PS13*P[2][3]; +const ftype PS26 = -PS12*P[2][2] + PS24 - PS25 + PS6*P[2][10] + PS7*P[2][11] + PS9*P[2][12] + P[0][2]; +const ftype PS27 = PS11*P[1][3]; +const ftype PS28 = -PS12*P[2][3]; +const ftype PS29 = -PS13*P[3][3] - PS27 + PS28 + PS6*P[3][10] + PS7*P[3][11] + PS9*P[3][12] + P[0][3]; +const ftype PS30 = PS11*P[0][1]; +const ftype PS31 = PS12*P[0][2]; +const ftype PS32 = PS13*P[0][3]; +const ftype PS33 = -PS30 - PS31 - PS32 + PS6*P[0][10] + PS7*P[0][11] + PS9*P[0][12] + P[0][0]; +const ftype PS34 = 0.5F*q0; +const ftype PS35 = q2*q3; +const ftype PS36 = q0*q1; +const ftype PS37 = q1*q3; +const ftype PS38 = q0*q2; +const ftype PS39 = q1*q2; +const ftype PS40 = q0*q3; +const ftype PS41 = -PS2; +const ftype PS42 = sq(q0); +const ftype PS43 = -PS4 + PS42; +const ftype PS44 = PS0 + PS41 + PS43; +const ftype PS45 = -PS11*P[1][13] - PS12*P[2][13] - PS13*P[3][13] + PS6*P[10][13] + PS7*P[11][13] + PS9*P[12][13] + P[0][13]; +const ftype PS46 = PS37 + PS38; +const ftype PS47 = -PS11*P[1][15] - PS12*P[2][15] - PS13*P[3][15] + PS6*P[10][15] + PS7*P[11][15] + PS9*P[12][15] + P[0][15]; +const ftype PS48 = 2*PS47; +const ftype PS49 = dvy - dvy_b; +const ftype PS50 = dvx - dvx_b; +const ftype PS51 = dvz - dvz_b; +const ftype PS52 = PS49*q0 + PS50*q3 - PS51*q1; +const ftype PS53 = 2*PS29; +const ftype PS54 = -PS39 + PS40; +const ftype PS55 = -PS11*P[1][14] - PS12*P[2][14] - PS13*P[3][14] + PS6*P[10][14] + PS7*P[11][14] + PS9*P[12][14] + P[0][14]; +const ftype PS56 = 2*PS55; +const ftype PS57 = -PS49*q3 + PS50*q0 + PS51*q2; +const ftype PS58 = 2*PS33; +const ftype PS59 = PS49*q1 - PS50*q2 + PS51*q0; +const ftype PS60 = 2*PS59; +const ftype PS61 = PS49*q2 + PS50*q1 + PS51*q3; +const ftype PS62 = 2*PS61; +const ftype PS63 = -PS11*P[1][4] - PS12*P[2][4] - PS13*P[3][4] + PS6*P[4][10] + PS7*P[4][11] + PS9*P[4][12] + P[0][4]; +const ftype PS64 = -PS0; +const ftype PS65 = PS2 + PS43 + PS64; +const ftype PS66 = PS39 + PS40; +const ftype PS67 = 2*PS45; +const ftype PS68 = -PS35 + PS36; +const ftype PS69 = -PS11*P[1][5] - PS12*P[2][5] - PS13*P[3][5] + PS6*P[5][10] + PS7*P[5][11] + PS9*P[5][12] + P[0][5]; +const ftype PS70 = PS4 + PS41 + PS42 + PS64; +const ftype PS71 = PS35 + PS36; +const ftype PS72 = 2*PS57; +const ftype PS73 = -PS37 + PS38; +const ftype PS74 = 2*PS52; +const ftype PS75 = -PS11*P[1][6] - PS12*P[2][6] - PS13*P[3][6] + PS6*P[6][10] + PS7*P[6][11] + PS9*P[6][12] + P[0][6]; +const ftype PS76 = -PS34*P[10][11]; +const ftype PS77 = PS11*P[0][11] - PS12*P[3][11] + PS13*P[2][11] - PS19 + PS76 + PS9*P[11][11] + P[1][11]; +const ftype PS78 = PS13*P[0][2]; +const ftype PS79 = PS12*P[0][3]; +const ftype PS80 = PS11*P[0][0] - PS34*P[0][10] - PS7*P[0][12] + PS78 - PS79 + PS9*P[0][11] + P[0][1]; +const ftype PS81 = PS11*P[0][2]; +const ftype PS82 = PS13*P[2][2] + PS28 - PS34*P[2][10] - PS7*P[2][12] + PS81 + PS9*P[2][11] + P[1][2]; +const ftype PS83 = PS9*P[10][11]; +const ftype PS84 = PS7*P[10][12]; +const ftype PS85 = PS11*P[0][10] - PS12*P[3][10] + PS13*P[2][10] - PS34*P[10][10] + PS83 - PS84 + P[1][10]; +const ftype PS86 = -PS34*P[10][12]; +const ftype PS87 = PS11*P[0][12] - PS12*P[3][12] + PS13*P[2][12] + PS16 - PS7*P[12][12] + PS86 + P[1][12]; +const ftype PS88 = PS11*P[0][3]; +const ftype PS89 = -PS12*P[3][3] + PS25 - PS34*P[3][10] - PS7*P[3][12] + PS88 + PS9*P[3][11] + P[1][3]; +const ftype PS90 = PS13*P[1][2]; +const ftype PS91 = PS12*P[1][3]; +const ftype PS92 = PS30 - PS34*P[1][10] - PS7*P[1][12] + PS9*P[1][11] + PS90 - PS91 + P[1][1]; +const ftype PS93 = PS11*P[0][13] - PS12*P[3][13] + PS13*P[2][13] - PS34*P[10][13] - PS7*P[12][13] + PS9*P[11][13] + P[1][13]; +const ftype PS94 = PS11*P[0][15] - PS12*P[3][15] + PS13*P[2][15] - PS34*P[10][15] - PS7*P[12][15] + PS9*P[11][15] + P[1][15]; +const ftype PS95 = 2*PS94; +const ftype PS96 = PS11*P[0][14] - PS12*P[3][14] + PS13*P[2][14] - PS34*P[10][14] - PS7*P[12][14] + PS9*P[11][14] + P[1][14]; +const ftype PS97 = 2*PS96; +const ftype PS98 = PS11*P[0][4] - PS12*P[3][4] + PS13*P[2][4] - PS34*P[4][10] - PS7*P[4][12] + PS9*P[4][11] + P[1][4]; +const ftype PS99 = 2*PS93; +const ftype PS100 = PS11*P[0][5] - PS12*P[3][5] + PS13*P[2][5] - PS34*P[5][10] - PS7*P[5][12] + PS9*P[5][11] + P[1][5]; +const ftype PS101 = PS11*P[0][6] - PS12*P[3][6] + PS13*P[2][6] - PS34*P[6][10] - PS7*P[6][12] + PS9*P[6][11] + P[1][6]; +const ftype PS102 = -PS34*P[11][12]; +const ftype PS103 = -PS10 + PS102 + PS11*P[3][12] + PS12*P[0][12] - PS13*P[1][12] + PS6*P[12][12] + P[2][12]; +const ftype PS104 = PS11*P[3][3] + PS22 - PS34*P[3][11] + PS6*P[3][12] + PS79 - PS9*P[3][10] + P[2][3]; +const ftype PS105 = PS13*P[0][1]; +const ftype PS106 = -PS105 + PS12*P[0][0] - PS34*P[0][11] + PS6*P[0][12] + PS88 - PS9*P[0][10] + P[0][2]; +const ftype PS107 = PS6*P[11][12]; +const ftype PS108 = PS107 + PS11*P[3][11] + PS12*P[0][11] - PS13*P[1][11] - PS34*P[11][11] - PS83 + P[2][11]; +const ftype PS109 = PS11*P[3][10] + PS12*P[0][10] - PS13*P[1][10] + PS18 + PS76 - PS9*P[10][10] + P[2][10]; +const ftype PS110 = PS12*P[0][1]; +const ftype PS111 = PS110 - PS13*P[1][1] + PS27 - PS34*P[1][11] + PS6*P[1][12] - PS9*P[1][10] + P[1][2]; +const ftype PS112 = PS11*P[2][3]; +const ftype PS113 = PS112 + PS31 - PS34*P[2][11] + PS6*P[2][12] - PS9*P[2][10] - PS90 + P[2][2]; +const ftype PS114 = PS11*P[3][13] + PS12*P[0][13] - PS13*P[1][13] - PS34*P[11][13] + PS6*P[12][13] - PS9*P[10][13] + P[2][13]; +const ftype PS115 = PS11*P[3][15] + PS12*P[0][15] - PS13*P[1][15] - PS34*P[11][15] + PS6*P[12][15] - PS9*P[10][15] + P[2][15]; +const ftype PS116 = 2*PS115; +const ftype PS117 = PS11*P[3][14] + PS12*P[0][14] - PS13*P[1][14] - PS34*P[11][14] + PS6*P[12][14] - PS9*P[10][14] + P[2][14]; +const ftype PS118 = 2*PS117; +const ftype PS119 = PS11*P[3][4] + PS12*P[0][4] - PS13*P[1][4] - PS34*P[4][11] + PS6*P[4][12] - PS9*P[4][10] + P[2][4]; +const ftype PS120 = 2*PS114; +const ftype PS121 = PS11*P[3][5] + PS12*P[0][5] - PS13*P[1][5] - PS34*P[5][11] + PS6*P[5][12] - PS9*P[5][10] + P[2][5]; +const ftype PS122 = PS11*P[3][6] + PS12*P[0][6] - PS13*P[1][6] - PS34*P[6][11] + PS6*P[6][12] - PS9*P[6][10] + P[2][6]; +const ftype PS123 = -PS11*P[2][10] + PS12*P[1][10] + PS13*P[0][10] - PS15 + PS7*P[10][10] + PS86 + P[3][10]; +const ftype PS124 = PS105 + PS12*P[1][1] + PS24 - PS34*P[1][12] - PS6*P[1][11] + PS7*P[1][10] + P[1][3]; +const ftype PS125 = PS110 + PS13*P[0][0] - PS34*P[0][12] - PS6*P[0][11] + PS7*P[0][10] - PS81 + P[0][3]; +const ftype PS126 = -PS107 - PS11*P[2][12] + PS12*P[1][12] + PS13*P[0][12] - PS34*P[12][12] + PS84 + P[3][12]; +const ftype PS127 = PS102 - PS11*P[2][11] + PS12*P[1][11] + PS13*P[0][11] - PS6*P[11][11] + PS8 + P[3][11]; +const ftype PS128 = -PS11*P[2][2] + PS21 - PS34*P[2][12] - PS6*P[2][11] + PS7*P[2][10] + PS78 + P[2][3]; +const ftype PS129 = -PS112 + PS32 - PS34*P[3][12] - PS6*P[3][11] + PS7*P[3][10] + PS91 + P[3][3]; +const ftype PS130 = -PS11*P[2][13] + PS12*P[1][13] + PS13*P[0][13] - PS34*P[12][13] - PS6*P[11][13] + PS7*P[10][13] + P[3][13]; +const ftype PS131 = -PS11*P[2][15] + PS12*P[1][15] + PS13*P[0][15] - PS34*P[12][15] - PS6*P[11][15] + PS7*P[10][15] + P[3][15]; +const ftype PS132 = 2*PS131; +const ftype PS133 = -PS11*P[2][14] + PS12*P[1][14] + PS13*P[0][14] - PS34*P[12][14] - PS6*P[11][14] + PS7*P[10][14] + P[3][14]; +const ftype PS134 = 2*PS133; +const ftype PS135 = -PS11*P[2][4] + PS12*P[1][4] + PS13*P[0][4] - PS34*P[4][12] - PS6*P[4][11] + PS7*P[4][10] + P[3][4]; +const ftype PS136 = 2*PS130; +const ftype PS137 = -PS11*P[2][5] + PS12*P[1][5] + PS13*P[0][5] - PS34*P[5][12] - PS6*P[5][11] + PS7*P[5][10] + P[3][5]; +const ftype PS138 = -PS11*P[2][6] + PS12*P[1][6] + PS13*P[0][6] - PS34*P[6][12] - PS6*P[6][11] + PS7*P[6][10] + P[3][6]; +const ftype PS139 = 2*PS46; +const ftype PS140 = 2*PS54; +const ftype PS141 = -PS139*P[13][15] + PS140*P[13][14] - PS44*P[13][13] + PS60*P[2][13] + PS62*P[1][13] + PS72*P[0][13] - PS74*P[3][13] + P[4][13]; +const ftype PS142 = -PS139*P[15][15] + PS140*P[14][15] - PS44*P[13][15] + PS60*P[2][15] + PS62*P[1][15] + PS72*P[0][15] - PS74*P[3][15] + P[4][15]; +const ftype PS143 = PS62*P[1][3]; +const ftype PS144 = PS72*P[0][3]; +const ftype PS145 = -PS139*P[3][15] + PS140*P[3][14] + PS143 + PS144 - PS44*P[3][13] + PS60*P[2][3] - PS74*P[3][3] + P[3][4]; +const ftype PS146 = -PS139*P[14][15] + PS140*P[14][14] - PS44*P[13][14] + PS60*P[2][14] + PS62*P[1][14] + PS72*P[0][14] - PS74*P[3][14] + P[4][14]; +const ftype PS147 = PS60*P[0][2]; +const ftype PS148 = PS74*P[0][3]; +const ftype PS149 = -PS139*P[0][15] + PS140*P[0][14] + PS147 - PS148 - PS44*P[0][13] + PS62*P[0][1] + PS72*P[0][0] + P[0][4]; +const ftype PS150 = PS62*P[1][2]; +const ftype PS151 = PS72*P[0][2]; +const ftype PS152 = -PS139*P[2][15] + PS140*P[2][14] + PS150 + PS151 - PS44*P[2][13] + PS60*P[2][2] - PS74*P[2][3] + P[2][4]; +const ftype PS153 = PS60*P[1][2]; +const ftype PS154 = PS74*P[1][3]; +const ftype PS155 = -PS139*P[1][15] + PS140*P[1][14] + PS153 - PS154 - PS44*P[1][13] + PS62*P[1][1] + PS72*P[0][1] + P[1][4]; +const ftype PS156 = 4*dvyVar; +const ftype PS157 = 4*dvzVar; +const ftype PS158 = -PS139*P[4][15] + PS140*P[4][14] - PS44*P[4][13] + PS60*P[2][4] + PS62*P[1][4] + PS72*P[0][4] - PS74*P[3][4] + P[4][4]; +const ftype PS159 = 2*PS141; +const ftype PS160 = 2*PS68; +const ftype PS161 = PS65*dvyVar; +const ftype PS162 = 2*PS66; +const ftype PS163 = PS44*dvxVar; +const ftype PS164 = -PS139*P[5][15] + PS140*P[5][14] - PS44*P[5][13] + PS60*P[2][5] + PS62*P[1][5] + PS72*P[0][5] - PS74*P[3][5] + P[4][5]; +const ftype PS165 = 2*PS71; +const ftype PS166 = 2*PS73; +const ftype PS167 = PS70*dvzVar; +const ftype PS168 = -PS139*P[6][15] + PS140*P[6][14] - PS44*P[6][13] + PS60*P[2][6] + PS62*P[1][6] + PS72*P[0][6] - PS74*P[3][6] + P[4][6]; +const ftype PS169 = PS160*P[14][15] - PS162*P[13][14] - PS60*P[1][14] + PS62*P[2][14] - PS65*P[14][14] + PS72*P[3][14] + PS74*P[0][14] + P[5][14]; +const ftype PS170 = PS160*P[13][15] - PS162*P[13][13] - PS60*P[1][13] + PS62*P[2][13] - PS65*P[13][14] + PS72*P[3][13] + PS74*P[0][13] + P[5][13]; +const ftype PS171 = PS74*P[0][1]; +const ftype PS172 = PS150 + PS160*P[1][15] - PS162*P[1][13] + PS171 - PS60*P[1][1] - PS65*P[1][14] + PS72*P[1][3] + P[1][5]; +const ftype PS173 = PS160*P[15][15] - PS162*P[13][15] - PS60*P[1][15] + PS62*P[2][15] - PS65*P[14][15] + PS72*P[3][15] + PS74*P[0][15] + P[5][15]; +const ftype PS174 = PS62*P[2][3]; +const ftype PS175 = PS148 + PS160*P[3][15] - PS162*P[3][13] + PS174 - PS60*P[1][3] - PS65*P[3][14] + PS72*P[3][3] + P[3][5]; +const ftype PS176 = PS60*P[0][1]; +const ftype PS177 = PS144 + PS160*P[0][15] - PS162*P[0][13] - PS176 + PS62*P[0][2] - PS65*P[0][14] + PS74*P[0][0] + P[0][5]; +const ftype PS178 = PS72*P[2][3]; +const ftype PS179 = -PS153 + PS160*P[2][15] - PS162*P[2][13] + PS178 + PS62*P[2][2] - PS65*P[2][14] + PS74*P[0][2] + P[2][5]; +const ftype PS180 = 4*dvxVar; +const ftype PS181 = PS160*P[5][15] - PS162*P[5][13] - PS60*P[1][5] + PS62*P[2][5] - PS65*P[5][14] + PS72*P[3][5] + PS74*P[0][5] + P[5][5]; +const ftype PS182 = PS160*P[6][15] - PS162*P[6][13] - PS60*P[1][6] + PS62*P[2][6] - PS65*P[6][14] + PS72*P[3][6] + PS74*P[0][6] + P[5][6]; +const ftype PS183 = -PS165*P[14][15] + PS166*P[13][15] + PS60*P[0][15] + PS62*P[3][15] - PS70*P[15][15] - PS72*P[2][15] + PS74*P[1][15] + P[6][15]; +const ftype PS184 = -PS165*P[14][14] + PS166*P[13][14] + PS60*P[0][14] + PS62*P[3][14] - PS70*P[14][15] - PS72*P[2][14] + PS74*P[1][14] + P[6][14]; +const ftype PS185 = -PS165*P[13][14] + PS166*P[13][13] + PS60*P[0][13] + PS62*P[3][13] - PS70*P[13][15] - PS72*P[2][13] + PS74*P[1][13] + P[6][13]; +const ftype PS186 = -PS165*P[6][14] + PS166*P[6][13] + PS60*P[0][6] + PS62*P[3][6] - PS70*P[6][15] - PS72*P[2][6] + PS74*P[1][6] + P[6][6]; nextP[0][0] = PS0*PS1 - PS11*PS23 - PS12*PS26 - PS13*PS29 + PS14*PS6 + PS17*PS7 + PS2*PS3 + PS20*PS9 + PS33 + PS4*PS5; @@ -202,20 +202,20 @@ nextP[0][4] = PS23*PS62 + PS26*PS60 - PS44*PS45 - PS46*PS48 - PS52*PS53 + PS54*P nextP[1][4] = -PS44*PS93 - PS46*PS95 + PS54*PS97 + PS60*PS82 + PS62*PS92 + PS72*PS80 - PS74*PS89 + PS98; nextP[2][4] = -PS104*PS74 + PS106*PS72 + PS111*PS62 + PS113*PS60 - PS114*PS44 - PS116*PS46 + PS118*PS54 + PS119; nextP[3][4] = PS124*PS62 + PS125*PS72 + PS128*PS60 - PS129*PS74 - PS130*PS44 - PS132*PS46 + PS134*PS54 + PS135; -nextP[4][4] = -PS139*PS142 + PS140*PS146 - PS141*PS44 - PS145*PS74 + PS149*PS72 + PS152*PS60 + PS155*PS62 + PS156*powf(PS54, 2) + PS157*powf(PS46, 2) + PS158 + powf(PS44, 2)*dvxVar; +nextP[4][4] = -PS139*PS142 + PS140*PS146 - PS141*PS44 - PS145*PS74 + PS149*PS72 + PS152*PS60 + PS155*PS62 + PS156*sq(PS54) + PS157*sq(PS46) + PS158 + sq(PS44)*dvxVar; nextP[0][5] = -PS23*PS60 + PS26*PS62 + PS48*PS68 + PS52*PS58 + PS53*PS57 - PS55*PS65 - PS66*PS67 + PS69; nextP[1][5] = PS100 - PS60*PS92 + PS62*PS82 - PS65*PS96 - PS66*PS99 + PS68*PS95 + PS72*PS89 + PS74*PS80; nextP[2][5] = PS104*PS72 + PS106*PS74 - PS111*PS60 + PS113*PS62 + PS116*PS68 - PS117*PS65 - PS120*PS66 + PS121; nextP[3][5] = -PS124*PS60 + PS125*PS74 + PS128*PS62 + PS129*PS72 + PS132*PS68 - PS133*PS65 - PS136*PS66 + PS137; nextP[4][5] = -PS140*PS161 + PS142*PS160 + PS145*PS72 - PS146*PS65 + PS149*PS74 + PS152*PS62 - PS155*PS60 - PS157*PS46*PS68 - PS159*PS66 + PS162*PS163 + PS164; -nextP[5][5] = PS157*powf(PS68, 2) + PS160*PS173 - PS162*PS170 - PS169*PS65 - PS172*PS60 + PS175*PS72 + PS177*PS74 + PS179*PS62 + PS180*powf(PS66, 2) + PS181 + powf(PS65, 2)*dvyVar; +nextP[5][5] = PS157*sq(PS68) + PS160*PS173 - PS162*PS170 - PS169*PS65 - PS172*PS60 + PS175*PS72 + PS177*PS74 + PS179*PS62 + PS180*sq(PS66) + PS181 + sq(PS65)*dvyVar; nextP[0][6] = PS23*PS74 - PS26*PS72 - PS47*PS70 + PS53*PS61 - PS56*PS71 + PS58*PS59 + PS67*PS73 + PS75; nextP[1][6] = PS101 + PS60*PS80 + PS62*PS89 - PS70*PS94 - PS71*PS97 - PS72*PS82 + PS73*PS99 + PS74*PS92; nextP[2][6] = PS104*PS62 + PS106*PS60 + PS111*PS74 - PS113*PS72 - PS115*PS70 - PS118*PS71 + PS120*PS73 + PS122; nextP[3][6] = PS124*PS74 + PS125*PS60 - PS128*PS72 + PS129*PS62 - PS131*PS70 - PS134*PS71 + PS136*PS73 + PS138; nextP[4][6] = PS139*PS167 - PS142*PS70 + PS145*PS62 - PS146*PS165 + PS149*PS60 - PS152*PS72 + PS155*PS74 - PS156*PS54*PS71 + PS159*PS73 - PS163*PS166 + PS168; nextP[5][6] = -PS160*PS167 + PS161*PS165 - PS165*PS169 + PS166*PS170 + PS172*PS74 - PS173*PS70 + PS175*PS62 + PS177*PS60 - PS179*PS72 - PS180*PS66*PS73 + PS182; -nextP[6][6] = PS156*powf(PS71, 2) - PS165*PS184 + PS166*PS185 + PS180*powf(PS73, 2) - PS183*PS70 + PS186 + PS60*(-PS151 - PS165*P[0][14] + PS166*P[0][13] + PS171 + PS60*P[0][0] + PS62*P[0][3] - PS70*P[0][15] + P[0][6]) + PS62*(PS154 - PS165*P[3][14] + PS166*P[3][13] - PS178 + PS60*P[0][3] + PS62*P[3][3] - PS70*P[3][15] + P[3][6]) + powf(PS70, 2)*dvzVar - PS72*(PS147 - PS165*P[2][14] + PS166*P[2][13] + PS174 - PS70*P[2][15] - PS72*P[2][2] + PS74*P[1][2] + P[2][6]) + PS74*(PS143 - PS165*P[1][14] + PS166*P[1][13] + PS176 - PS70*P[1][15] - PS72*P[1][2] + PS74*P[1][1] + P[1][6]); +nextP[6][6] = PS156*sq(PS71) - PS165*PS184 + PS166*PS185 + PS180*sq(PS73) - PS183*PS70 + PS186 + PS60*(-PS151 - PS165*P[0][14] + PS166*P[0][13] + PS171 + PS60*P[0][0] + PS62*P[0][3] - PS70*P[0][15] + P[0][6]) + PS62*(PS154 - PS165*P[3][14] + PS166*P[3][13] - PS178 + PS60*P[0][3] + PS62*P[3][3] - PS70*P[3][15] + P[3][6]) + sq(PS70)*dvzVar - PS72*(PS147 - PS165*P[2][14] + PS166*P[2][13] + PS174 - PS70*P[2][15] - PS72*P[2][2] + PS74*P[1][2] + P[2][6]) + PS74*(PS143 - PS165*P[1][14] + PS166*P[1][13] + PS176 - PS70*P[1][15] - PS72*P[1][2] + PS74*P[1][1] + P[1][6]); nextP[0][7] = -PS11*P[1][7] - PS12*P[2][7] - PS13*P[3][7] + PS6*P[7][10] + PS63*dt + PS7*P[7][11] + PS9*P[7][12] + P[0][7]; nextP[1][7] = PS11*P[0][7] - PS12*P[3][7] + PS13*P[2][7] - PS34*P[7][10] - PS7*P[7][12] + PS9*P[7][11] + PS98*dt + P[1][7]; nextP[2][7] = PS11*P[3][7] + PS119*dt + PS12*P[0][7] - PS13*P[1][7] - PS34*P[7][11] + PS6*P[7][12] - PS9*P[7][10] + P[2][7]; diff --git a/libraries/AP_NavEKF3/derivation/generated/yaw_generated.cpp b/libraries/AP_NavEKF3/derivation/generated/yaw_generated.cpp index 976ddd7a38..dabbccac33 100644 --- a/libraries/AP_NavEKF3/derivation/generated/yaw_generated.cpp +++ b/libraries/AP_NavEKF3/derivation/generated/yaw_generated.cpp @@ -1,14 +1,14 @@ // calculate 321 yaw observation matrix - option A -const float SA0 = 2*q3; -const float SA1 = 2*q2; -const float SA2 = SA0*q0 + SA1*q1; -const float SA3 = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2); -const float SA4 = powf(SA3, -2); -const float SA5 = 1.0F/(powf(SA2, 2)*SA4 + 1); -const float SA6 = 1.0F/SA3; -const float SA7 = SA2*SA4; -const float SA8 = 2*SA7; -const float SA9 = 2*SA6; +const ftype SA0 = 2*q3; +const ftype SA1 = 2*q2; +const ftype SA2 = SA0*q0 + SA1*q1; +const ftype SA3 = sq(q0) + sq(q1) - sq(q2) - sq(q3); +const ftype SA4 = 1.0/sq(SA3); +const ftype SA5 = 1.0F/(sq(SA2)*SA4 + 1); +const ftype SA6 = 1.0F/SA3; +const ftype SA7 = SA2*SA4; +const ftype SA8 = 2*SA7; +const ftype SA9 = 2*SA6; H_YAW[0] = SA5*(SA0*SA6 - SA8*q0); @@ -38,16 +38,16 @@ H_YAW[23] = 0; // calculate 321 yaw observation matrix - option B -const float SB0 = 2*q0; -const float SB1 = 2*q1; -const float SB2 = SB0*q3 + SB1*q2; -const float SB3 = powf(SB2, -2); -const float SB4 = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2); -const float SB5 = 1.0F/(SB3*powf(SB4, 2) + 1); -const float SB6 = 1.0F/SB2; -const float SB7 = SB3*SB4; -const float SB8 = 2*SB7; -const float SB9 = 2*SB6; +const ftype SB0 = 2*q0; +const ftype SB1 = 2*q1; +const ftype SB2 = SB0*q3 + SB1*q2; +const ftype SB3 = 1.0/sq(SB2); +const ftype SB4 = sq(q0) + sq(q1) - sq(q2) - sq(q3); +const ftype SB5 = 1.0F/(SB3*sq(SB4) + 1); +const ftype SB6 = 1.0F/SB2; +const ftype SB7 = SB3*SB4; +const ftype SB8 = 2*SB7; +const ftype SB9 = 2*SB6; H_YAW[0] = -SB5*(SB0*SB6 - SB8*q3); @@ -77,16 +77,16 @@ H_YAW[23] = 0; // calculate 312 yaw observation matrix - option A -const float SA0 = 2*q3; -const float SA1 = 2*q2; -const float SA2 = SA0*q0 - SA1*q1; -const float SA3 = powf(q0, 2) - powf(q1, 2) + powf(q2, 2) - powf(q3, 2); -const float SA4 = powf(SA3, -2); -const float SA5 = 1.0F/(powf(SA2, 2)*SA4 + 1); -const float SA6 = 1.0F/SA3; -const float SA7 = SA2*SA4; -const float SA8 = 2*SA7; -const float SA9 = 2*SA6; +const ftype SA0 = 2*q3; +const ftype SA1 = 2*q2; +const ftype SA2 = SA0*q0 - SA1*q1; +const ftype SA3 = sq(q0) - sq(q1) + sq(q2) - sq(q3); +const ftype SA4 = 1.0/sq(SA3); +const ftype SA5 = 1.0F/(sq(SA2)*SA4 + 1); +const ftype SA6 = 1.0F/SA3; +const ftype SA7 = SA2*SA4; +const ftype SA8 = 2*SA7; +const ftype SA9 = 2*SA6; H_YAW[0] = SA5*(SA0*SA6 - SA8*q0); @@ -116,16 +116,16 @@ H_YAW[23] = 0; // calculate 312 yaw observation matrix - option B -const float SB0 = 2*q0; -const float SB1 = 2*q1; -const float SB2 = -SB0*q3 + SB1*q2; -const float SB3 = powf(SB2, -2); -const float SB4 = -powf(q0, 2) + powf(q1, 2) - powf(q2, 2) + powf(q3, 2); -const float SB5 = 1.0F/(SB3*powf(SB4, 2) + 1); -const float SB6 = 1.0F/SB2; -const float SB7 = SB3*SB4; -const float SB8 = 2*SB7; -const float SB9 = 2*SB6; +const ftype SB0 = 2*q0; +const ftype SB1 = 2*q1; +const ftype SB2 = -SB0*q3 + SB1*q2; +const ftype SB3 = 1.0/sq(SB2); +const ftype SB4 = -sq(q0) + sq(q1) - sq(q2) + sq(q3); +const ftype SB5 = 1.0F/(SB3*sq(SB4) + 1); +const ftype SB6 = 1.0F/SB2; +const ftype SB7 = SB3*SB4; +const ftype SB8 = 2*SB7; +const ftype SB9 = 2*SB6; H_YAW[0] = -SB5*(-SB0*SB6 + SB8*q3);