diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index 3b78e0aeda..f2b4020ef4 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -42,52 +42,30 @@ */ #include "ekf.h" +#include "python/ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h" +#include "python/ekf_derivation/generated/compute_mag_y_innov_var_and_h.h" +#include "python/ekf_derivation/generated/compute_mag_z_innov_var_and_h.h" #include bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, bool update_all_states) { - // assign intermediate variables - const float q0 = _state.quat_nominal(0); - const float q1 = _state.quat_nominal(1); - const float q2 = _state.quat_nominal(2); - const float q3 = _state.quat_nominal(3); - - const float magN = _state.mag_I(0); - const float magE = _state.mag_I(1); - const float magD = _state.mag_I(2); - // XYZ Measurement uncertainty. Need to consider timing errors for fast rotations const float R_MAG = sq(fmaxf(_params.mag_noise, 0.0f)); // calculate intermediate variables used for X axis innovation variance, observation Jacobians and Kalman gains const char* numerical_error_covariance_reset_string = "numerical error - covariance reset"; - const float HKX0 = -magD*q2 + magE*q3 + magN*q0; - const float HKX1 = magD*q3 + magE*q2 + magN*q1; - const float HKX2 = magE*q1; - const float HKX3 = magD*q0; - const float HKX4 = magN*q2; - const float HKX5 = magD*q1 + magE*q0 - magN*q3; - const float HKX6 = ecl::powf(q0, 2) + ecl::powf(q1, 2) - ecl::powf(q2, 2) - ecl::powf(q3, 2); - const float HKX7 = q0*q3 + q1*q2; - const float HKX8 = q1*q3; - const float HKX9 = q0*q2; - const float HKX10 = 2*HKX7; - const float HKX11 = -2*HKX8 + 2*HKX9; - const float HKX12 = 2*HKX1; - const float HKX13 = 2*HKX0; - const float HKX14 = -2*HKX2 + 2*HKX3 + 2*HKX4; - const float HKX15 = 2*HKX5; - const float HKX16 = HKX10*P(0,17) - HKX11*P(0,18) + HKX12*P(0,1) + HKX13*P(0,0) - HKX14*P(0,2) + HKX15*P(0,3) + HKX6*P(0,16) + P(0,19); - const float HKX17 = HKX10*P(16,17) - HKX11*P(16,18) + HKX12*P(1,16) + HKX13*P(0,16) - HKX14*P(2,16) + HKX15*P(3,16) + HKX6*P(16,16) + P(16,19); - const float HKX18 = HKX10*P(17,18) - HKX11*P(18,18) + HKX12*P(1,18) + HKX13*P(0,18) - HKX14*P(2,18) + HKX15*P(3,18) + HKX6*P(16,18) + P(18,19); - const float HKX19 = HKX10*P(2,17) - HKX11*P(2,18) + HKX12*P(1,2) + HKX13*P(0,2) - HKX14*P(2,2) + HKX15*P(2,3) + HKX6*P(2,16) + P(2,19); - const float HKX20 = HKX10*P(17,17) - HKX11*P(17,18) + HKX12*P(1,17) + HKX13*P(0,17) - HKX14*P(2,17) + HKX15*P(3,17) + HKX6*P(16,17) + P(17,19); - const float HKX21 = HKX10*P(3,17) - HKX11*P(3,18) + HKX12*P(1,3) + HKX13*P(0,3) - HKX14*P(2,3) + HKX15*P(3,3) + HKX6*P(3,16) + P(3,19); - const float HKX22 = HKX10*P(1,17) - HKX11*P(1,18) + HKX12*P(1,1) + HKX13*P(0,1) - HKX14*P(1,2) + HKX15*P(1,3) + HKX6*P(1,16) + P(1,19); - const float HKX23 = HKX10*P(17,19) - HKX11*P(18,19) + HKX12*P(1,19) + HKX13*P(0,19) - HKX14*P(2,19) + HKX15*P(3,19) + HKX6*P(16,19) + P(19,19); + Vector3f mag_innov; + Vector3f innov_var; - aid_src_mag.innovation_variance[0] = HKX10*HKX20 - HKX11*HKX18 + HKX12*HKX22 + HKX13*HKX16 - HKX14*HKX19 + HKX15*HKX21 + HKX17*HKX6 + HKX23 + R_MAG; + // Observation jacobian and Kalman gain vectors + SparseVector24f<0,1,2,3,16,17,18,19,20,21> Hfusion; + Vector24f H; + const Vector24f state_vector = getStateAtFusionHorizonAsVector(); + sym::ComputeMagInnovInnovVarAndHx(state_vector, P, mag, R_MAG, FLT_EPSILON, &mag_innov, &innov_var, &H); + Hfusion = H; + + innov_var.copyTo(aid_src_mag.innovation_variance); if (aid_src_mag.innovation_variance[0] < R_MAG) { // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned @@ -101,34 +79,6 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, b _fault_status.flags.bad_mag_x = false; - const float HKX24 = 1.0F/aid_src_mag.innovation_variance[0]; - - // intermediate variables for calculation of innovations variances for Y and Z axes - // don't calculate all terms needed for observation jacobians and Kalman gains because - // these will have to be recalculated when the X and Y axes are fused - const float IV0 = q0*q1; - const float IV1 = q2*q3; - const float IV2 = 2*IV0 + 2*IV1; - 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 = ecl::powf(q2, 2); - const float IV9 = ecl::powf(q3, 2); - const float IV10 = ecl::powf(q0, 2) - ecl::powf(q1, 2); - 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; - - aid_src_mag.innovation_variance[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; - aid_src_mag.innovation_variance[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; - // check innovation variances for being badly conditioned if (aid_src_mag.innovation_variance[1] < R_MAG) { // the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned @@ -154,21 +104,13 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, b _fault_status.flags.bad_mag_z = false; - // rotate magnetometer earth field state into body frame - const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal); - const Vector3f mag_I_rot = R_to_body * _state.mag_I; - - // compute magnetometer innovations - const Vector3f mag_observation = mag - _state.mag_B; - Vector3f mag_innov = mag_I_rot - mag_observation; - // do not use the synthesized measurement for the magnetomter Z component for 3D fusion if (_control_status.flags.synthetic_mag_z) { mag_innov(2) = 0.0f; } for (int i = 0; i < 3; i++) { - aid_src_mag.observation[i] = mag_observation(i); + aid_src_mag.observation[i] = mag(i) - _state.mag_B(i); aid_src_mag.observation_variance[i] = R_MAG; aid_src_mag.innovation[i] = mag_innov(i); } @@ -193,83 +135,23 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, b return false; } - // Observation jacobian and Kalman gain vectors - SparseVector24f<0,1,2,3,16,17,18,19,20,21> Hfusion; - Vector24f Kfusion; - bool fused[3] {false, false, false}; // update the states and covariance using sequential fusion of the magnetometer components for (uint8_t index = 0; index <= 2; index++) { - // Calculate Kalman gains and observation jacobians if (index == 0) { - // Calculate X axis observation jacobians - Hfusion.at<0>() = 2*HKX0; - Hfusion.at<1>() = 2*HKX1; - Hfusion.at<2>() = 2*HKX2 - 2*HKX3 - 2*HKX4; - Hfusion.at<3>() = 2*HKX5; - Hfusion.at<16>() = HKX6; - Hfusion.at<17>() = 2*HKX7; - Hfusion.at<18>() = 2*HKX8 - 2*HKX9; - Hfusion.at<19>() = 1; - - // Calculate X axis Kalman gains - if (update_all_states) { - Kfusion(0) = HKX16*HKX24; - Kfusion(1) = HKX22*HKX24; - Kfusion(2) = HKX19*HKX24; - Kfusion(3) = HKX21*HKX24; - - for (unsigned row = 4; row <= 15; row++) { - Kfusion(row) = HKX24*(HKX10*P(row,17) - HKX11*P(row,18) + HKX12*P(1,row) + HKX13*P(0,row) - HKX14*P(2,row) + HKX15*P(3,row) + HKX6*P(row,16) + P(row,19)); - } - - for (unsigned row = 22; row <= 23; row++) { - Kfusion(row) = HKX24*(HKX10*P(17,row) - HKX11*P(18,row) + HKX12*P(1,row) + HKX13*P(0,row) - HKX14*P(2,row) + HKX15*P(3,row) + HKX6*P(16,row) + P(19,row)); - } - } - - Kfusion(16) = HKX17*HKX24; - Kfusion(17) = HKX20*HKX24; - Kfusion(18) = HKX18*HKX24; - Kfusion(19) = HKX23*HKX24; - - for (unsigned row = 20; row <= 21; row++) { - Kfusion(row) = HKX24*(HKX10*P(17,row) - HKX11*P(18,row) + HKX12*P(1,row) + HKX13*P(0,row) - HKX14*P(2,row) + HKX15*P(3,row) + HKX6*P(16,row) + P(19,row)); - } + // everything was already computed above } else if (index == 1) { + // recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes) + sym::ComputeMagYInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src_mag.innovation_variance[index], &H); + Hfusion = H; - // recalculate innovation variance because states and covariances have changed due to previous fusion - const float HKY0 = magD*q1 + magE*q0 - magN*q3; - const float HKY1 = magD*q0 - magE*q1 + magN*q2; - const float HKY2 = magD*q3 + magE*q2 + magN*q1; - const float HKY3 = magD*q2; - const float HKY4 = magE*q3; - const float HKY5 = magN*q0; - const float HKY6 = q1*q2; - const float HKY7 = q0*q3; - const float HKY8 = ecl::powf(q0, 2) - ecl::powf(q1, 2) + ecl::powf(q2, 2) - ecl::powf(q3, 2); - const float HKY9 = q0*q1 + q2*q3; - const float HKY10 = 2*HKY9; - const float HKY11 = -2*HKY6 + 2*HKY7; - const float HKY12 = 2*HKY2; - const float HKY13 = 2*HKY0; - const float HKY14 = 2*HKY1; - const float HKY15 = -2*HKY3 + 2*HKY4 + 2*HKY5; - const float HKY16 = HKY10*P(0,18) - HKY11*P(0,16) + HKY12*P(0,2) + HKY13*P(0,0) + HKY14*P(0,1) - HKY15*P(0,3) + HKY8*P(0,17) + P(0,20); - const float HKY17 = HKY10*P(17,18) - HKY11*P(16,17) + HKY12*P(2,17) + HKY13*P(0,17) + HKY14*P(1,17) - HKY15*P(3,17) + HKY8*P(17,17) + P(17,20); - const float HKY18 = HKY10*P(16,18) - HKY11*P(16,16) + HKY12*P(2,16) + HKY13*P(0,16) + HKY14*P(1,16) - HKY15*P(3,16) + HKY8*P(16,17) + P(16,20); - const float HKY19 = HKY10*P(3,18) - HKY11*P(3,16) + HKY12*P(2,3) + HKY13*P(0,3) + HKY14*P(1,3) - HKY15*P(3,3) + HKY8*P(3,17) + P(3,20); - const float HKY20 = HKY10*P(18,18) - HKY11*P(16,18) + HKY12*P(2,18) + HKY13*P(0,18) + HKY14*P(1,18) - HKY15*P(3,18) + HKY8*P(17,18) + P(18,20); - const float HKY21 = HKY10*P(1,18) - HKY11*P(1,16) + HKY12*P(1,2) + HKY13*P(0,1) + HKY14*P(1,1) - HKY15*P(1,3) + HKY8*P(1,17) + P(1,20); - const float HKY22 = HKY10*P(2,18) - HKY11*P(2,16) + HKY12*P(2,2) + HKY13*P(0,2) + HKY14*P(1,2) - HKY15*P(2,3) + HKY8*P(2,17) + P(2,20); - const float HKY23 = HKY10*P(18,20) - HKY11*P(16,20) + HKY12*P(2,20) + HKY13*P(0,20) + HKY14*P(1,20) - HKY15*P(3,20) + HKY8*P(17,20) + P(20,20); + // recalculate innovation using the updated state + aid_src_mag.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index); - aid_src_mag.innovation_variance[1] = (HKY10*HKY20 - HKY11*HKY18 + HKY12*HKY22 + HKY13*HKY16 + HKY14*HKY21 - HKY15*HKY19 + HKY17*HKY8 + HKY23 + R_MAG); - - if (aid_src_mag.innovation_variance[1] < R_MAG) { + if (aid_src_mag.innovation_variance[index] < R_MAG) { // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned _fault_status.flags.bad_mag_y = true; @@ -278,78 +160,21 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, b ECL_ERR("magY %s", numerical_error_covariance_reset_string); return false; } - const float HKY24 = 1.0F/aid_src_mag.innovation_variance[1]; - - // Calculate Y axis observation jacobians - Hfusion.setZero(); - Hfusion.at<0>() = 2*HKY0; - Hfusion.at<1>() = 2*HKY1; - Hfusion.at<2>() = 2*HKY2; - Hfusion.at<3>() = 2*HKY3 - 2*HKY4 - 2*HKY5; - Hfusion.at<16>() = 2*HKY6 - 2*HKY7; - Hfusion.at<17>() = HKY8; - Hfusion.at<18>() = 2*HKY9; - Hfusion.at<20>() = 1; - - // Calculate Y axis Kalman gains - if (update_all_states) { - Kfusion(0) = HKY16*HKY24; - Kfusion(1) = HKY21*HKY24; - Kfusion(2) = HKY22*HKY24; - Kfusion(3) = HKY19*HKY24; - - for (unsigned row = 4; row <= 15; row++) { - Kfusion(row) = HKY24*(HKY10*P(row,18) - HKY11*P(row,16) + HKY12*P(2,row) + HKY13*P(0,row) + HKY14*P(1,row) - HKY15*P(3,row) + HKY8*P(row,17) + P(row,20)); - } - - for (unsigned row = 22; row <= 23; row++) { - Kfusion(row) = HKY24*(HKY10*P(18,row) - HKY11*P(16,row) + HKY12*P(2,row) + HKY13*P(0,row) + HKY14*P(1,row) - HKY15*P(3,row) + HKY8*P(17,row) + P(20,row)); - } - } - - Kfusion(16) = HKY18*HKY24; - Kfusion(17) = HKY17*HKY24; - Kfusion(18) = HKY20*HKY24; - Kfusion(19) = HKY24*(HKY10*P(18,19) - HKY11*P(16,19) + HKY12*P(2,19) + HKY13*P(0,19) + HKY14*P(1,19) - HKY15*P(3,19) + HKY8*P(17,19) + P(19,20)); - Kfusion(20) = HKY23*HKY24; - Kfusion(21) = HKY24*(HKY10*P(18,21) - HKY11*P(16,21) + HKY12*P(2,21) + HKY13*P(0,21) + HKY14*P(1,21) - HKY15*P(3,21) + HKY8*P(17,21) + P(20,21)); } else if (index == 2) { - // we do not fuse synthesized magnetomter measurements when doing 3D fusion if (_control_status.flags.synthetic_mag_z) { continue; } - // recalculate innovation variance becasue states and covariances have changed due to previous fusion - const float HKZ0 = magD*q0 - magE*q1 + magN*q2; - const float HKZ1 = magN*q3; - const float HKZ2 = magD*q1; - const float HKZ3 = magE*q0; - const float HKZ4 = -magD*q2 + magE*q3 + magN*q0; - const float HKZ5 = magD*q3 + magE*q2 + magN*q1; - const float HKZ6 = q0*q2 + q1*q3; - const float HKZ7 = q2*q3; - const float HKZ8 = q0*q1; - const float HKZ9 = ecl::powf(q0, 2) - ecl::powf(q1, 2) - ecl::powf(q2, 2) + ecl::powf(q3, 2); - const float HKZ10 = 2*HKZ6; - const float HKZ11 = -2*HKZ7 + 2*HKZ8; - const float HKZ12 = 2*HKZ5; - const float HKZ13 = 2*HKZ0; - const float HKZ14 = -2*HKZ1 + 2*HKZ2 + 2*HKZ3; - const float HKZ15 = 2*HKZ4; - const float HKZ16 = HKZ10*P(0,16) - HKZ11*P(0,17) + HKZ12*P(0,3) + HKZ13*P(0,0) - HKZ14*P(0,1) + HKZ15*P(0,2) + HKZ9*P(0,18) + P(0,21); - const float HKZ17 = HKZ10*P(16,18) - HKZ11*P(17,18) + HKZ12*P(3,18) + HKZ13*P(0,18) - HKZ14*P(1,18) + HKZ15*P(2,18) + HKZ9*P(18,18) + P(18,21); - const float HKZ18 = HKZ10*P(16,17) - HKZ11*P(17,17) + HKZ12*P(3,17) + HKZ13*P(0,17) - HKZ14*P(1,17) + HKZ15*P(2,17) + HKZ9*P(17,18) + P(17,21); - const float HKZ19 = HKZ10*P(1,16) - HKZ11*P(1,17) + HKZ12*P(1,3) + HKZ13*P(0,1) - HKZ14*P(1,1) + HKZ15*P(1,2) + HKZ9*P(1,18) + P(1,21); - const float HKZ20 = HKZ10*P(16,16) - HKZ11*P(16,17) + HKZ12*P(3,16) + HKZ13*P(0,16) - HKZ14*P(1,16) + HKZ15*P(2,16) + HKZ9*P(16,18) + P(16,21); - const float HKZ21 = HKZ10*P(3,16) - HKZ11*P(3,17) + HKZ12*P(3,3) + HKZ13*P(0,3) - HKZ14*P(1,3) + HKZ15*P(2,3) + HKZ9*P(3,18) + P(3,21); - const float HKZ22 = HKZ10*P(2,16) - HKZ11*P(2,17) + HKZ12*P(2,3) + HKZ13*P(0,2) - HKZ14*P(1,2) + HKZ15*P(2,2) + HKZ9*P(2,18) + P(2,21); - const float HKZ23 = HKZ10*P(16,21) - HKZ11*P(17,21) + HKZ12*P(3,21) + HKZ13*P(0,21) - HKZ14*P(1,21) + HKZ15*P(2,21) + HKZ9*P(18,21) + P(21,21); + // recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes) + sym::ComputeMagZInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src_mag.innovation_variance[index], &H); + Hfusion = H; - aid_src_mag.innovation_variance[2] = (HKZ10*HKZ20 - HKZ11*HKZ18 + HKZ12*HKZ21 + HKZ13*HKZ16 - HKZ14*HKZ19 + HKZ15*HKZ22 + HKZ17*HKZ9 + HKZ23 + R_MAG); + // recalculate innovation using the updated state + aid_src_mag.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index); - if (aid_src_mag.innovation_variance[2] < R_MAG) { + if (aid_src_mag.innovation_variance[index] < R_MAG) { // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned _fault_status.flags.bad_mag_z = true; @@ -358,45 +183,18 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, b ECL_ERR("magZ %s", numerical_error_covariance_reset_string); return false; } + } - const float HKZ24 = 1.0F/aid_src_mag.innovation_variance[2]; + Vector24f Kfusion = P * Hfusion / aid_src_mag.innovation_variance[index]; - // calculate Z axis observation jacobians - Hfusion.setZero(); - Hfusion.at<0>() = 2*HKZ0; - Hfusion.at<1>() = 2*HKZ1 - 2*HKZ2 - 2*HKZ3; - Hfusion.at<2>() = 2*HKZ4; - Hfusion.at<3>() = 2*HKZ5; - Hfusion.at<16>() = 2*HKZ6; - Hfusion.at<17>() = 2*HKZ7 - 2*HKZ8; - Hfusion.at<18>() = HKZ9; - Hfusion.at<21>() = 1; - - // Calculate Z axis Kalman gains - if (update_all_states) { - Kfusion(0) = HKZ16*HKZ24; - Kfusion(1) = HKZ19*HKZ24; - Kfusion(2) = HKZ22*HKZ24; - Kfusion(3) = HKZ21*HKZ24; - - for (unsigned row = 4; row <= 15; row++) { - Kfusion(row) = HKZ24*(HKZ10*P(row,16) - HKZ11*P(row,17) + HKZ12*P(3,row) + HKZ13*P(0,row) - HKZ14*P(1,row) + HKZ15*P(2,row) + HKZ9*P(row,18) + P(row,21)); - } - - for (unsigned row = 22; row <= 23; row++) { - Kfusion(row) = HKZ24*(HKZ10*P(16,row) - HKZ11*P(17,row) + HKZ12*P(3,row) + HKZ13*P(0,row) - HKZ14*P(1,row) + HKZ15*P(2,row) + HKZ9*P(18,row) + P(21,row)); - } + if (!update_all_states) { + for (unsigned row = 0; row <= 15; row++) { + Kfusion(row) = 0.f; } - Kfusion(16) = HKZ20*HKZ24; - Kfusion(17) = HKZ18*HKZ24; - Kfusion(18) = HKZ17*HKZ24; - - for (unsigned row = 19; row <= 20; row++) { - Kfusion(row) = HKZ24*(HKZ10*P(16,row) - HKZ11*P(17,row) + HKZ12*P(3,row) + HKZ13*P(0,row) - HKZ14*P(1,row) + HKZ15*P(2,row) + HKZ9*P(18,row) + P(row,21)); + for (unsigned row = 22; row <= 23; row++) { + Kfusion(row) = 0.f; } - - Kfusion(21) = HKZ23*HKZ24; } if (measurementUpdate(Kfusion, Hfusion, aid_src_mag.innovation[index])) { diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 1350c356fd..346a4223f2 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -195,6 +195,64 @@ def compute_sideslip_h_and_k( return (H.T, K) +def predict_mag_body(state) -> sf.V3: + q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz]) + mag_field_earth = sf.V3(state[State.ix], state[State.iy], state[State.iz]) + mag_bias_body = sf.V3(state[State.ibx], state[State.iby], state[State.ibz]) + + mag_body = quat_to_rot(q_att).T * mag_field_earth + mag_bias_body + return mag_body + +def compute_mag_innov_innov_var_and_hx( + state: VState, + P: MState, + meas: sf.V3, + R: sf.Scalar, + epsilon: sf.Scalar +) -> (sf.V3, sf.V3, VState): + + meas_pred = predict_mag_body(state); + + innov = meas_pred - meas + + innov_var = sf.V3() + Hx = sf.V1(meas_pred[0]).jacobian(state) + innov_var[0] = (Hx * P * Hx.T + R)[0,0] + Hy = sf.V1(meas_pred[1]).jacobian(state) + innov_var[1] = (Hy * P * Hy.T + R)[0,0] + Hz = sf.V1(meas_pred[2]).jacobian(state) + innov_var[2] = (Hz * P * Hz.T + R)[0,0] + + return (innov, innov_var, Hx.T) + +def compute_mag_y_innov_var_and_h( + state: VState, + P: MState, + R: sf.Scalar, + epsilon: sf.Scalar +) -> (sf.Scalar, VState): + + meas_pred = predict_mag_body(state); + + H = sf.V1(meas_pred[1]).jacobian(state) + innov_var = (H * P * H.T + R)[0,0] + + return (innov_var, H.T) + +def compute_mag_z_innov_var_and_h( + state: VState, + P: MState, + R: sf.Scalar, + epsilon: sf.Scalar +) -> (sf.Scalar, VState): + + meas_pred = predict_mag_body(state); + + H = sf.V1(meas_pred[2]).jacobian(state) + innov_var = (H * P * H.T + R)[0,0] + + return (innov_var, H.T) + print("Derive EKF2 equations...") generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"]) generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"]) @@ -202,3 +260,6 @@ generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"]) generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"]) generate_px4_function(compute_sideslip_h_and_k, output_names=["H", "K"]) generate_px4_function(predict_covariance, output_names=["P_new"]) +generate_px4_function(compute_mag_innov_innov_var_and_hx, output_names=["innov", "innov_var", "Hx"]) +generate_px4_function(compute_mag_y_innov_var_and_h, output_names=["innov_var", "H"]) +generate_px4_function(compute_mag_z_innov_var_and_h, output_names=["innov_var", "H"]) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py index 882014ca84..42970f10c8 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py @@ -101,7 +101,7 @@ def generate_px4_function(function_name, output_names): print(" |- {}".format(os.path.relpath(f, metadata.output_dir))) # Replace cstdlib and Eigen functions by PX4 equivalents - with fileinput.FileInput(os.path.abspath(metadata.generated_files[0]), inplace=True, backup='.bak') as file: + with fileinput.FileInput(os.path.abspath(metadata.generated_files[0]), inplace=True) as file: for line in file: line = line.replace("std::max", "math::max") line = line.replace("std::min", "math::min") diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h new file mode 100644 index 0000000000..a657636194 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h @@ -0,0 +1,177 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// backends/cpp/templates/function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: compute_mag_innov_innov_var_and_hx + * + * Args: + * state: Matrix24_1 + * P: Matrix24_24 + * meas: Matrix31 + * R: Scalar + * epsilon: Scalar + * + * Outputs: + * innov: Matrix31 + * innov_var: Matrix31 + * Hx: Matrix24_1 + */ +template +void ComputeMagInnovInnovVarAndHx(const matrix::Matrix& state, + const matrix::Matrix& P, + const matrix::Matrix& meas, const Scalar R, + const Scalar epsilon, + matrix::Matrix* const innov = nullptr, + matrix::Matrix* const innov_var = nullptr, + matrix::Matrix* const Hx = nullptr) { + // Total ops: 470 + + // Input arrays + + // Intermediate terms (48) + const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp1 = -_tmp0; + const Scalar _tmp2 = std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp3 = std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp4 = std::pow(state(0, 0), Scalar(2)); + const Scalar _tmp5 = -_tmp3 + _tmp4; + const Scalar _tmp6 = _tmp1 + _tmp2 + _tmp5; + const Scalar _tmp7 = state(0, 0) * state(3, 0); + const Scalar _tmp8 = state(1, 0) * state(2, 0); + const Scalar _tmp9 = 2 * state(17, 0); + const Scalar _tmp10 = state(0, 0) * state(2, 0); + const Scalar _tmp11 = state(1, 0) * state(3, 0); + const Scalar _tmp12 = 2 * state(18, 0); + const Scalar _tmp13 = -_tmp2; + const Scalar _tmp14 = _tmp0 + _tmp13 + _tmp5; + const Scalar _tmp15 = state(2, 0) * state(3, 0); + const Scalar _tmp16 = state(0, 0) * state(1, 0); + const Scalar _tmp17 = 2 * state(16, 0); + const Scalar _tmp18 = _tmp1 + _tmp13 + _tmp3 + _tmp4; + const Scalar _tmp19 = _tmp9 * state(3, 0); + const Scalar _tmp20 = _tmp12 * state(2, 0); + const Scalar _tmp21 = 2 * state(0, 0); + const Scalar _tmp22 = _tmp21 * state(16, 0); + const Scalar _tmp23 = _tmp19 - _tmp20 + _tmp22; + const Scalar _tmp24 = _tmp12 * state(3, 0) + _tmp17 * state(1, 0) + _tmp9 * state(2, 0); + const Scalar _tmp25 = _tmp17 * state(3, 0); + const Scalar _tmp26 = _tmp12 * state(1, 0); + const Scalar _tmp27 = _tmp21 * state(17, 0); + const Scalar _tmp28 = -_tmp25 + _tmp26 + _tmp27; + const Scalar _tmp29 = _tmp17 * state(2, 0); + const Scalar _tmp30 = _tmp9 * state(1, 0); + const Scalar _tmp31 = _tmp12 * state(0, 0); + const Scalar _tmp32 = -_tmp29 + _tmp30 - _tmp31; + const Scalar _tmp33 = 2 * _tmp7; + const Scalar _tmp34 = 2 * _tmp8; + const Scalar _tmp35 = _tmp33 + _tmp34; + const Scalar _tmp36 = 2 * _tmp10; + const Scalar _tmp37 = 2 * _tmp11; + const Scalar _tmp38 = -_tmp36 + _tmp37; + const Scalar _tmp39 = _tmp29 - _tmp30 + _tmp31; + const Scalar _tmp40 = -_tmp19 + _tmp20 - _tmp22; + const Scalar _tmp41 = -_tmp33 + _tmp34; + const Scalar _tmp42 = 2 * _tmp15; + const Scalar _tmp43 = 2 * _tmp16; + const Scalar _tmp44 = _tmp42 + _tmp43; + const Scalar _tmp45 = _tmp25 - _tmp26 - _tmp27; + const Scalar _tmp46 = _tmp36 + _tmp37; + const Scalar _tmp47 = _tmp42 - _tmp43; + + // Output terms (3) + if (innov != nullptr) { + matrix::Matrix& _innov = (*innov); + + _innov(0, 0) = _tmp12 * (-_tmp10 + _tmp11) + _tmp6 * state(16, 0) + _tmp9 * (_tmp7 + _tmp8) - + meas(0, 0) + state(19, 0); + _innov(1, 0) = _tmp12 * (_tmp15 + _tmp16) + _tmp14 * state(17, 0) + _tmp17 * (-_tmp7 + _tmp8) - + meas(1, 0) + state(20, 0); + _innov(2, 0) = _tmp17 * (_tmp10 + _tmp11) + _tmp18 * state(18, 0) + _tmp9 * (_tmp15 - _tmp16) - + meas(2, 0) + state(21, 0); + } + + if (innov_var != nullptr) { + matrix::Matrix& _innov_var = (*innov_var); + + _innov_var(0, 0) = + P(0, 19) * _tmp23 + P(1, 19) * _tmp24 + P(16, 19) * _tmp6 + P(17, 19) * _tmp35 + + P(18, 19) * _tmp38 + P(19, 19) + P(2, 19) * _tmp32 + P(3, 19) * _tmp28 + R + + _tmp23 * (P(0, 0) * _tmp23 + P(1, 0) * _tmp24 + P(16, 0) * _tmp6 + P(17, 0) * _tmp35 + + P(18, 0) * _tmp38 + P(19, 0) + P(2, 0) * _tmp32 + P(3, 0) * _tmp28) + + _tmp24 * (P(0, 1) * _tmp23 + P(1, 1) * _tmp24 + P(16, 1) * _tmp6 + P(17, 1) * _tmp35 + + P(18, 1) * _tmp38 + P(19, 1) + P(2, 1) * _tmp32 + P(3, 1) * _tmp28) + + _tmp28 * (P(0, 3) * _tmp23 + P(1, 3) * _tmp24 + P(16, 3) * _tmp6 + P(17, 3) * _tmp35 + + P(18, 3) * _tmp38 + P(19, 3) + P(2, 3) * _tmp32 + P(3, 3) * _tmp28) + + _tmp32 * (P(0, 2) * _tmp23 + P(1, 2) * _tmp24 + P(16, 2) * _tmp6 + P(17, 2) * _tmp35 + + P(18, 2) * _tmp38 + P(19, 2) + P(2, 2) * _tmp32 + P(3, 2) * _tmp28) + + _tmp35 * (P(0, 17) * _tmp23 + P(1, 17) * _tmp24 + P(16, 17) * _tmp6 + P(17, 17) * _tmp35 + + P(18, 17) * _tmp38 + P(19, 17) + P(2, 17) * _tmp32 + P(3, 17) * _tmp28) + + _tmp38 * (P(0, 18) * _tmp23 + P(1, 18) * _tmp24 + P(16, 18) * _tmp6 + P(17, 18) * _tmp35 + + P(18, 18) * _tmp38 + P(19, 18) + P(2, 18) * _tmp32 + P(3, 18) * _tmp28) + + _tmp6 * (P(0, 16) * _tmp23 + P(1, 16) * _tmp24 + P(16, 16) * _tmp6 + P(17, 16) * _tmp35 + + P(18, 16) * _tmp38 + P(19, 16) + P(2, 16) * _tmp32 + P(3, 16) * _tmp28); + _innov_var(1, 0) = + P(0, 20) * _tmp28 + P(1, 20) * _tmp39 + P(16, 20) * _tmp41 + P(17, 20) * _tmp14 + + P(18, 20) * _tmp44 + P(2, 20) * _tmp24 + P(20, 20) + P(3, 20) * _tmp40 + R + + _tmp14 * (P(0, 17) * _tmp28 + P(1, 17) * _tmp39 + P(16, 17) * _tmp41 + P(17, 17) * _tmp14 + + P(18, 17) * _tmp44 + P(2, 17) * _tmp24 + P(20, 17) + P(3, 17) * _tmp40) + + _tmp24 * (P(0, 2) * _tmp28 + P(1, 2) * _tmp39 + P(16, 2) * _tmp41 + P(17, 2) * _tmp14 + + P(18, 2) * _tmp44 + P(2, 2) * _tmp24 + P(20, 2) + P(3, 2) * _tmp40) + + _tmp28 * (P(0, 0) * _tmp28 + P(1, 0) * _tmp39 + P(16, 0) * _tmp41 + P(17, 0) * _tmp14 + + P(18, 0) * _tmp44 + P(2, 0) * _tmp24 + P(20, 0) + P(3, 0) * _tmp40) + + _tmp39 * (P(0, 1) * _tmp28 + P(1, 1) * _tmp39 + P(16, 1) * _tmp41 + P(17, 1) * _tmp14 + + P(18, 1) * _tmp44 + P(2, 1) * _tmp24 + P(20, 1) + P(3, 1) * _tmp40) + + _tmp40 * (P(0, 3) * _tmp28 + P(1, 3) * _tmp39 + P(16, 3) * _tmp41 + P(17, 3) * _tmp14 + + P(18, 3) * _tmp44 + P(2, 3) * _tmp24 + P(20, 3) + P(3, 3) * _tmp40) + + _tmp41 * (P(0, 16) * _tmp28 + P(1, 16) * _tmp39 + P(16, 16) * _tmp41 + P(17, 16) * _tmp14 + + P(18, 16) * _tmp44 + P(2, 16) * _tmp24 + P(20, 16) + P(3, 16) * _tmp40) + + _tmp44 * (P(0, 18) * _tmp28 + P(1, 18) * _tmp39 + P(16, 18) * _tmp41 + P(17, 18) * _tmp14 + + P(18, 18) * _tmp44 + P(2, 18) * _tmp24 + P(20, 18) + P(3, 18) * _tmp40); + _innov_var(2, 0) = + P(0, 21) * _tmp39 + P(1, 21) * _tmp45 + P(16, 21) * _tmp46 + P(17, 21) * _tmp47 + + P(18, 21) * _tmp18 + P(2, 21) * _tmp23 + P(21, 21) + P(3, 21) * _tmp24 + R + + _tmp18 * (P(0, 18) * _tmp39 + P(1, 18) * _tmp45 + P(16, 18) * _tmp46 + P(17, 18) * _tmp47 + + P(18, 18) * _tmp18 + P(2, 18) * _tmp23 + P(21, 18) + P(3, 18) * _tmp24) + + _tmp23 * (P(0, 2) * _tmp39 + P(1, 2) * _tmp45 + P(16, 2) * _tmp46 + P(17, 2) * _tmp47 + + P(18, 2) * _tmp18 + P(2, 2) * _tmp23 + P(21, 2) + P(3, 2) * _tmp24) + + _tmp24 * (P(0, 3) * _tmp39 + P(1, 3) * _tmp45 + P(16, 3) * _tmp46 + P(17, 3) * _tmp47 + + P(18, 3) * _tmp18 + P(2, 3) * _tmp23 + P(21, 3) + P(3, 3) * _tmp24) + + _tmp39 * (P(0, 0) * _tmp39 + P(1, 0) * _tmp45 + P(16, 0) * _tmp46 + P(17, 0) * _tmp47 + + P(18, 0) * _tmp18 + P(2, 0) * _tmp23 + P(21, 0) + P(3, 0) * _tmp24) + + _tmp45 * (P(0, 1) * _tmp39 + P(1, 1) * _tmp45 + P(16, 1) * _tmp46 + P(17, 1) * _tmp47 + + P(18, 1) * _tmp18 + P(2, 1) * _tmp23 + P(21, 1) + P(3, 1) * _tmp24) + + _tmp46 * (P(0, 16) * _tmp39 + P(1, 16) * _tmp45 + P(16, 16) * _tmp46 + P(17, 16) * _tmp47 + + P(18, 16) * _tmp18 + P(2, 16) * _tmp23 + P(21, 16) + P(3, 16) * _tmp24) + + _tmp47 * (P(0, 17) * _tmp39 + P(1, 17) * _tmp45 + P(16, 17) * _tmp46 + P(17, 17) * _tmp47 + + P(18, 17) * _tmp18 + P(2, 17) * _tmp23 + P(21, 17) + P(3, 17) * _tmp24); + } + + if (Hx != nullptr) { + matrix::Matrix& _Hx = (*Hx); + + _Hx.setZero(); + + _Hx(0, 0) = _tmp23; + _Hx(1, 0) = _tmp24; + _Hx(2, 0) = _tmp32; + _Hx(3, 0) = _tmp28; + _Hx(16, 0) = _tmp6; + _Hx(17, 0) = _tmp35; + _Hx(18, 0) = _tmp38; + _Hx(19, 0) = 1; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_y_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_y_innov_var_and_h.h new file mode 100644 index 0000000000..0e97debcd7 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_y_innov_var_and_h.h @@ -0,0 +1,91 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// backends/cpp/templates/function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: compute_mag_y_innov_var_and_h + * + * Args: + * state: Matrix24_1 + * P: Matrix24_24 + * R: Scalar + * epsilon: Scalar + * + * Outputs: + * innov_var: Scalar + * H: Matrix24_1 + */ +template +void ComputeMagYInnovVarAndH(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar R, + const Scalar epsilon, Scalar* const innov_var = nullptr, + matrix::Matrix* const H = nullptr) { + // Total ops: 164 + + // Input arrays + + // Intermediate terms (11) + const Scalar _tmp0 = 2 * state(3, 0); + const Scalar _tmp1 = 2 * state(1, 0); + const Scalar _tmp2 = 2 * state(0, 0); + const Scalar _tmp3 = -_tmp0 * state(16, 0) + _tmp1 * state(18, 0) + _tmp2 * state(17, 0); + const Scalar _tmp4 = 2 * state(2, 0); + const Scalar _tmp5 = -_tmp1 * state(17, 0) + _tmp2 * state(18, 0) + _tmp4 * state(16, 0); + const Scalar _tmp6 = -_tmp0 * state(17, 0) - _tmp2 * state(16, 0) + _tmp4 * state(18, 0); + const Scalar _tmp7 = _tmp0 * state(18, 0) + _tmp1 * state(16, 0) + _tmp4 * state(17, 0); + const Scalar _tmp8 = -_tmp0 * state(0, 0) + _tmp1 * state(2, 0); + const Scalar _tmp9 = _tmp0 * state(2, 0) + _tmp1 * state(0, 0); + const Scalar _tmp10 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(1, 0), Scalar(2)) + + std::pow(state(2, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2)); + + // Output terms (2) + if (innov_var != nullptr) { + Scalar& _innov_var = (*innov_var); + + _innov_var = + P(0, 20) * _tmp3 + P(1, 20) * _tmp5 + P(16, 20) * _tmp8 + P(17, 20) * _tmp10 + + P(18, 20) * _tmp9 + P(2, 20) * _tmp7 + P(20, 20) + P(3, 20) * _tmp6 + R + + _tmp10 * (P(0, 17) * _tmp3 + P(1, 17) * _tmp5 + P(16, 17) * _tmp8 + P(17, 17) * _tmp10 + + P(18, 17) * _tmp9 + P(2, 17) * _tmp7 + P(20, 17) + P(3, 17) * _tmp6) + + _tmp3 * (P(0, 0) * _tmp3 + P(1, 0) * _tmp5 + P(16, 0) * _tmp8 + P(17, 0) * _tmp10 + + P(18, 0) * _tmp9 + P(2, 0) * _tmp7 + P(20, 0) + P(3, 0) * _tmp6) + + _tmp5 * (P(0, 1) * _tmp3 + P(1, 1) * _tmp5 + P(16, 1) * _tmp8 + P(17, 1) * _tmp10 + + P(18, 1) * _tmp9 + P(2, 1) * _tmp7 + P(20, 1) + P(3, 1) * _tmp6) + + _tmp6 * (P(0, 3) * _tmp3 + P(1, 3) * _tmp5 + P(16, 3) * _tmp8 + P(17, 3) * _tmp10 + + P(18, 3) * _tmp9 + P(2, 3) * _tmp7 + P(20, 3) + P(3, 3) * _tmp6) + + _tmp7 * (P(0, 2) * _tmp3 + P(1, 2) * _tmp5 + P(16, 2) * _tmp8 + P(17, 2) * _tmp10 + + P(18, 2) * _tmp9 + P(2, 2) * _tmp7 + P(20, 2) + P(3, 2) * _tmp6) + + _tmp8 * (P(0, 16) * _tmp3 + P(1, 16) * _tmp5 + P(16, 16) * _tmp8 + P(17, 16) * _tmp10 + + P(18, 16) * _tmp9 + P(2, 16) * _tmp7 + P(20, 16) + P(3, 16) * _tmp6) + + _tmp9 * (P(0, 18) * _tmp3 + P(1, 18) * _tmp5 + P(16, 18) * _tmp8 + P(17, 18) * _tmp10 + + P(18, 18) * _tmp9 + P(2, 18) * _tmp7 + P(20, 18) + P(3, 18) * _tmp6); + } + + if (H != nullptr) { + matrix::Matrix& _H = (*H); + + _H.setZero(); + + _H(0, 0) = _tmp3; + _H(1, 0) = _tmp5; + _H(2, 0) = _tmp7; + _H(3, 0) = _tmp6; + _H(16, 0) = _tmp8; + _H(17, 0) = _tmp10; + _H(18, 0) = _tmp9; + _H(20, 0) = 1; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_z_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_z_innov_var_and_h.h new file mode 100644 index 0000000000..1cac4d6876 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_z_innov_var_and_h.h @@ -0,0 +1,91 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// backends/cpp/templates/function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: compute_mag_z_innov_var_and_h + * + * Args: + * state: Matrix24_1 + * P: Matrix24_24 + * R: Scalar + * epsilon: Scalar + * + * Outputs: + * innov_var: Scalar + * H: Matrix24_1 + */ +template +void ComputeMagZInnovVarAndH(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar R, + const Scalar epsilon, Scalar* const innov_var = nullptr, + matrix::Matrix* const H = nullptr) { + // Total ops: 164 + + // Input arrays + + // Intermediate terms (11) + const Scalar _tmp0 = 2 * state(2, 0); + const Scalar _tmp1 = 2 * state(1, 0); + const Scalar _tmp2 = 2 * state(0, 0); + const Scalar _tmp3 = _tmp0 * state(16, 0) - _tmp1 * state(17, 0) + _tmp2 * state(18, 0); + const Scalar _tmp4 = 2 * state(3, 0); + const Scalar _tmp5 = -_tmp1 * state(18, 0) - _tmp2 * state(17, 0) + _tmp4 * state(16, 0); + const Scalar _tmp6 = _tmp0 * state(17, 0) + _tmp1 * state(16, 0) + _tmp4 * state(18, 0); + const Scalar _tmp7 = -_tmp0 * state(18, 0) + _tmp2 * state(16, 0) + _tmp4 * state(17, 0); + const Scalar _tmp8 = _tmp0 * state(0, 0) + _tmp1 * state(3, 0); + const Scalar _tmp9 = _tmp0 * state(3, 0) - _tmp1 * state(0, 0); + const Scalar _tmp10 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(1, 0), Scalar(2)) - + std::pow(state(2, 0), Scalar(2)) + std::pow(state(3, 0), Scalar(2)); + + // Output terms (2) + if (innov_var != nullptr) { + Scalar& _innov_var = (*innov_var); + + _innov_var = + P(0, 21) * _tmp3 + P(1, 21) * _tmp5 + P(16, 21) * _tmp8 + P(17, 21) * _tmp9 + + P(18, 21) * _tmp10 + P(2, 21) * _tmp7 + P(21, 21) + P(3, 21) * _tmp6 + R + + _tmp10 * (P(0, 18) * _tmp3 + P(1, 18) * _tmp5 + P(16, 18) * _tmp8 + P(17, 18) * _tmp9 + + P(18, 18) * _tmp10 + P(2, 18) * _tmp7 + P(21, 18) + P(3, 18) * _tmp6) + + _tmp3 * (P(0, 0) * _tmp3 + P(1, 0) * _tmp5 + P(16, 0) * _tmp8 + P(17, 0) * _tmp9 + + P(18, 0) * _tmp10 + P(2, 0) * _tmp7 + P(21, 0) + P(3, 0) * _tmp6) + + _tmp5 * (P(0, 1) * _tmp3 + P(1, 1) * _tmp5 + P(16, 1) * _tmp8 + P(17, 1) * _tmp9 + + P(18, 1) * _tmp10 + P(2, 1) * _tmp7 + P(21, 1) + P(3, 1) * _tmp6) + + _tmp6 * (P(0, 3) * _tmp3 + P(1, 3) * _tmp5 + P(16, 3) * _tmp8 + P(17, 3) * _tmp9 + + P(18, 3) * _tmp10 + P(2, 3) * _tmp7 + P(21, 3) + P(3, 3) * _tmp6) + + _tmp7 * (P(0, 2) * _tmp3 + P(1, 2) * _tmp5 + P(16, 2) * _tmp8 + P(17, 2) * _tmp9 + + P(18, 2) * _tmp10 + P(2, 2) * _tmp7 + P(21, 2) + P(3, 2) * _tmp6) + + _tmp8 * (P(0, 16) * _tmp3 + P(1, 16) * _tmp5 + P(16, 16) * _tmp8 + P(17, 16) * _tmp9 + + P(18, 16) * _tmp10 + P(2, 16) * _tmp7 + P(21, 16) + P(3, 16) * _tmp6) + + _tmp9 * (P(0, 17) * _tmp3 + P(1, 17) * _tmp5 + P(16, 17) * _tmp8 + P(17, 17) * _tmp9 + + P(18, 17) * _tmp10 + P(2, 17) * _tmp7 + P(21, 17) + P(3, 17) * _tmp6); + } + + if (H != nullptr) { + matrix::Matrix& _H = (*H); + + _H.setZero(); + + _H(0, 0) = _tmp3; + _H(1, 0) = _tmp5; + _H(2, 0) = _tmp7; + _H(3, 0) = _tmp6; + _H(16, 0) = _tmp8; + _H(17, 0) = _tmp9; + _H(18, 0) = _tmp10; + _H(21, 0) = 1; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym