From 34fba4dfd762cd9ed95fba2eaf0933466360f04b Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sun, 11 Aug 2024 14:46:34 -0500 Subject: [PATCH] AP_NavEKF3: document provenance of drag force fusion equations It's from the older version of the generator as well. Update variable names in the real code to be the same as the generated code. Skip generation of the unused alternate form of the equations. Delete `acc_bf_generated.cpp` as we know exactly how to generate it now. --- .../AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp | 42 ++-- libraries/AP_NavEKF3/derivation/generate_1.py | 4 +- libraries/AP_NavEKF3/derivation/generate_2.py | 14 +- .../derivation/generated/acc_bf_generated.cpp | 182 ------------------ 4 files changed, 30 insertions(+), 212 deletions(-) delete mode 100644 libraries/AP_NavEKF3/derivation/generated/acc_bf_generated.cpp diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp index 3d99531963..ca64421348 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp @@ -484,8 +484,8 @@ void NavEKF3_core::FuseSideslip() #if EK3_FEATURE_DRAG_FUSION /* * Fuse X and Y body axis specific forces using explicit algebraic equations generated with SymPy. - * See AP_NavEKF3/derivation/main.py for derivation - * Output for change reference: AP_NavEKF3/derivation/generated/acc_bf_generated.cpp + * See derivation/generate_2.py for derivation + * Output for change reference: derivation/generated/acc_bf_generated.cpp */ void NavEKF3_core::FuseDragForces() { @@ -538,20 +538,20 @@ void NavEKF3_core::FuseDragForces() if (axis_index == 0) { // drag can be modelled as an arbitrary combination of bluff body drag that proportional to // speed squared, and rotor momentum drag that is proportional to speed. - ftype Kacc; // Derivative of specific force wrt airspeed + ftype Kaccx; // Derivative of specific force wrt airspeed if (using_mcoef && using_bcoef_x) { // mixed bluff body and propeller momentum drag const ftype airSpd = (bcoef_x / rho) * (- mcoef + sqrtF(sq(mcoef) + 2.0f * (rho / bcoef_x) * fabsF(mea_acc))); - Kacc = fmaxF(1e-1f, (rho / bcoef_x) * airSpd + mcoef * density_ratio); + Kaccx = fmaxF(1e-1f, (rho / bcoef_x) * airSpd + mcoef * density_ratio); predAccel = (0.5f / bcoef_x) * rho * sq(rel_wind_body[0]) * dragForceSign - rel_wind_body[0] * mcoef * density_ratio; } else if (using_mcoef) { // propeller momentum drag only - Kacc = fmaxF(1e-1f, mcoef * density_ratio); + Kaccx = fmaxF(1e-1f, mcoef * density_ratio); predAccel = - rel_wind_body[0] * mcoef * density_ratio; } else if (using_bcoef_x) { // bluff body drag only const ftype airSpd = sqrtF((2.0f * bcoef_x * fabsF(mea_acc)) / rho); - Kacc = fmaxF(1e-1f, (rho / bcoef_x) * airSpd); + Kaccx = fmaxF(1e-1f, (rho / bcoef_x) * airSpd); predAccel = (0.5f / bcoef_x) * rho * sq(rel_wind_body[0]) * dragForceSign; } else { // skip this axis @@ -562,12 +562,12 @@ void NavEKF3_core::FuseDragForces() const ftype HK0 = vn - vwn; const ftype HK1 = ve - vwe; const ftype HK2 = HK0*q0 + HK1*q3 - q2*vd; - const ftype HK3 = 2*Kacc; + 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*Kacc; + const ftype HK8 = HK7*Kaccx; const ftype HK9 = q0*q3 + q1*q2; const ftype HK10 = HK3*HK9; const ftype HK11 = q0*q2 - q1*q3; @@ -580,7 +580,7 @@ void NavEKF3_core::FuseDragForces() 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(Kacc); + 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]; @@ -591,14 +591,14 @@ void NavEKF3_core::FuseDragForces() 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 = Kacc/(-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 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); // calculate innovation variance and exit if badly conditioned innovDragVar.x = (-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); if (innovDragVar.x < R_ACC) { return; } - const ftype HK32 = Kacc / innovDragVar.x; + const ftype HK32 = Kaccx / innovDragVar.x; // Observation Jacobians Hfusion[0] = -HK2*HK3; @@ -613,7 +613,7 @@ void NavEKF3_core::FuseDragForces() // Kalman gains // Don't allow modification of any states other than wind velocity - we only need a wind estimate. - // See AP_NavEKF3/derivation/generated/acc_bf_generated.cpp for un-implemented Kalman gain equations. + // See derivation/generated/acc_bf_generated.cpp for un-implemented Kalman gain equations. Kfusion[22] = -HK28*HK32; Kfusion[23] = -HK20*HK32; @@ -621,20 +621,20 @@ void NavEKF3_core::FuseDragForces() } else if (axis_index == 1) { // drag can be modelled as an arbitrary combination of bluff body drag that proportional to // speed squared, and rotor momentum drag that is proportional to speed. - ftype Kacc; // Derivative of specific force wrt airspeed + ftype Kaccy; // Derivative of specific force wrt airspeed if (using_mcoef && using_bcoef_y) { // mixed bluff body and propeller momentum drag const ftype airSpd = (bcoef_y / rho) * (- mcoef + sqrtF(sq(mcoef) + 2.0f * (rho / bcoef_y) * fabsF(mea_acc))); - Kacc = fmaxF(1e-1f, (rho / bcoef_y) * airSpd + mcoef * density_ratio); + Kaccy = fmaxF(1e-1f, (rho / bcoef_y) * airSpd + mcoef * density_ratio); predAccel = (0.5f / bcoef_y) * rho * sq(rel_wind_body[1]) * dragForceSign - rel_wind_body[1] * mcoef * density_ratio; } else if (using_mcoef) { // propeller momentum drag only - Kacc = fmaxF(1e-1f, mcoef * density_ratio); + Kaccy = fmaxF(1e-1f, mcoef * density_ratio); predAccel = - rel_wind_body[1] * mcoef * density_ratio; } else if (using_bcoef_y) { // bluff body drag only const ftype airSpd = sqrtF((2.0f * bcoef_y * fabsF(mea_acc)) / rho); - Kacc = fmaxF(1e-1f, (rho / bcoef_y) * airSpd); + Kaccy = fmaxF(1e-1f, (rho / bcoef_y) * airSpd); predAccel = (0.5f / bcoef_y) * rho * sq(rel_wind_body[1]) * dragForceSign; } else { // nothing more to do @@ -645,14 +645,14 @@ void NavEKF3_core::FuseDragForces() const ftype HK0 = ve - vwe; const ftype HK1 = vn - vwn; const ftype HK2 = HK0*q0 - HK1*q3 + q1*vd; - const ftype HK3 = 2*Kacc; + 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*Kacc; + const ftype HK10 = HK9*Kaccy; const ftype HK11 = q0*q1 + q2*q3; const ftype HK12 = 2*HK11; const ftype HK13 = 2*HK7; @@ -661,7 +661,7 @@ void NavEKF3_core::FuseDragForces() 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(Kacc); + 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]; @@ -681,7 +681,7 @@ void NavEKF3_core::FuseDragForces() // calculation is badly conditioned return; } - const ftype HK32 = Kacc / innovDragVar.y; + const ftype HK32 = Kaccy / innovDragVar.y; // Observation Jacobians Hfusion[0] = -HK2*HK3; @@ -696,7 +696,7 @@ void NavEKF3_core::FuseDragForces() // Kalman gains // Don't allow modification of any states other than wind velocity at this stage of development - we only need a wind estimate. - // See AP_NavEKF3/derivation/generated/acc_bf_generated.cpp for un-implemented Kalman gain equations. + // See derivation/generated/acc_bf_generated.cpp for un-implemented Kalman gain equations. Kfusion[22] = -HK22*HK32; Kfusion[23] = -HK28*HK32; } diff --git a/libraries/AP_NavEKF3/derivation/generate_1.py b/libraries/AP_NavEKF3/derivation/generate_1.py index bba10bc009..46e8722508 100755 --- a/libraries/AP_NavEKF3/derivation/generate_1.py +++ b/libraries/AP_NavEKF3/derivation/generate_1.py @@ -697,8 +697,8 @@ def generate_code(): optical_flow_observation(P,state,R_to_body,vx,vy,vz) print('Generating body frame velocity observation code ...') body_frame_velocity_observation(P,state,R_to_body,vx,vy,vz) - print('Generating body frame acceleration observation code ...') - body_frame_accel_observation(P,state,R_to_body,vx,vy,vz,wx,wy) + # print('Generating body frame acceleration observation code ...') + # body_frame_accel_observation(P,state,R_to_body,vx,vy,vz,wx,wy) print('Generating yaw estimator code ...') yaw_estimator() print('Code generation finished!') diff --git a/libraries/AP_NavEKF3/derivation/generate_2.py b/libraries/AP_NavEKF3/derivation/generate_2.py index 072a89fee3..1c4e4fc759 100755 --- a/libraries/AP_NavEKF3/derivation/generate_2.py +++ b/libraries/AP_NavEKF3/derivation/generate_2.py @@ -277,12 +277,12 @@ def body_frame_accel_observation(P,state,R_to_body,vx,vy,vz,wx,wy): acc_bf_code_generator.close() - # calculate a combined result for a possible reduction in operations, but will use more stack - equations = generate_observation_vector_equations(P,state,observation,obs_var,2) + # # calculate a combined result for a possible reduction in operations, but will use more stack + # equations = generate_observation_vector_equations(P,state,observation,obs_var,2) - acc_bf_code_generator_alt = CodeGenerator("./generated/acc_bf_generated_alt.cpp") - write_equations_to_file(equations,acc_bf_code_generator_alt,3) - acc_bf_code_generator_alt.close() + # acc_bf_code_generator_alt = CodeGenerator("./generated/acc_bf_generated_alt.cpp") + # write_equations_to_file(equations,acc_bf_code_generator_alt,3) + # acc_bf_code_generator_alt.close() return @@ -684,8 +684,8 @@ def generate_code(): # optical_flow_observation(P,state,R_to_body,vx,vy,vz) # print('Generating body frame velocity observation code ...') # body_frame_velocity_observation(P,state,R_to_body,vx,vy,vz) - # print('Generating body frame acceleration observation code ...') - # body_frame_accel_observation(P,state,R_to_body,vx,vy,vz,wx,wy) + print('Generating body frame acceleration observation code ...') + body_frame_accel_observation(P,state,R_to_body,vx,vy,vz,wx,wy) # print('Generating yaw estimator code ...') # yaw_estimator() print('Code generation finished!') diff --git a/libraries/AP_NavEKF3/derivation/generated/acc_bf_generated.cpp b/libraries/AP_NavEKF3/derivation/generated/acc_bf_generated.cpp deleted file mode 100644 index 21b18f298c..0000000000 --- a/libraries/AP_NavEKF3/derivation/generated/acc_bf_generated.cpp +++ /dev/null @@ -1,182 +0,0 @@ -// Axis 0 equations -// Sub Expressions -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 -Hfusion[0] = -HK2*HK3; -Hfusion[1] = -HK3*HK4; -Hfusion[2] = HK3*HK5; -Hfusion[3] = -HK3*HK6; -Hfusion[4] = -HK8; -Hfusion[5] = -HK10; -Hfusion[6] = HK11*HK3; -Hfusion[7] = 0; -Hfusion[8] = 0; -Hfusion[9] = 0; -Hfusion[10] = 0; -Hfusion[11] = 0; -Hfusion[12] = 0; -Hfusion[13] = 0; -Hfusion[14] = 0; -Hfusion[15] = 0; -Hfusion[16] = 0; -Hfusion[17] = 0; -Hfusion[18] = 0; -Hfusion[19] = 0; -Hfusion[20] = 0; -Hfusion[21] = 0; -Hfusion[22] = HK8; -Hfusion[23] = HK10; - - -// Kalman gains -Kfusion[0] = -HK18*HK32; -Kfusion[1] = -HK29*HK32; -Kfusion[2] = -HK30*HK32; -Kfusion[3] = -HK31*HK32; -Kfusion[4] = -HK26*HK32; -Kfusion[5] = -HK23*HK32; -Kfusion[6] = -HK24*HK32; -Kfusion[7] = -HK32*(HK12*P[5][7] - HK12*P[7][23] - HK13*P[6][7] + HK14*P[1][7] + HK15*P[0][7] - HK16*P[2][7] + HK17*P[3][7] + HK7*P[4][7] - HK7*P[7][22]); -Kfusion[8] = -HK32*(HK12*P[5][8] - HK12*P[8][23] - HK13*P[6][8] + HK14*P[1][8] + HK15*P[0][8] - HK16*P[2][8] + HK17*P[3][8] + HK7*P[4][8] - HK7*P[8][22]); -Kfusion[9] = -HK32*(HK12*P[5][9] - HK12*P[9][23] - HK13*P[6][9] + HK14*P[1][9] + HK15*P[0][9] - HK16*P[2][9] + HK17*P[3][9] + HK7*P[4][9] - HK7*P[9][22]); -Kfusion[10] = -HK32*(-HK12*P[10][23] + HK12*P[5][10] - HK13*P[6][10] + HK14*P[1][10] + HK15*P[0][10] - HK16*P[2][10] + HK17*P[3][10] - HK7*P[10][22] + HK7*P[4][10]); -Kfusion[11] = -HK32*(-HK12*P[11][23] + HK12*P[5][11] - HK13*P[6][11] + HK14*P[1][11] + HK15*P[0][11] - HK16*P[2][11] + HK17*P[3][11] - HK7*P[11][22] + HK7*P[4][11]); -Kfusion[12] = -HK32*(-HK12*P[12][23] + HK12*P[5][12] - HK13*P[6][12] + HK14*P[1][12] + HK15*P[0][12] - HK16*P[2][12] + HK17*P[3][12] - HK7*P[12][22] + HK7*P[4][12]); -Kfusion[13] = -HK32*(-HK12*P[13][23] + HK12*P[5][13] - HK13*P[6][13] + HK14*P[1][13] + HK15*P[0][13] - HK16*P[2][13] + HK17*P[3][13] - HK7*P[13][22] + HK7*P[4][13]); -Kfusion[14] = -HK32*(-HK12*P[14][23] + HK12*P[5][14] - HK13*P[6][14] + HK14*P[1][14] + HK15*P[0][14] - HK16*P[2][14] + HK17*P[3][14] - HK7*P[14][22] + HK7*P[4][14]); -Kfusion[15] = -HK32*(-HK12*P[15][23] + HK12*P[5][15] - HK13*P[6][15] + HK14*P[1][15] + HK15*P[0][15] - HK16*P[2][15] + HK17*P[3][15] - HK7*P[15][22] + HK7*P[4][15]); -Kfusion[16] = -HK32*(-HK12*P[16][23] + HK12*P[5][16] - HK13*P[6][16] + HK14*P[1][16] + HK15*P[0][16] - HK16*P[2][16] + HK17*P[3][16] - HK7*P[16][22] + HK7*P[4][16]); -Kfusion[17] = -HK32*(-HK12*P[17][23] + HK12*P[5][17] - HK13*P[6][17] + HK14*P[1][17] + HK15*P[0][17] - HK16*P[2][17] + HK17*P[3][17] - HK7*P[17][22] + HK7*P[4][17]); -Kfusion[18] = -HK32*(-HK12*P[18][23] + HK12*P[5][18] - HK13*P[6][18] + HK14*P[1][18] + HK15*P[0][18] - HK16*P[2][18] + HK17*P[3][18] - HK7*P[18][22] + HK7*P[4][18]); -Kfusion[19] = -HK32*(-HK12*P[19][23] + HK12*P[5][19] - HK13*P[6][19] + HK14*P[1][19] + HK15*P[0][19] - HK16*P[2][19] + HK17*P[3][19] - HK7*P[19][22] + HK7*P[4][19]); -Kfusion[20] = -HK32*(-HK12*P[20][23] + HK12*P[5][20] - HK13*P[6][20] + HK14*P[1][20] + HK15*P[0][20] - HK16*P[2][20] + HK17*P[3][20] - HK7*P[20][22] + HK7*P[4][20]); -Kfusion[21] = -HK32*(-HK12*P[21][23] + HK12*P[5][21] - HK13*P[6][21] + HK14*P[1][21] + HK15*P[0][21] - HK16*P[2][21] + HK17*P[3][21] - HK7*P[21][22] + HK7*P[4][21]); -Kfusion[22] = -HK28*HK32; -Kfusion[23] = -HK20*HK32; - - -// Axis 1 equations -// Sub Expressions -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 -Hfusion[0] = -HK2*HK3; -Hfusion[1] = -HK3*HK4; -Hfusion[2] = -HK3*HK5; -Hfusion[3] = HK3*HK6; -Hfusion[4] = HK8; -Hfusion[5] = -HK10; -Hfusion[6] = -HK11*HK3; -Hfusion[7] = 0; -Hfusion[8] = 0; -Hfusion[9] = 0; -Hfusion[10] = 0; -Hfusion[11] = 0; -Hfusion[12] = 0; -Hfusion[13] = 0; -Hfusion[14] = 0; -Hfusion[15] = 0; -Hfusion[16] = 0; -Hfusion[17] = 0; -Hfusion[18] = 0; -Hfusion[19] = 0; -Hfusion[20] = 0; -Hfusion[21] = 0; -Hfusion[22] = -HK8; -Hfusion[23] = HK10; - - -// Kalman gains -Kfusion[0] = -HK18*HK32; -Kfusion[1] = -HK30*HK32; -Kfusion[2] = -HK29*HK32; -Kfusion[3] = -HK31*HK32; -Kfusion[4] = -HK24*HK32; -Kfusion[5] = -HK26*HK32; -Kfusion[6] = -HK20*HK32; -Kfusion[7] = -HK32*(HK12*P[6][7] - HK13*P[4][7] + HK13*P[7][22] + HK14*P[2][7] + HK15*P[0][7] + HK16*P[1][7] - HK17*P[3][7] + HK9*P[5][7] - HK9*P[7][23]); -Kfusion[8] = -HK32*(HK12*P[6][8] - HK13*P[4][8] + HK13*P[8][22] + HK14*P[2][8] + HK15*P[0][8] + HK16*P[1][8] - HK17*P[3][8] + HK9*P[5][8] - HK9*P[8][23]); -Kfusion[9] = -HK32*(HK12*P[6][9] - HK13*P[4][9] + HK13*P[9][22] + HK14*P[2][9] + HK15*P[0][9] + HK16*P[1][9] - HK17*P[3][9] + HK9*P[5][9] - HK9*P[9][23]); -Kfusion[10] = -HK32*(HK12*P[6][10] + HK13*P[10][22] - HK13*P[4][10] + HK14*P[2][10] + HK15*P[0][10] + HK16*P[1][10] - HK17*P[3][10] - HK9*P[10][23] + HK9*P[5][10]); -Kfusion[11] = -HK32*(HK12*P[6][11] + HK13*P[11][22] - HK13*P[4][11] + HK14*P[2][11] + HK15*P[0][11] + HK16*P[1][11] - HK17*P[3][11] - HK9*P[11][23] + HK9*P[5][11]); -Kfusion[12] = -HK32*(HK12*P[6][12] + HK13*P[12][22] - HK13*P[4][12] + HK14*P[2][12] + HK15*P[0][12] + HK16*P[1][12] - HK17*P[3][12] - HK9*P[12][23] + HK9*P[5][12]); -Kfusion[13] = -HK32*(HK12*P[6][13] + HK13*P[13][22] - HK13*P[4][13] + HK14*P[2][13] + HK15*P[0][13] + HK16*P[1][13] - HK17*P[3][13] - HK9*P[13][23] + HK9*P[5][13]); -Kfusion[14] = -HK32*(HK12*P[6][14] + HK13*P[14][22] - HK13*P[4][14] + HK14*P[2][14] + HK15*P[0][14] + HK16*P[1][14] - HK17*P[3][14] - HK9*P[14][23] + HK9*P[5][14]); -Kfusion[15] = -HK32*(HK12*P[6][15] + HK13*P[15][22] - HK13*P[4][15] + HK14*P[2][15] + HK15*P[0][15] + HK16*P[1][15] - HK17*P[3][15] - HK9*P[15][23] + HK9*P[5][15]); -Kfusion[16] = -HK32*(HK12*P[6][16] + HK13*P[16][22] - HK13*P[4][16] + HK14*P[2][16] + HK15*P[0][16] + HK16*P[1][16] - HK17*P[3][16] - HK9*P[16][23] + HK9*P[5][16]); -Kfusion[17] = -HK32*(HK12*P[6][17] + HK13*P[17][22] - HK13*P[4][17] + HK14*P[2][17] + HK15*P[0][17] + HK16*P[1][17] - HK17*P[3][17] - HK9*P[17][23] + HK9*P[5][17]); -Kfusion[18] = -HK32*(HK12*P[6][18] + HK13*P[18][22] - HK13*P[4][18] + HK14*P[2][18] + HK15*P[0][18] + HK16*P[1][18] - HK17*P[3][18] - HK9*P[18][23] + HK9*P[5][18]); -Kfusion[19] = -HK32*(HK12*P[6][19] + HK13*P[19][22] - HK13*P[4][19] + HK14*P[2][19] + HK15*P[0][19] + HK16*P[1][19] - HK17*P[3][19] - HK9*P[19][23] + HK9*P[5][19]); -Kfusion[20] = -HK32*(HK12*P[6][20] + HK13*P[20][22] - HK13*P[4][20] + HK14*P[2][20] + HK15*P[0][20] + HK16*P[1][20] - HK17*P[3][20] - HK9*P[20][23] + HK9*P[5][20]); -Kfusion[21] = -HK32*(HK12*P[6][21] + HK13*P[21][22] - HK13*P[4][21] + HK14*P[2][21] + HK15*P[0][21] + HK16*P[1][21] - HK17*P[3][21] - HK9*P[21][23] + HK9*P[5][21]); -Kfusion[22] = -HK22*HK32; -Kfusion[23] = -HK28*HK32; - -