diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index 26a199c18f..e2e49ffac2 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/optflow_fusion.cpp @@ -45,6 +45,8 @@ #include #include +#include "python/ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h" +#include "python/ekf_derivation/generated/compute_flow_y_innov_var_and_h.h" #include "utils.hpp" void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src) @@ -106,17 +108,6 @@ void Ekf::fuseOptFlow() const float R_LOS = _aid_src_optical_flow.observation_variance[0]; - // get latest estimated orientation - 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); - - // get latest velocity in earth frame - const float vn = _state.vel(0); - const float ve = _state.vel(1); - const float vd = _state.vel(2); - // calculate the height above the ground of the optical flow camera. Since earth frame is NED // a positive offset in earth frame leads to a smaller height above the ground. const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body; @@ -127,129 +118,19 @@ void Ekf::fuseOptFlow() const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal); const float range = height_above_gnd_est / earth_to_body(2, 2); // absolute distance to the frame region in view - // The derivation allows for an arbitrary body to flow sensor frame rotation which is - // currently not supported by the EKF, so assume sensor frame is aligned with the - // body frame - const Dcmf Tbs = matrix::eye(); + const Vector24f state_vector = getStateAtFusionHorizonAsVector(); - // Sub Expressions - const float HK0 = -Tbs(1,0)*q2 + Tbs(1,1)*q1 + Tbs(1,2)*q0; - const float HK1 = Tbs(1,0)*q3 + Tbs(1,1)*q0 - Tbs(1,2)*q1; - const float HK2 = Tbs(1,0)*q0 - Tbs(1,1)*q3 + Tbs(1,2)*q2; - const float HK3 = HK0*vd + HK1*ve + HK2*vn; - const float HK4 = 1.0F/range; - const float HK5 = 2*HK4; - const float HK6 = Tbs(1,0)*q1 + Tbs(1,1)*q2 + Tbs(1,2)*q3; - const float HK7 = -HK0*ve + HK1*vd + HK6*vn; - const float HK8 = HK0*vn - HK2*vd + HK6*ve; - const float HK9 = -HK1*vn + HK2*ve + HK6*vd; - const float HK10 = q0*q2; - const float HK11 = q1*q3; - const float HK12 = HK10 + HK11; - const float HK13 = 2*Tbs(1,2); - const float HK14 = q0*q3; - const float HK15 = q1*q2; - const float HK16 = HK14 - HK15; - const float HK17 = 2*Tbs(1,1); - const float HK18 = ecl::powf(q1, 2); - const float HK19 = ecl::powf(q2, 2); - const float HK20 = -HK19; - const float HK21 = ecl::powf(q0, 2); - const float HK22 = ecl::powf(q3, 2); - const float HK23 = HK21 - HK22; - const float HK24 = HK18 + HK20 + HK23; - const float HK25 = HK12*HK13 - HK16*HK17 + HK24*Tbs(1,0); - const float HK26 = HK14 + HK15; - const float HK27 = 2*Tbs(1,0); - const float HK28 = q0*q1; - const float HK29 = q2*q3; - const float HK30 = HK28 - HK29; - const float HK31 = -HK18; - const float HK32 = HK19 + HK23 + HK31; - const float HK33 = -HK13*HK30 + HK26*HK27 + HK32*Tbs(1,1); - const float HK34 = HK28 + HK29; - const float HK35 = HK10 - HK11; - const float HK36 = HK20 + HK21 + HK22 + HK31; - const float HK37 = HK17*HK34 - HK27*HK35 + HK36*Tbs(1,2); - const float HK38 = 2*HK3; - const float HK39 = 2*HK7; - const float HK40 = 2*HK8; - const float HK41 = 2*HK9; - const float HK42 = HK25*P(0,4) + HK33*P(0,5) + HK37*P(0,6) + HK38*P(0,0) + HK39*P(0,1) + HK40*P(0,2) + HK41*P(0,3); - const float HK43 = ecl::powf(range, -2); - const float HK44 = HK25*P(4,6) + HK33*P(5,6) + HK37*P(6,6) + HK38*P(0,6) + HK39*P(1,6) + HK40*P(2,6) + HK41*P(3,6); - const float HK45 = HK25*P(4,5) + HK33*P(5,5) + HK37*P(5,6) + HK38*P(0,5) + HK39*P(1,5) + HK40*P(2,5) + HK41*P(3,5); - const float HK46 = HK25*P(4,4) + HK33*P(4,5) + HK37*P(4,6) + HK38*P(0,4) + HK39*P(1,4) + HK40*P(2,4) + HK41*P(3,4); - const float HK47 = HK25*P(2,4) + HK33*P(2,5) + HK37*P(2,6) + HK38*P(0,2) + HK39*P(1,2) + HK40*P(2,2) + HK41*P(2,3); - const float HK48 = HK25*P(3,4) + HK33*P(3,5) + HK37*P(3,6) + HK38*P(0,3) + HK39*P(1,3) + HK40*P(2,3) + HK41*P(3,3); - const float HK49 = HK25*P(1,4) + HK33*P(1,5) + HK37*P(1,6) + HK38*P(0,1) + HK39*P(1,1) + HK40*P(1,2) + HK41*P(1,3); - // const float HK50 = HK4/(HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS); + Vector2f innov_var; + Vector24f H; + sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H); + innov_var.copyTo(_aid_src_optical_flow.innovation_variance); - // calculate innovation variance for X axis observation and protect against a badly conditioned calculation - _aid_src_optical_flow.innovation_variance[0] = (HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS); - - if (_aid_src_optical_flow.innovation_variance[0] < R_LOS) { + if ((_aid_src_optical_flow.innovation_variance[0] < R_LOS) + || (_aid_src_optical_flow.innovation_variance[1] < R_LOS)) { // we need to reinitialise the covariance matrix and abort this fusion step initialiseCovariance(); return; } - const float HK50 = HK4 / _aid_src_optical_flow.innovation_variance[0]; - - const float HK51 = Tbs(0,1)*q1; - const float HK52 = Tbs(0,2)*q0; - const float HK53 = Tbs(0,0)*q2; - const float HK54 = HK51 + HK52 - HK53; - const float HK55 = Tbs(0,0)*q3; - const float HK56 = Tbs(0,1)*q0; - const float HK57 = Tbs(0,2)*q1; - const float HK58 = HK55 + HK56 - HK57; - const float HK59 = Tbs(0,0)*q0; - const float HK60 = Tbs(0,2)*q2; - const float HK61 = Tbs(0,1)*q3; - const float HK62 = HK59 + HK60 - HK61; - const float HK63 = HK54*vd + HK58*ve + HK62*vn; - const float HK64 = Tbs(0,0)*q1 + Tbs(0,1)*q2 + Tbs(0,2)*q3; - const float HK65 = HK58*vd + HK64*vn; - const float HK66 = -HK54*ve + HK65; - const float HK67 = HK54*vn + HK64*ve; - const float HK68 = -HK62*vd + HK67; - const float HK69 = HK62*ve + HK64*vd; - const float HK70 = -HK58*vn + HK69; - const float HK71 = 2*Tbs(0,1); - const float HK72 = 2*Tbs(0,2); - const float HK73 = HK12*HK72 + HK24*Tbs(0,0); - const float HK74 = -HK16*HK71 + HK73; - const float HK75 = 2*Tbs(0,0); - const float HK76 = HK26*HK75 + HK32*Tbs(0,1); - const float HK77 = -HK30*HK72 + HK76; - const float HK78 = HK34*HK71 + HK36*Tbs(0,2); - const float HK79 = -HK35*HK75 + HK78; - const float HK80 = 2*HK63; - const float HK81 = 2*HK65 + 2*ve*(-HK51 - HK52 + HK53); - const float HK82 = 2*HK67 + 2*vd*(-HK59 - HK60 + HK61); - const float HK83 = 2*HK69 + 2*vn*(-HK55 - HK56 + HK57); - const float HK84 = HK71*(-HK14 + HK15) + HK73; - const float HK85 = HK72*(-HK28 + HK29) + HK76; - const float HK86 = HK75*(-HK10 + HK11) + HK78; - const float HK87 = HK80*P(0,0) + HK81*P(0,1) + HK82*P(0,2) + HK83*P(0,3) + HK84*P(0,4) + HK85*P(0,5) + HK86*P(0,6); - const float HK88 = HK80*P(0,6) + HK81*P(1,6) + HK82*P(2,6) + HK83*P(3,6) + HK84*P(4,6) + HK85*P(5,6) + HK86*P(6,6); - const float HK89 = HK80*P(0,5) + HK81*P(1,5) + HK82*P(2,5) + HK83*P(3,5) + HK84*P(4,5) + HK85*P(5,5) + HK86*P(5,6); - const float HK90 = HK80*P(0,4) + HK81*P(1,4) + HK82*P(2,4) + HK83*P(3,4) + HK84*P(4,4) + HK85*P(4,5) + HK86*P(4,6); - const float HK91 = HK80*P(0,2) + HK81*P(1,2) + HK82*P(2,2) + HK83*P(2,3) + HK84*P(2,4) + HK85*P(2,5) + HK86*P(2,6); - const float HK92 = 2*HK43; - const float HK93 = HK80*P(0,3) + HK81*P(1,3) + HK82*P(2,3) + HK83*P(3,3) + HK84*P(3,4) + HK85*P(3,5) + HK86*P(3,6); - const float HK94 = HK80*P(0,1) + HK81*P(1,1) + HK82*P(1,2) + HK83*P(1,3) + HK84*P(1,4) + HK85*P(1,5) + HK86*P(1,6); - // const float HK95 = HK4/(HK43*HK74*HK90 + HK43*HK77*HK89 + HK43*HK79*HK88 + HK43*HK80*HK87 + HK66*HK92*HK94 + HK68*HK91*HK92 + HK70*HK92*HK93 + R_LOS); - - // calculate innovation variance for Y axis observation and protect against a badly conditioned calculation - _aid_src_optical_flow.innovation_variance[1] = (HK43*HK74*HK90 + HK43*HK77*HK89 + HK43*HK79*HK88 + HK43*HK80*HK87 + HK66*HK92*HK94 + HK68*HK91*HK92 + HK70*HK92*HK93 + R_LOS); - if (_aid_src_optical_flow.innovation_variance[1] < R_LOS) { - // we need to reinitialise the covariance matrix and abort this fusion step - initialiseCovariance(); - return; - } - - const float HK95 = HK4 / _aid_src_optical_flow.innovation_variance[1]; // run the innovation consistency check and record result setEstimatorAidStatusTestRatio(_aid_src_optical_flow, math::max(_params.flow_innov_gate, 1.f)); @@ -267,28 +148,10 @@ void Ekf::fuseOptFlow() // fuse observation axes sequentially { // Optical flow observation Jacobians - axis 0 - SparseVector24f<0,1,2,3,4,5,6> Hfusion; - Hfusion.at<0>() = HK3*HK5; - Hfusion.at<1>() = HK5*HK7; - Hfusion.at<2>() = HK5*HK8; - Hfusion.at<3>() = HK5*HK9; - Hfusion.at<4>() = HK25*HK4; - Hfusion.at<5>() = HK33*HK4; - Hfusion.at<6>() = HK37*HK4; + SparseVector24f<0,1,2,3,4,5,6> Hfusion(H); // Optical flow Kalman gains - axis 0 - Vector24f Kfusion; - Kfusion(0) = HK42*HK50; - Kfusion(1) = HK49*HK50; - Kfusion(2) = HK47*HK50; - Kfusion(3) = HK48*HK50; - Kfusion(4) = HK46*HK50; - Kfusion(5) = HK45*HK50; - Kfusion(6) = HK44*HK50; - - for (unsigned row = 7; row <= 23; row++) { - Kfusion(row) = HK50*(HK25*P(4,row) + HK33*P(5,row) + HK37*P(6,row) + HK38*P(0,row) + HK39*P(1,row) + HK40*P(2,row) + HK41*P(3,row)); - } + Vector24f Kfusion = P * Hfusion / _aid_src_optical_flow.innovation_variance[0]; if (measurementUpdate(Kfusion, Hfusion, _aid_src_optical_flow.innovation[0])) { fused[0] = true; @@ -301,29 +164,12 @@ void Ekf::fuseOptFlow() } { + sym::ComputeFlowYInnovVarAndH(state_vector, P, range, R_LOS, FLT_EPSILON, &_aid_src_optical_flow.innovation_variance[1], &H); // Optical flow observation Jacobians - axis 1 - SparseVector24f<0,1,2,3,4,5,6> Hfusion; - Hfusion.at<0>() = -HK5*HK63; - Hfusion.at<1>() = -HK5*HK66; - Hfusion.at<2>() = -HK5*HK68; - Hfusion.at<3>() = -HK5*HK70; - Hfusion.at<4>() = -HK4*HK74; - Hfusion.at<5>() = -HK4*HK77; - Hfusion.at<6>() = -HK4*HK79; + SparseVector24f<0,1,2,3,4,5,6> Hfusion(H); // Optical flow Kalman gains - axis 1 - Vector24f Kfusion; - Kfusion(0) = -HK87*HK95; - Kfusion(1) = -HK94*HK95; - Kfusion(2) = -HK91*HK95; - Kfusion(3) = -HK93*HK95; - Kfusion(4) = -HK90*HK95; - Kfusion(5) = -HK89*HK95; - Kfusion(6) = -HK88*HK95; - - for (unsigned row = 7; row <= 23; row++) { - Kfusion(row) = -HK95*(HK80*P(0,row) + HK81*P(1,row) + HK82*P(2,row) + HK83*P(3,row) + HK84*P(4,row) + HK85*P(5,row) + HK86*P(6,row)); - } + Vector24f Kfusion = P * Hfusion / _aid_src_optical_flow.innovation_variance[1]; if (measurementUpdate(Kfusion, Hfusion, _aid_src_optical_flow.innovation[1])) { fused[1] = true; diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 9965260e54..003543dfa0 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -337,6 +337,56 @@ def compute_mag_declination_innov_innov_var_and_h( return (innov, innov_var, H.T) +def predict_opt_flow(state, distance, epsilon): + q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz]) + R_to_earth = quat_to_rot(q_att) + R_to_body = R_to_earth.T + + # Calculate earth relative velocity in a non-rotating sensor frame + v = sf.V3(state[State.vx], state[State.vy], state[State.vz]) + rel_vel_sensor = R_to_body * v + + # Divide by range to get predicted angular LOS rates relative to X and Y + # axes. Note these are rates in a non-rotating sensor frame + flow_pred = sf.V2() + flow_pred[0] = rel_vel_sensor[1] / distance + flow_pred[1] = -rel_vel_sensor[0] / distance + flow_pred = add_epsilon_sign(flow_pred, distance, epsilon) + + return flow_pred + + +def compute_flow_xy_innov_var_and_hx( + state: VState, + P: MState, + distance: sf.Scalar, + R: sf.Scalar, + epsilon: sf.Scalar +) -> (sf.V2, VState): + meas_pred = predict_opt_flow(state, distance, epsilon); + + innov_var = sf.V2() + 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] + + return (innov_var, Hx.T) + +def compute_flow_y_innov_var_and_h( + state: VState, + P: MState, + distance: sf.Scalar, + R: sf.Scalar, + epsilon: sf.Scalar +) -> (sf.Scalar, VState): + meas_pred = predict_opt_flow(state, distance, epsilon); + + Hy = sf.V1(meas_pred[1]).jacobian(state) + innov_var = (Hy * P * Hy.T + R)[0,0] + + return (innov_var, Hy.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"]) @@ -352,3 +402,5 @@ generate_px4_function(compute_yaw_321_innov_var_and_h_alternate, output_names=[" generate_px4_function(compute_yaw_312_innov_var_and_h, output_names=["innov_var", "H"]) generate_px4_function(compute_yaw_312_innov_var_and_h_alternate, output_names=["innov_var", "H"]) generate_px4_function(compute_mag_declination_innov_innov_var_and_h, output_names=["innov", "innov_var", "H"]) +generate_px4_function(compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"]) +generate_px4_function(compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"]) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h new file mode 100644 index 0000000000..56f3d87737 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h @@ -0,0 +1,127 @@ +// ----------------------------------------------------------------------------- +// 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_flow_xy_innov_var_and_hx + * + * Args: + * state: Matrix24_1 + * P: Matrix24_24 + * distance: Scalar + * R: Scalar + * epsilon: Scalar + * + * Outputs: + * innov_var: Matrix21 + * H: Matrix24_1 + */ +template +void ComputeFlowXyInnovVarAndHx(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar distance, + const Scalar R, const Scalar epsilon, + matrix::Matrix* const innov_var = nullptr, + matrix::Matrix* const H = nullptr) { + // Total ops: 285 + + // Input arrays + + // Intermediate terms (29) + const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp2 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp3 = + Scalar(1.0) / + (distance + epsilon * (2 * math::min(0, (((distance) > 0) - ((distance) < 0))) + 1)); + const Scalar _tmp4 = _tmp3 * (_tmp0 - _tmp1 + _tmp2); + const Scalar _tmp5 = 2 * state(3, 0); + const Scalar _tmp6 = _tmp5 * state(0, 0); + const Scalar _tmp7 = 2 * state(2, 0); + const Scalar _tmp8 = _tmp7 * state(1, 0); + const Scalar _tmp9 = _tmp3 * (-_tmp6 + _tmp8); + const Scalar _tmp10 = 2 * state(4, 0); + const Scalar _tmp11 = _tmp10 * state(0, 0); + const Scalar _tmp12 = 2 * state(5, 0); + const Scalar _tmp13 = _tmp12 * state(3, 0); + const Scalar _tmp14 = _tmp7 * state(6, 0); + const Scalar _tmp15 = _tmp3 * (-_tmp11 - _tmp13 + _tmp14); + const Scalar _tmp16 = 2 * state(1, 0); + const Scalar _tmp17 = + _tmp3 * (-_tmp10 * state(3, 0) + _tmp12 * state(0, 0) + _tmp16 * state(6, 0)); + const Scalar _tmp18 = _tmp7 * state(4, 0); + const Scalar _tmp19 = _tmp12 * state(1, 0); + const Scalar _tmp20 = 2 * state(0, 0) * state(6, 0); + const Scalar _tmp21 = _tmp3 * (_tmp18 - _tmp19 + _tmp20); + const Scalar _tmp22 = _tmp3 * (_tmp10 * state(1, 0) + _tmp5 * state(6, 0) + _tmp7 * state(5, 0)); + const Scalar _tmp23 = _tmp3 * (_tmp16 * state(0, 0) + _tmp7 * state(3, 0)); + const Scalar _tmp24 = _tmp3 * (-_tmp0 + _tmp1 + _tmp2); + const Scalar _tmp25 = _tmp3 * (_tmp6 + _tmp8); + const Scalar _tmp26 = _tmp3 * (_tmp16 * state(3, 0) - _tmp7 * state(0, 0)); + const Scalar _tmp27 = _tmp3 * (_tmp11 + _tmp13 - _tmp14); + const Scalar _tmp28 = _tmp3 * (-_tmp18 + _tmp19 - _tmp20); + + // Output terms (2) + if (innov_var != nullptr) { + matrix::Matrix& _innov_var = (*innov_var); + + _innov_var(0, 0) = + R + + _tmp15 * (P(0, 3) * _tmp17 + P(1, 3) * _tmp21 + P(2, 3) * _tmp22 + P(3, 3) * _tmp15 + + P(4, 3) * _tmp9 + P(5, 3) * _tmp4 + P(6, 3) * _tmp23) + + _tmp17 * (P(0, 0) * _tmp17 + P(1, 0) * _tmp21 + P(2, 0) * _tmp22 + P(3, 0) * _tmp15 + + P(4, 0) * _tmp9 + P(5, 0) * _tmp4 + P(6, 0) * _tmp23) + + _tmp21 * (P(0, 1) * _tmp17 + P(1, 1) * _tmp21 + P(2, 1) * _tmp22 + P(3, 1) * _tmp15 + + P(4, 1) * _tmp9 + P(5, 1) * _tmp4 + P(6, 1) * _tmp23) + + _tmp22 * (P(0, 2) * _tmp17 + P(1, 2) * _tmp21 + P(2, 2) * _tmp22 + P(3, 2) * _tmp15 + + P(4, 2) * _tmp9 + P(5, 2) * _tmp4 + P(6, 2) * _tmp23) + + _tmp23 * (P(0, 6) * _tmp17 + P(1, 6) * _tmp21 + P(2, 6) * _tmp22 + P(3, 6) * _tmp15 + + P(4, 6) * _tmp9 + P(5, 6) * _tmp4 + P(6, 6) * _tmp23) + + _tmp4 * (P(0, 5) * _tmp17 + P(1, 5) * _tmp21 + P(2, 5) * _tmp22 + P(3, 5) * _tmp15 + + P(4, 5) * _tmp9 + P(5, 5) * _tmp4 + P(6, 5) * _tmp23) + + _tmp9 * (P(0, 4) * _tmp17 + P(1, 4) * _tmp21 + P(2, 4) * _tmp22 + P(3, 4) * _tmp15 + + P(4, 4) * _tmp9 + P(5, 4) * _tmp4 + P(6, 4) * _tmp23); + _innov_var(1, 0) = + R - + _tmp17 * (-P(0, 3) * _tmp27 - P(1, 3) * _tmp22 - P(2, 3) * _tmp28 - P(3, 3) * _tmp17 - + P(4, 3) * _tmp24 - P(5, 3) * _tmp25 - P(6, 3) * _tmp26) - + _tmp22 * (-P(0, 1) * _tmp27 - P(1, 1) * _tmp22 - P(2, 1) * _tmp28 - P(3, 1) * _tmp17 - + P(4, 1) * _tmp24 - P(5, 1) * _tmp25 - P(6, 1) * _tmp26) - + _tmp24 * (-P(0, 4) * _tmp27 - P(1, 4) * _tmp22 - P(2, 4) * _tmp28 - P(3, 4) * _tmp17 - + P(4, 4) * _tmp24 - P(5, 4) * _tmp25 - P(6, 4) * _tmp26) - + _tmp25 * (-P(0, 5) * _tmp27 - P(1, 5) * _tmp22 - P(2, 5) * _tmp28 - P(3, 5) * _tmp17 - + P(4, 5) * _tmp24 - P(5, 5) * _tmp25 - P(6, 5) * _tmp26) - + _tmp26 * (-P(0, 6) * _tmp27 - P(1, 6) * _tmp22 - P(2, 6) * _tmp28 - P(3, 6) * _tmp17 - + P(4, 6) * _tmp24 - P(5, 6) * _tmp25 - P(6, 6) * _tmp26) - + _tmp27 * (-P(0, 0) * _tmp27 - P(1, 0) * _tmp22 - P(2, 0) * _tmp28 - P(3, 0) * _tmp17 - + P(4, 0) * _tmp24 - P(5, 0) * _tmp25 - P(6, 0) * _tmp26) - + _tmp28 * (-P(0, 2) * _tmp27 - P(1, 2) * _tmp22 - P(2, 2) * _tmp28 - P(3, 2) * _tmp17 - + P(4, 2) * _tmp24 - P(5, 2) * _tmp25 - P(6, 2) * _tmp26); + } + + if (H != nullptr) { + matrix::Matrix& _H = (*H); + + _H.setZero(); + + _H(0, 0) = _tmp17; + _H(1, 0) = _tmp21; + _H(2, 0) = _tmp22; + _H(3, 0) = _tmp15; + _H(4, 0) = _tmp9; + _H(5, 0) = _tmp4; + _H(6, 0) = _tmp23; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_y_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_y_innov_var_and_h.h new file mode 100644 index 0000000000..70737d1220 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_y_innov_var_and_h.h @@ -0,0 +1,95 @@ +// ----------------------------------------------------------------------------- +// 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_flow_y_innov_var_and_h + * + * Args: + * state: Matrix24_1 + * P: Matrix24_24 + * distance: Scalar + * R: Scalar + * epsilon: Scalar + * + * Outputs: + * innov_var: Scalar + * H: Matrix24_1 + */ +template +void ComputeFlowYInnovVarAndH(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar distance, + const Scalar R, const Scalar epsilon, + Scalar* const innov_var = nullptr, + matrix::Matrix* const H = nullptr) { + // Total ops: 171 + + // Input arrays + + // Intermediate terms (13) + const Scalar _tmp0 = + Scalar(1.0) / + (distance + epsilon * (2 * math::min(0, (((distance) > 0) - ((distance) < 0))) + 1)); + const Scalar _tmp1 = + _tmp0 * (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))); + const Scalar _tmp2 = 2 * state(0, 0); + const Scalar _tmp3 = 2 * state(1, 0); + const Scalar _tmp4 = _tmp0 * (_tmp2 * state(3, 0) + _tmp3 * state(2, 0)); + const Scalar _tmp5 = _tmp0 * (-_tmp2 * state(2, 0) + _tmp3 * state(3, 0)); + const Scalar _tmp6 = 2 * state(4, 0); + const Scalar _tmp7 = 2 * state(6, 0); + const Scalar _tmp8 = _tmp0 * (_tmp2 * state(5, 0) - _tmp6 * state(3, 0) + _tmp7 * state(1, 0)); + const Scalar _tmp9 = 2 * state(5, 0); + const Scalar _tmp10 = _tmp0 * (_tmp2 * state(4, 0) - _tmp7 * state(2, 0) + _tmp9 * state(3, 0)); + const Scalar _tmp11 = _tmp0 * (_tmp3 * state(4, 0) + _tmp7 * state(3, 0) + _tmp9 * state(2, 0)); + const Scalar _tmp12 = _tmp0 * (_tmp3 * state(5, 0) - _tmp6 * state(2, 0) - _tmp7 * state(0, 0)); + + // Output terms (2) + if (innov_var != nullptr) { + Scalar& _innov_var = (*innov_var); + + _innov_var = R - + _tmp1 * (-P(0, 4) * _tmp10 - P(1, 4) * _tmp11 - P(2, 4) * _tmp12 - + P(3, 4) * _tmp8 - P(4, 4) * _tmp1 - P(5, 4) * _tmp4 - P(6, 4) * _tmp5) - + _tmp10 * (-P(0, 0) * _tmp10 - P(1, 0) * _tmp11 - P(2, 0) * _tmp12 - + P(3, 0) * _tmp8 - P(4, 0) * _tmp1 - P(5, 0) * _tmp4 - P(6, 0) * _tmp5) - + _tmp11 * (-P(0, 1) * _tmp10 - P(1, 1) * _tmp11 - P(2, 1) * _tmp12 - + P(3, 1) * _tmp8 - P(4, 1) * _tmp1 - P(5, 1) * _tmp4 - P(6, 1) * _tmp5) - + _tmp12 * (-P(0, 2) * _tmp10 - P(1, 2) * _tmp11 - P(2, 2) * _tmp12 - + P(3, 2) * _tmp8 - P(4, 2) * _tmp1 - P(5, 2) * _tmp4 - P(6, 2) * _tmp5) - + _tmp4 * (-P(0, 5) * _tmp10 - P(1, 5) * _tmp11 - P(2, 5) * _tmp12 - + P(3, 5) * _tmp8 - P(4, 5) * _tmp1 - P(5, 5) * _tmp4 - P(6, 5) * _tmp5) - + _tmp5 * (-P(0, 6) * _tmp10 - P(1, 6) * _tmp11 - P(2, 6) * _tmp12 - + P(3, 6) * _tmp8 - P(4, 6) * _tmp1 - P(5, 6) * _tmp4 - P(6, 6) * _tmp5) - + _tmp8 * (-P(0, 3) * _tmp10 - P(1, 3) * _tmp11 - P(2, 3) * _tmp12 - + P(3, 3) * _tmp8 - P(4, 3) * _tmp1 - P(5, 3) * _tmp4 - P(6, 3) * _tmp5); + } + + if (H != nullptr) { + matrix::Matrix& _H = (*H); + + _H.setZero(); + + _H(0, 0) = -_tmp10; + _H(1, 0) = -_tmp11; + _H(2, 0) = -_tmp12; + _H(3, 0) = -_tmp8; + _H(4, 0) = -_tmp1; + _H(5, 0) = -_tmp4; + _H(6, 0) = -_tmp5; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym