From 0dc71e1867da00923fc46160481200e3542582b6 Mon Sep 17 00:00:00 2001 From: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)> Date: Fri, 3 Nov 2023 13:09:29 +0100 Subject: [PATCH] flow: add distance exponent factor --- msg/EstimatorStates.msg | 4 +- src/modules/ekf2/EKF/covariance.cpp | 2 + src/modules/ekf2/EKF/ekf.cpp | 1 + src/modules/ekf2/EKF/ekf_helper.cpp | 1 + src/modules/ekf2/EKF/estimator_interface.cpp | 1 + src/modules/ekf2/EKF/optflow_fusion.cpp | 11 +- .../EKF/python/ekf_derivation/derivation.py | 13 +- .../generated/compute_airspeed_h_and_k.h | 24 +- .../compute_airspeed_innov_and_innov_var.h | 8 +- .../compute_drag_x_innov_var_and_k.h | 14 +- .../compute_drag_y_innov_var_and_k.h | 14 +- .../compute_flow_xy_innov_var_and_hx.h | 183 +-- .../compute_flow_y_innov_var_and_h.h | 121 +- .../compute_gnss_yaw_pred_innov_var_and_h.h | 14 +- .../compute_gravity_innov_var_and_k_and_h.h | 31 +- ...ute_mag_declination_pred_innov_var_and_h.h | 14 +- .../compute_mag_innov_innov_var_and_hx.h | 14 +- .../generated/compute_mag_y_innov_var_and_h.h | 14 +- .../generated/compute_mag_z_innov_var_and_h.h | 14 +- .../generated/compute_sideslip_h_and_k.h | 37 +- .../compute_sideslip_innov_and_innov_var.h | 8 +- .../compute_yaw_312_innov_var_and_h.h | 14 +- ...ompute_yaw_312_innov_var_and_h_alternate.h | 14 +- .../compute_yaw_321_innov_var_and_h.h | 14 +- ...ompute_yaw_321_innov_var_and_h_alternate.h | 14 +- .../generated/predict_covariance.h | 1034 +++++++++-------- .../python/ekf_derivation/generated/state.h | 15 +- 27 files changed, 869 insertions(+), 779 deletions(-) diff --git a/msg/EstimatorStates.msg b/msg/EstimatorStates.msg index 885246d8a5..6aca5d3b52 100644 --- a/msg/EstimatorStates.msg +++ b/msg/EstimatorStates.msg @@ -1,7 +1,7 @@ uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # the timestamp of the raw data (microseconds) -float32[25] states # Internal filter states +float32[26] states # Internal filter states uint8 n_states # Number of states effectively used -float32[24] covariances # Diagonal Elements of Covariance Matrix +float32[25] covariances # Diagonal Elements of Covariance Matrix diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 050f7f9619..f2fc93d5d7 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -100,6 +100,7 @@ void Ekf::initialiseCovariance() resetWindCov(); #endif // CONFIG_EKF2_WIND P(State::flow_scale.idx, State::flow_scale.idx) = sq(0.1f); + P(State::flow_exp.idx, State::flow_exp.idx) = sq(0.1f); } void Ekf::predictCovariance(const imuSample &imu_delayed) @@ -210,6 +211,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) if (_control_status.flags.opt_flow) { const float flow_scale_nsd = 0.1f; P(State::flow_scale.idx, State::flow_scale.idx) += sq(flow_scale_nsd) * dt; + P(State::flow_exp.idx, State::flow_exp.idx) += sq(flow_scale_nsd) * dt; } // covariance matrix is symmetrical, so copy upper half to lower half diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 5c014a3a59..d25d0e496b 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -72,6 +72,7 @@ void Ekf::reset() _state.wind_vel.setZero(); #endif // CONFIG_EKF2_WIND _state.flow_scale = 1.f; + _state.flow_exp = 1.f; #if defined(CONFIG_EKF2_RANGE_FINDER) _range_sensor.setPitchOffset(_params.rng_sens_pitch); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 37fc479135..dff067276f 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -775,6 +775,7 @@ void Ekf::fuse(const VectorState &K, float innovation) _state.wind_vel -= K.slice(State::wind_vel.idx, 0) * innovation; #endif // CONFIG_EKF2_WIND _state.flow_scale -= (K.slice(State::flow_scale.idx, 0) * innovation)(0, 0); + _state.flow_exp -= (K.slice(State::flow_exp.idx, 0) * innovation)(0, 0); } void Ekf::uncorrelateQuatFromOtherStates() diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 850e409bdb..103ca5c954 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -368,6 +368,7 @@ void EstimatorInterface::setOpticalFlowData(const flowSample &flow) flowSample optflow_sample_new{flow}; optflow_sample_new.time_us = time_us; + optflow_sample_new.flow_xy_rad *= 0.82f; _flow_buffer->push(optflow_sample_new); diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index cfd2957c81..cbe93fcc09 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/optflow_fusion.cpp @@ -61,16 +61,17 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src) // Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis. const Vector2f opt_flow_rate = _flow_compensated_XY_rad / _flow_sample_delayed.dt; + const float range_scaled = std::pow(range, _state.flow_exp); // compute the velocities in body and local frames from corrected optical flow measurement for logging only - _flow_vel_body(0) = -opt_flow_rate(1) * range; - _flow_vel_body(1) = opt_flow_rate(0) * range; + _flow_vel_body(0) = -opt_flow_rate(1) * range_scaled / _state.flow_scale; + _flow_vel_body(1) = opt_flow_rate(0) * range_scaled / _state.flow_scale; _flow_vel_ne = Vector2f(_R_to_earth * Vector3f(_flow_vel_body(0), _flow_vel_body(1), 0.f)); aid_src.observation[0] = opt_flow_rate(0); // flow around the X axis aid_src.observation[1] = opt_flow_rate(1); // flow around the Y axis - aid_src.innovation[0] = _state.flow_scale * (vel_body(1) / range) - aid_src.observation[0]; - aid_src.innovation[1] = _state.flow_scale * (-vel_body(0) / range) - aid_src.observation[1]; + aid_src.innovation[0] = _state.flow_scale * (vel_body(1) / range_scaled) - aid_src.observation[0]; + aid_src.innovation[1] = _state.flow_scale * (-vel_body(0) / range_scaled) - aid_src.observation[1]; // calculate the optical flow observation variance const float R_LOS = calcOptFlowMeasVar(_flow_sample_delayed); @@ -134,7 +135,7 @@ void Ekf::fuseOptFlow() // recalculate the innovation using the updated state const Vector2f vel_body = predictFlowVelBody(); range = predictFlowRange(); - _aid_src_optical_flow.innovation[1] = _state.flow_scale * (-vel_body(0) / range) - _aid_src_optical_flow.observation[1]; + _aid_src_optical_flow.innovation[1] = _state.flow_scale * (-vel_body(0) / std::pow(range, _state.flow_exp)) - _aid_src_optical_flow.observation[1]; if (_aid_src_optical_flow.innovation_variance[1] < R_LOS) { // we need to reinitialise the covariance matrix and abort this fusion step diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 9cd62a4200..ff090aba2b 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -63,7 +63,8 @@ State = Values( mag_I = sf.V3(), mag_B = sf.V3(), wind_vel = sf.V2(), - flow_scale = sf.V1() + flow_scale = sf.V1(), + flow_exp = sf.V1() ) if args.disable_mag: @@ -129,7 +130,8 @@ def predict_covariance( mag_I = sf.V3.symbolic("mag_I"), mag_B = sf.V3.symbolic("mag_B"), wind_vel = sf.V2.symbolic("wind_vel"), - flow_scale = sf.V1.symbolic("flow_scale") + flow_scale = sf.V1.symbolic("flow_scale"), + flow_exp = sf.V1.symbolic("flow_exp") ) if args.disable_mag: @@ -473,10 +475,11 @@ def predict_opt_flow(state, distance, epsilon): # 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 + dist_scaled = distance**state["flow_exp"][0] + flow_pred[0] = rel_vel_sensor[1] / dist_scaled + flow_pred[1] = -rel_vel_sensor[0] / dist_scaled flow_pred *= state["flow_scale"] - flow_pred = add_epsilon_sign(flow_pred, distance, epsilon) + flow_pred = add_epsilon_sign(flow_pred, dist_scaled, epsilon) return flow_pred diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h index 52bb16353d..dd8208a4c4 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h @@ -16,21 +16,21 @@ namespace sym { * Symbolic function: compute_airspeed_h_and_k * * Args: - * state: Matrix25_1 - * P: Matrix24_24 + * state: Matrix26_1 + * P: Matrix25_25 * innov_var: Scalar * epsilon: Scalar * * Outputs: - * H: Matrix24_1 - * K: Matrix24_1 + * H: Matrix25_1 + * K: Matrix25_1 */ template -void ComputeAirspeedHAndK(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar innov_var, - const Scalar epsilon, matrix::Matrix* const H = nullptr, - matrix::Matrix* const K = nullptr) { - // Total ops: 256 +void ComputeAirspeedHAndK(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar innov_var, + const Scalar epsilon, matrix::Matrix* const H = nullptr, + matrix::Matrix* const K = nullptr) { + // Total ops: 266 // Input arrays @@ -47,7 +47,7 @@ void ComputeAirspeedHAndK(const matrix::Matrix& state, // Output terms (2) if (H != nullptr) { - matrix::Matrix& _h = (*H); + matrix::Matrix& _h = (*H); _h.setZero(); @@ -59,7 +59,7 @@ void ComputeAirspeedHAndK(const matrix::Matrix& state, } if (K != nullptr) { - matrix::Matrix& _k = (*K); + matrix::Matrix& _k = (*K); _k(0, 0) = _tmp6 * (-P(0, 21) * _tmp3 - P(0, 22) * _tmp4 + P(0, 3) * _tmp3 + P(0, 4) * _tmp4 + P(0, 5) * _tmp5); @@ -109,6 +109,8 @@ void ComputeAirspeedHAndK(const matrix::Matrix& state, P(22, 4) * _tmp4 + P(22, 5) * _tmp5); _k(23, 0) = _tmp6 * (-P(23, 21) * _tmp3 - P(23, 22) * _tmp4 + P(23, 3) * _tmp3 + P(23, 4) * _tmp4 + P(23, 5) * _tmp5); + _k(24, 0) = _tmp6 * (-P(24, 21) * _tmp3 - P(24, 22) * _tmp4 + P(24, 3) * _tmp3 + + P(24, 4) * _tmp4 + P(24, 5) * _tmp5); } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_innov_and_innov_var.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_innov_and_innov_var.h index e97088d173..8124a71934 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_innov_and_innov_var.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_innov_and_innov_var.h @@ -16,8 +16,8 @@ namespace sym { * Symbolic function: compute_airspeed_innov_and_innov_var * * Args: - * state: Matrix25_1 - * P: Matrix24_24 + * state: Matrix26_1 + * P: Matrix25_25 * airspeed: Scalar * R: Scalar * epsilon: Scalar @@ -27,8 +27,8 @@ namespace sym { * innov_var: Scalar */ template -void ComputeAirspeedInnovAndInnovVar(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar airspeed, +void ComputeAirspeedInnovAndInnovVar(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar airspeed, const Scalar R, const Scalar epsilon, Scalar* const innov = nullptr, Scalar* const innov_var = nullptr) { diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_x_innov_var_and_k.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_x_innov_var_and_k.h index c506707f59..2b6508e666 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_x_innov_var_and_k.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_x_innov_var_and_k.h @@ -16,8 +16,8 @@ namespace sym { * Symbolic function: compute_drag_x_innov_var_and_k * * Args: - * state: Matrix25_1 - * P: Matrix24_24 + * state: Matrix26_1 + * P: Matrix25_25 * rho: Scalar * cd: Scalar * cm: Scalar @@ -26,14 +26,14 @@ namespace sym { * * Outputs: * innov_var: Scalar - * K: Matrix24_1 + * K: Matrix25_1 */ template -void ComputeDragXInnovVarAndK(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar rho, +void ComputeDragXInnovVarAndK(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar rho, const Scalar cd, const Scalar cm, const Scalar R, const Scalar epsilon, Scalar* const innov_var = nullptr, - matrix::Matrix* const K = nullptr) { + matrix::Matrix* const K = nullptr) { // Total ops: 348 // Input arrays @@ -153,7 +153,7 @@ void ComputeDragXInnovVarAndK(const matrix::Matrix& state, } if (K != nullptr) { - matrix::Matrix& _k = (*K); + matrix::Matrix& _k = (*K); _k.setZero(); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_y_innov_var_and_k.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_y_innov_var_and_k.h index 8ca125cf65..823b975751 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_y_innov_var_and_k.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_y_innov_var_and_k.h @@ -16,8 +16,8 @@ namespace sym { * Symbolic function: compute_drag_y_innov_var_and_k * * Args: - * state: Matrix25_1 - * P: Matrix24_24 + * state: Matrix26_1 + * P: Matrix25_25 * rho: Scalar * cd: Scalar * cm: Scalar @@ -26,14 +26,14 @@ namespace sym { * * Outputs: * innov_var: Scalar - * K: Matrix24_1 + * K: Matrix25_1 */ template -void ComputeDragYInnovVarAndK(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar rho, +void ComputeDragYInnovVarAndK(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar rho, const Scalar cd, const Scalar cm, const Scalar R, const Scalar epsilon, Scalar* const innov_var = nullptr, - matrix::Matrix* const K = nullptr) { + matrix::Matrix* const K = nullptr) { // Total ops: 348 // Input arrays @@ -155,7 +155,7 @@ void ComputeDragYInnovVarAndK(const matrix::Matrix& state, } if (K != nullptr) { - matrix::Matrix& _k = (*K); + matrix::Matrix& _k = (*K); _k.setZero(); 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 index 47099ff889..7cdcbcc0bf 100644 --- 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 @@ -16,119 +16,130 @@ namespace sym { * Symbolic function: compute_flow_xy_innov_var_and_hx * * Args: - * state: Matrix25_1 - * P: Matrix24_24 + * state: Matrix26_1 + * P: Matrix25_25 * distance: Scalar * R: Scalar * epsilon: Scalar * * Outputs: * innov_var: Matrix21 - * H: Matrix24_1 + * H: Matrix25_1 */ template -void ComputeFlowXyInnovVarAndHx(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar distance, +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: 253 + matrix::Matrix* const H = nullptr) { + // Total ops: 302 + + // Unused inputs + (void)epsilon; // Input arrays - // Intermediate terms (41) - const Scalar _tmp0 = 2 * state(2, 0); - const Scalar _tmp1 = _tmp0 * state(0, 0); - const Scalar _tmp2 = 2 * state(1, 0) * state(3, 0); - const Scalar _tmp3 = -_tmp2; - const Scalar _tmp4 = 2 * state(0, 0); - const Scalar _tmp5 = _tmp4 * state(3, 0); - const Scalar _tmp6 = -_tmp5; - const Scalar _tmp7 = _tmp0 * state(1, 0); - const Scalar _tmp8 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp9 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp10 = -_tmp9; - const Scalar _tmp11 = _tmp10 + _tmp8; - const Scalar _tmp12 = std::pow(state(0, 0), Scalar(2)); + // Intermediate terms (45) + const Scalar _tmp0 = std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp1 = std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp2 = 1 - 2 * _tmp1; + const Scalar _tmp3 = -2 * _tmp0 + _tmp2; + const Scalar _tmp4 = std::pow(distance, Scalar(-state(25, 0))); + const Scalar _tmp5 = _tmp4 * state(24, 0); + const Scalar _tmp6 = _tmp3 * _tmp5; + const Scalar _tmp7 = 2 * state(0, 0); + const Scalar _tmp8 = _tmp7 * state(2, 0); + const Scalar _tmp9 = 2 * state(1, 0); + const Scalar _tmp10 = _tmp9 * state(3, 0); + const Scalar _tmp11 = 2 * state(2, 0) * state(3, 0); + const Scalar _tmp12 = _tmp9 * state(0, 0); const Scalar _tmp13 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp14 = -_tmp12 + _tmp13; - const Scalar _tmp15 = - Scalar(1.0) / - (distance + epsilon * (2 * math::min(0, (((distance) > 0) - ((distance) < 0))) + 1)); - const Scalar _tmp16 = _tmp15 * state(24, 0); - const Scalar _tmp17 = _tmp16 * (state(4, 0) * (_tmp11 + _tmp14) + state(5, 0) * (_tmp6 - _tmp7) + - state(6, 0) * (_tmp1 + _tmp3)); - const Scalar _tmp18 = 1 - 2 * _tmp8; - const Scalar _tmp19 = _tmp18 - 2 * _tmp9; - const Scalar _tmp20 = _tmp6 + _tmp7; - const Scalar _tmp21 = _tmp0 * state(3, 0); - const Scalar _tmp22 = _tmp4 * state(1, 0); - const Scalar _tmp23 = _tmp21 + _tmp22; - const Scalar _tmp24 = _tmp20 * state(4, 0) + _tmp23 * state(6, 0); - const Scalar _tmp25 = _tmp15 * (_tmp19 * state(5, 0) + _tmp24); - const Scalar _tmp26 = _tmp16 * _tmp19; - const Scalar _tmp27 = _tmp16 * (state(4, 0) * (_tmp1 + _tmp2) + state(5, 0) * (_tmp21 - _tmp22) + - state(6, 0) * (_tmp11 + _tmp12 - _tmp13)); - const Scalar _tmp28 = _tmp16 * _tmp20; - const Scalar _tmp29 = _tmp16 * _tmp23; - const Scalar _tmp30 = -2 * _tmp13 + _tmp18; - const Scalar _tmp31 = _tmp16 * _tmp30; - const Scalar _tmp32 = -_tmp1; - const Scalar _tmp33 = -_tmp8; + const Scalar _tmp14 = std::pow(state(0, 0), Scalar(2)); + const Scalar _tmp15 = -_tmp0; + const Scalar _tmp16 = _tmp14 + _tmp15; + const Scalar _tmp17 = _tmp5 * (state(4, 0) * (_tmp10 + _tmp8) + state(5, 0) * (_tmp11 - _tmp12) + + state(6, 0) * (_tmp1 - _tmp13 + _tmp16)); + const Scalar _tmp18 = std::log(distance); + const Scalar _tmp19 = _tmp7 * state(3, 0); + const Scalar _tmp20 = -_tmp19; + const Scalar _tmp21 = _tmp9 * state(2, 0); + const Scalar _tmp22 = _tmp20 + _tmp21; + const Scalar _tmp23 = _tmp11 + _tmp12; + const Scalar _tmp24 = _tmp22 * state(4, 0) + _tmp23 * state(6, 0); + const Scalar _tmp25 = _tmp4 * (_tmp24 + _tmp3 * state(5, 0)); + const Scalar _tmp26 = _tmp18 * _tmp25 * state(24, 0); + const Scalar _tmp27 = _tmp22 * _tmp5; + const Scalar _tmp28 = -_tmp10; + const Scalar _tmp29 = -_tmp14; + const Scalar _tmp30 = _tmp5 * (state(4, 0) * (_tmp1 + _tmp13 + _tmp15 + _tmp29) + + state(5, 0) * (_tmp20 - _tmp21) + state(6, 0) * (_tmp28 + _tmp8)); + const Scalar _tmp31 = _tmp23 * _tmp5; + const Scalar _tmp32 = -_tmp8; + const Scalar _tmp33 = -_tmp1 + _tmp13; const Scalar _tmp34 = - _tmp16 * (state(4, 0) * (_tmp3 + _tmp32) + state(5, 0) * (-_tmp21 + _tmp22) + - state(6, 0) * (_tmp14 + _tmp33 + _tmp9)); - const Scalar _tmp35 = _tmp2 + _tmp32; - const Scalar _tmp36 = _tmp16 * _tmp35; - const Scalar _tmp37 = _tmp5 + _tmp7; - const Scalar _tmp38 = _tmp16 * _tmp37; - const Scalar _tmp39 = _tmp16 * (_tmp24 + state(5, 0) * (_tmp10 + _tmp12 + _tmp13 + _tmp33)); - const Scalar _tmp40 = - _tmp15 * (_tmp30 * state(4, 0) + _tmp35 * state(6, 0) + _tmp37 * state(5, 0)); + _tmp5 * (state(4, 0) * (_tmp28 + _tmp32) + state(5, 0) * (-_tmp11 + _tmp12) + + state(6, 0) * (_tmp0 + _tmp29 + _tmp33)); + const Scalar _tmp35 = _tmp5 * (_tmp24 + state(5, 0) * (_tmp16 + _tmp33)); + const Scalar _tmp36 = _tmp10 + _tmp32; + const Scalar _tmp37 = _tmp19 + _tmp21; + const Scalar _tmp38 = -2 * _tmp13 + _tmp2; + const Scalar _tmp39 = _tmp36 * state(6, 0) + _tmp37 * state(5, 0) + _tmp38 * state(4, 0); + const Scalar _tmp40 = _tmp18 * _tmp39 * _tmp5; + const Scalar _tmp41 = _tmp38 * _tmp5; + const Scalar _tmp42 = _tmp37 * _tmp5; + const Scalar _tmp43 = _tmp36 * _tmp5; + const Scalar _tmp44 = _tmp39 * _tmp4; // Output terms (2) if (innov_var != nullptr) { matrix::Matrix& _innov_var = (*innov_var); - _innov_var(0, 0) = R + - _tmp17 * (P(0, 2) * _tmp27 + P(2, 2) * _tmp17 + P(23, 2) * _tmp25 + - P(3, 2) * _tmp28 + P(4, 2) * _tmp26 + P(5, 2) * _tmp29) + - _tmp25 * (P(0, 23) * _tmp27 + P(2, 23) * _tmp17 + P(23, 23) * _tmp25 + - P(3, 23) * _tmp28 + P(4, 23) * _tmp26 + P(5, 23) * _tmp29) + - _tmp26 * (P(0, 4) * _tmp27 + P(2, 4) * _tmp17 + P(23, 4) * _tmp25 + - P(3, 4) * _tmp28 + P(4, 4) * _tmp26 + P(5, 4) * _tmp29) + - _tmp27 * (P(0, 0) * _tmp27 + P(2, 0) * _tmp17 + P(23, 0) * _tmp25 + - P(3, 0) * _tmp28 + P(4, 0) * _tmp26 + P(5, 0) * _tmp29) + - _tmp28 * (P(0, 3) * _tmp27 + P(2, 3) * _tmp17 + P(23, 3) * _tmp25 + - P(3, 3) * _tmp28 + P(4, 3) * _tmp26 + P(5, 3) * _tmp29) + - _tmp29 * (P(0, 5) * _tmp27 + P(2, 5) * _tmp17 + P(23, 5) * _tmp25 + - P(3, 5) * _tmp28 + P(4, 5) * _tmp26 + P(5, 5) * _tmp29); - _innov_var(1, 0) = R - - _tmp31 * (-P(1, 3) * _tmp34 - P(2, 3) * _tmp39 - P(23, 3) * _tmp40 - - P(3, 3) * _tmp31 - P(4, 3) * _tmp38 - P(5, 3) * _tmp36) - - _tmp34 * (-P(1, 1) * _tmp34 - P(2, 1) * _tmp39 - P(23, 1) * _tmp40 - - P(3, 1) * _tmp31 - P(4, 1) * _tmp38 - P(5, 1) * _tmp36) - - _tmp36 * (-P(1, 5) * _tmp34 - P(2, 5) * _tmp39 - P(23, 5) * _tmp40 - - P(3, 5) * _tmp31 - P(4, 5) * _tmp38 - P(5, 5) * _tmp36) - - _tmp38 * (-P(1, 4) * _tmp34 - P(2, 4) * _tmp39 - P(23, 4) * _tmp40 - - P(3, 4) * _tmp31 - P(4, 4) * _tmp38 - P(5, 4) * _tmp36) - - _tmp39 * (-P(1, 2) * _tmp34 - P(2, 2) * _tmp39 - P(23, 2) * _tmp40 - - P(3, 2) * _tmp31 - P(4, 2) * _tmp38 - P(5, 2) * _tmp36) - - _tmp40 * (-P(1, 23) * _tmp34 - P(2, 23) * _tmp39 - P(23, 23) * _tmp40 - - P(3, 23) * _tmp31 - P(4, 23) * _tmp38 - P(5, 23) * _tmp36); + _innov_var(0, 0) = + R + + _tmp17 * (P(0, 0) * _tmp17 + P(2, 0) * _tmp30 + P(23, 0) * _tmp25 - P(24, 0) * _tmp26 + + P(3, 0) * _tmp27 + P(4, 0) * _tmp6 + P(5, 0) * _tmp31) + + _tmp25 * (P(0, 23) * _tmp17 + P(2, 23) * _tmp30 + P(23, 23) * _tmp25 - P(24, 23) * _tmp26 + + P(3, 23) * _tmp27 + P(4, 23) * _tmp6 + P(5, 23) * _tmp31) - + _tmp26 * (P(0, 24) * _tmp17 + P(2, 24) * _tmp30 + P(23, 24) * _tmp25 - P(24, 24) * _tmp26 + + P(3, 24) * _tmp27 + P(4, 24) * _tmp6 + P(5, 24) * _tmp31) + + _tmp27 * (P(0, 3) * _tmp17 + P(2, 3) * _tmp30 + P(23, 3) * _tmp25 - P(24, 3) * _tmp26 + + P(3, 3) * _tmp27 + P(4, 3) * _tmp6 + P(5, 3) * _tmp31) + + _tmp30 * (P(0, 2) * _tmp17 + P(2, 2) * _tmp30 + P(23, 2) * _tmp25 - P(24, 2) * _tmp26 + + P(3, 2) * _tmp27 + P(4, 2) * _tmp6 + P(5, 2) * _tmp31) + + _tmp31 * (P(0, 5) * _tmp17 + P(2, 5) * _tmp30 + P(23, 5) * _tmp25 - P(24, 5) * _tmp26 + + P(3, 5) * _tmp27 + P(4, 5) * _tmp6 + P(5, 5) * _tmp31) + + _tmp6 * (P(0, 4) * _tmp17 + P(2, 4) * _tmp30 + P(23, 4) * _tmp25 - P(24, 4) * _tmp26 + + P(3, 4) * _tmp27 + P(4, 4) * _tmp6 + P(5, 4) * _tmp31); + _innov_var(1, 0) = + R - + _tmp34 * (-P(1, 1) * _tmp34 - P(2, 1) * _tmp35 - P(23, 1) * _tmp44 + P(24, 1) * _tmp40 - + P(3, 1) * _tmp41 - P(4, 1) * _tmp42 - P(5, 1) * _tmp43) - + _tmp35 * (-P(1, 2) * _tmp34 - P(2, 2) * _tmp35 - P(23, 2) * _tmp44 + P(24, 2) * _tmp40 - + P(3, 2) * _tmp41 - P(4, 2) * _tmp42 - P(5, 2) * _tmp43) + + _tmp40 * (-P(1, 24) * _tmp34 - P(2, 24) * _tmp35 - P(23, 24) * _tmp44 + P(24, 24) * _tmp40 - + P(3, 24) * _tmp41 - P(4, 24) * _tmp42 - P(5, 24) * _tmp43) - + _tmp41 * (-P(1, 3) * _tmp34 - P(2, 3) * _tmp35 - P(23, 3) * _tmp44 + P(24, 3) * _tmp40 - + P(3, 3) * _tmp41 - P(4, 3) * _tmp42 - P(5, 3) * _tmp43) - + _tmp42 * (-P(1, 4) * _tmp34 - P(2, 4) * _tmp35 - P(23, 4) * _tmp44 + P(24, 4) * _tmp40 - + P(3, 4) * _tmp41 - P(4, 4) * _tmp42 - P(5, 4) * _tmp43) - + _tmp43 * (-P(1, 5) * _tmp34 - P(2, 5) * _tmp35 - P(23, 5) * _tmp44 + P(24, 5) * _tmp40 - + P(3, 5) * _tmp41 - P(4, 5) * _tmp42 - P(5, 5) * _tmp43) - + _tmp44 * (-P(1, 23) * _tmp34 - P(2, 23) * _tmp35 - P(23, 23) * _tmp44 + P(24, 23) * _tmp40 - + P(3, 23) * _tmp41 - P(4, 23) * _tmp42 - P(5, 23) * _tmp43); } if (H != nullptr) { - matrix::Matrix& _h = (*H); + matrix::Matrix& _h = (*H); _h.setZero(); - _h(0, 0) = _tmp27; - _h(2, 0) = _tmp17; - _h(3, 0) = _tmp28; - _h(4, 0) = _tmp26; - _h(5, 0) = _tmp29; + _h(0, 0) = _tmp17; + _h(2, 0) = _tmp30; + _h(3, 0) = _tmp27; + _h(4, 0) = _tmp6; + _h(5, 0) = _tmp31; _h(23, 0) = _tmp25; + _h(24, 0) = -_tmp26; } } // NOLINT(readability/fn_size) 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 index c0385b9d2e..9d2f06f847 100644 --- 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 @@ -16,87 +16,94 @@ namespace sym { * Symbolic function: compute_flow_y_innov_var_and_h * * Args: - * state: Matrix25_1 - * P: Matrix24_24 + * state: Matrix26_1 + * P: Matrix25_25 * distance: Scalar * R: Scalar * epsilon: Scalar * * Outputs: * innov_var: Scalar - * H: Matrix24_1 + * H: Matrix25_1 */ template -void ComputeFlowYInnovVarAndH(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar distance, +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: 148 + matrix::Matrix* const H = nullptr) { + // Total ops: 167 + + // Unused inputs + (void)epsilon; // Input arrays - // Intermediate terms (24) - const Scalar _tmp0 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp1 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp2 = -2 * _tmp0 - 2 * _tmp1 + 1; - const Scalar _tmp3 = - Scalar(1.0) / - (distance + epsilon * (2 * math::min(0, (((distance) > 0) - ((distance) < 0))) + 1)); - const Scalar _tmp4 = _tmp3 * state(24, 0); - const Scalar _tmp5 = _tmp2 * _tmp4; - const Scalar _tmp6 = 2 * state(0, 0); - const Scalar _tmp7 = -_tmp6 * state(2, 0); - const Scalar _tmp8 = 2 * state(1, 0); - const Scalar _tmp9 = _tmp8 * state(3, 0); - const Scalar _tmp10 = 2 * state(2, 0) * state(3, 0); - const Scalar _tmp11 = _tmp8 * state(0, 0); - const Scalar _tmp12 = std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp13 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp14 = -_tmp0 + _tmp1; - const Scalar _tmp15 = _tmp4 * (state(4, 0) * (_tmp7 - _tmp9) + state(5, 0) * (-_tmp10 + _tmp11) + - state(6, 0) * (-_tmp12 + _tmp13 + _tmp14)); - const Scalar _tmp16 = _tmp7 + _tmp9; - const Scalar _tmp17 = _tmp16 * _tmp4; - const Scalar _tmp18 = _tmp6 * state(3, 0); - const Scalar _tmp19 = _tmp8 * state(2, 0); - const Scalar _tmp20 = _tmp18 + _tmp19; - const Scalar _tmp21 = _tmp20 * _tmp4; - const Scalar _tmp22 = - _tmp4 * (state(4, 0) * (-_tmp18 + _tmp19) + state(5, 0) * (_tmp12 - _tmp13 + _tmp14) + - state(6, 0) * (_tmp10 + _tmp11)); - const Scalar _tmp23 = _tmp3 * (_tmp16 * state(6, 0) + _tmp2 * state(4, 0) + _tmp20 * state(5, 0)); + // Intermediate terms (25) + const Scalar _tmp0 = 2 * state(0, 0); + const Scalar _tmp1 = -_tmp0 * state(2, 0); + const Scalar _tmp2 = 2 * state(3, 0); + const Scalar _tmp3 = _tmp2 * state(1, 0); + const Scalar _tmp4 = _tmp2 * state(2, 0); + const Scalar _tmp5 = _tmp0 * state(1, 0); + const Scalar _tmp6 = std::pow(state(0, 0), Scalar(2)); + const Scalar _tmp7 = std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp8 = std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp9 = std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp10 = -_tmp8 + _tmp9; + const Scalar _tmp11 = std::pow(distance, Scalar(-state(25, 0))); + const Scalar _tmp12 = _tmp11 * state(24, 0); + const Scalar _tmp13 = _tmp12 * (state(4, 0) * (_tmp1 - _tmp3) + state(5, 0) * (-_tmp4 + _tmp5) + + state(6, 0) * (_tmp10 - _tmp6 + _tmp7)); + const Scalar _tmp14 = _tmp2 * state(0, 0); + const Scalar _tmp15 = 2 * state(1, 0) * state(2, 0); + const Scalar _tmp16 = + _tmp12 * (state(4, 0) * (-_tmp14 + _tmp15) + state(5, 0) * (_tmp10 + _tmp6 - _tmp7) + + state(6, 0) * (_tmp4 + _tmp5)); + const Scalar _tmp17 = _tmp1 + _tmp3; + const Scalar _tmp18 = _tmp14 + _tmp15; + const Scalar _tmp19 = -2 * _tmp8 - 2 * _tmp9 + 1; + const Scalar _tmp20 = + _tmp11 * (_tmp17 * state(6, 0) + _tmp18 * state(5, 0) + _tmp19 * state(4, 0)); + const Scalar _tmp21 = _tmp20 * state(24, 0) * std::log(distance); + const Scalar _tmp22 = _tmp12 * _tmp19; + const Scalar _tmp23 = _tmp12 * _tmp18; + const Scalar _tmp24 = _tmp12 * _tmp17; // Output terms (2) if (innov_var != nullptr) { Scalar& _innov_var = (*innov_var); - _innov_var = R - - _tmp15 * (-P(1, 1) * _tmp15 - P(2, 1) * _tmp22 - P(23, 1) * _tmp23 - - P(3, 1) * _tmp5 - P(4, 1) * _tmp21 - P(5, 1) * _tmp17) - - _tmp17 * (-P(1, 5) * _tmp15 - P(2, 5) * _tmp22 - P(23, 5) * _tmp23 - - P(3, 5) * _tmp5 - P(4, 5) * _tmp21 - P(5, 5) * _tmp17) - - _tmp21 * (-P(1, 4) * _tmp15 - P(2, 4) * _tmp22 - P(23, 4) * _tmp23 - - P(3, 4) * _tmp5 - P(4, 4) * _tmp21 - P(5, 4) * _tmp17) - - _tmp22 * (-P(1, 2) * _tmp15 - P(2, 2) * _tmp22 - P(23, 2) * _tmp23 - - P(3, 2) * _tmp5 - P(4, 2) * _tmp21 - P(5, 2) * _tmp17) - - _tmp23 * (-P(1, 23) * _tmp15 - P(2, 23) * _tmp22 - P(23, 23) * _tmp23 - - P(3, 23) * _tmp5 - P(4, 23) * _tmp21 - P(5, 23) * _tmp17) - - _tmp5 * (-P(1, 3) * _tmp15 - P(2, 3) * _tmp22 - P(23, 3) * _tmp23 - - P(3, 3) * _tmp5 - P(4, 3) * _tmp21 - P(5, 3) * _tmp17); + _innov_var = + R - + _tmp13 * (-P(1, 1) * _tmp13 - P(2, 1) * _tmp16 - P(23, 1) * _tmp20 + P(24, 1) * _tmp21 - + P(3, 1) * _tmp22 - P(4, 1) * _tmp23 - P(5, 1) * _tmp24) - + _tmp16 * (-P(1, 2) * _tmp13 - P(2, 2) * _tmp16 - P(23, 2) * _tmp20 + P(24, 2) * _tmp21 - + P(3, 2) * _tmp22 - P(4, 2) * _tmp23 - P(5, 2) * _tmp24) - + _tmp20 * (-P(1, 23) * _tmp13 - P(2, 23) * _tmp16 - P(23, 23) * _tmp20 + P(24, 23) * _tmp21 - + P(3, 23) * _tmp22 - P(4, 23) * _tmp23 - P(5, 23) * _tmp24) + + _tmp21 * (-P(1, 24) * _tmp13 - P(2, 24) * _tmp16 - P(23, 24) * _tmp20 + P(24, 24) * _tmp21 - + P(3, 24) * _tmp22 - P(4, 24) * _tmp23 - P(5, 24) * _tmp24) - + _tmp22 * (-P(1, 3) * _tmp13 - P(2, 3) * _tmp16 - P(23, 3) * _tmp20 + P(24, 3) * _tmp21 - + P(3, 3) * _tmp22 - P(4, 3) * _tmp23 - P(5, 3) * _tmp24) - + _tmp23 * (-P(1, 4) * _tmp13 - P(2, 4) * _tmp16 - P(23, 4) * _tmp20 + P(24, 4) * _tmp21 - + P(3, 4) * _tmp22 - P(4, 4) * _tmp23 - P(5, 4) * _tmp24) - + _tmp24 * (-P(1, 5) * _tmp13 - P(2, 5) * _tmp16 - P(23, 5) * _tmp20 + P(24, 5) * _tmp21 - + P(3, 5) * _tmp22 - P(4, 5) * _tmp23 - P(5, 5) * _tmp24); } if (H != nullptr) { - matrix::Matrix& _h = (*H); + matrix::Matrix& _h = (*H); _h.setZero(); - _h(1, 0) = -_tmp15; - _h(2, 0) = -_tmp22; - _h(3, 0) = -_tmp5; - _h(4, 0) = -_tmp21; - _h(5, 0) = -_tmp17; - _h(23, 0) = -_tmp23; + _h(1, 0) = -_tmp13; + _h(2, 0) = -_tmp16; + _h(3, 0) = -_tmp22; + _h(4, 0) = -_tmp23; + _h(5, 0) = -_tmp24; + _h(23, 0) = -_tmp20; + _h(24, 0) = _tmp21; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h index 4af9971a40..bebcd8cc77 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h @@ -16,8 +16,8 @@ namespace sym { * Symbolic function: compute_gnss_yaw_pred_innov_var_and_h * * Args: - * state: Matrix25_1 - * P: Matrix24_24 + * state: Matrix26_1 + * P: Matrix25_25 * antenna_yaw_offset: Scalar * R: Scalar * epsilon: Scalar @@ -25,15 +25,15 @@ namespace sym { * Outputs: * meas_pred: Scalar * innov_var: Scalar - * H: Matrix24_1 + * H: Matrix25_1 */ template -void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix& state, - const matrix::Matrix& P, +void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar antenna_yaw_offset, const Scalar R, const Scalar epsilon, Scalar* const meas_pred = nullptr, Scalar* const innov_var = nullptr, - matrix::Matrix* const H = nullptr) { + matrix::Matrix* const H = nullptr) { // Total ops: 95 // Input arrays @@ -88,7 +88,7 @@ void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix& state, } if (H != nullptr) { - matrix::Matrix& _h = (*H); + matrix::Matrix& _h = (*H); _h.setZero(); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h index 5ea56ed8f8..9f2c2b4c32 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h @@ -16,8 +16,8 @@ namespace sym { * Symbolic function: compute_gravity_innov_var_and_k_and_h * * Args: - * state: Matrix25_1 - * P: Matrix24_24 + * state: Matrix26_1 + * P: Matrix25_25 * meas: Matrix31 * R: Scalar * epsilon: Scalar @@ -25,21 +25,21 @@ namespace sym { * Outputs: * innov: Matrix31 * innov_var: Matrix31 - * Kx: Matrix24_1 - * Ky: Matrix24_1 - * Kz: Matrix24_1 + * Kx: Matrix25_1 + * Ky: Matrix25_1 + * Kz: Matrix25_1 */ template -void ComputeGravityInnovVarAndKAndH(const matrix::Matrix& state, - const matrix::Matrix& P, +void ComputeGravityInnovVarAndKAndH(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 Kx = nullptr, - matrix::Matrix* const Ky = nullptr, - matrix::Matrix* const Kz = nullptr) { - // Total ops: 373 + matrix::Matrix* const Kx = nullptr, + matrix::Matrix* const Ky = nullptr, + matrix::Matrix* const Kz = nullptr) { + // Total ops: 385 // Input arrays @@ -103,7 +103,7 @@ void ComputeGravityInnovVarAndKAndH(const matrix::Matrix& state, } if (Kx != nullptr) { - matrix::Matrix& _kx = (*Kx); + matrix::Matrix& _kx = (*Kx); _kx(0, 0) = _tmp28 * (P(0, 1) * _tmp13 + P(0, 2) * _tmp14); _kx(1, 0) = _tmp28 * (P(1, 2) * _tmp14 + _tmp16); @@ -129,10 +129,11 @@ void ComputeGravityInnovVarAndKAndH(const matrix::Matrix& state, _kx(21, 0) = _tmp28 * (P(21, 1) * _tmp13 + P(21, 2) * _tmp14); _kx(22, 0) = _tmp28 * (P(22, 1) * _tmp13 + P(22, 2) * _tmp14); _kx(23, 0) = _tmp28 * (P(23, 1) * _tmp13 + P(23, 2) * _tmp14); + _kx(24, 0) = _tmp28 * (P(24, 1) * _tmp13 + P(24, 2) * _tmp14); } if (Ky != nullptr) { - matrix::Matrix& _ky = (*Ky); + matrix::Matrix& _ky = (*Ky); _ky(0, 0) = _tmp29 * (P(0, 2) * _tmp19 + _tmp21); _ky(1, 0) = _tmp29 * (P(1, 0) * _tmp18 + P(1, 2) * _tmp19); @@ -158,10 +159,11 @@ void ComputeGravityInnovVarAndKAndH(const matrix::Matrix& state, _ky(21, 0) = _tmp29 * (P(21, 0) * _tmp18 + P(21, 2) * _tmp19); _ky(22, 0) = _tmp29 * (P(22, 0) * _tmp18 + P(22, 2) * _tmp19); _ky(23, 0) = _tmp29 * (P(23, 0) * _tmp18 + P(23, 2) * _tmp19); + _ky(24, 0) = _tmp29 * (P(24, 0) * _tmp18 + P(24, 2) * _tmp19); } if (Kz != nullptr) { - matrix::Matrix& _kz = (*Kz); + matrix::Matrix& _kz = (*Kz); _kz(0, 0) = _tmp30 * (P(0, 1) * _tmp24 + _tmp26); _kz(1, 0) = _tmp30 * (P(1, 0) * _tmp23 + _tmp25); @@ -187,6 +189,7 @@ void ComputeGravityInnovVarAndKAndH(const matrix::Matrix& state, _kz(21, 0) = _tmp30 * (P(21, 0) * _tmp23 + P(21, 1) * _tmp24); _kz(22, 0) = _tmp30 * (P(22, 0) * _tmp23 + P(22, 1) * _tmp24); _kz(23, 0) = _tmp30 * (P(23, 0) * _tmp23 + P(23, 1) * _tmp24); + _kz(24, 0) = _tmp30 * (P(24, 0) * _tmp23 + P(24, 1) * _tmp24); } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_declination_pred_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_declination_pred_innov_var_and_h.h index 803bf45e87..0a816a55c6 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_declination_pred_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_declination_pred_innov_var_and_h.h @@ -16,22 +16,22 @@ namespace sym { * Symbolic function: compute_mag_declination_pred_innov_var_and_h * * Args: - * state: Matrix25_1 - * P: Matrix24_24 + * state: Matrix26_1 + * P: Matrix25_25 * R: Scalar * epsilon: Scalar * * Outputs: * pred: Scalar * innov_var: Scalar - * H: Matrix24_1 + * H: Matrix25_1 */ template -void ComputeMagDeclinationPredInnovVarAndH(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar R, +void ComputeMagDeclinationPredInnovVarAndH(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar R, const Scalar epsilon, Scalar* const pred = nullptr, Scalar* const innov_var = nullptr, - matrix::Matrix* const H = nullptr) { + matrix::Matrix* const H = nullptr) { // Total ops: 22 // Input arrays @@ -59,7 +59,7 @@ void ComputeMagDeclinationPredInnovVarAndH(const matrix::Matrix& } if (H != nullptr) { - matrix::Matrix& _h = (*H); + matrix::Matrix& _h = (*H); _h.setZero(); 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 index 9f8a96895f..f49cd16fe6 100644 --- 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 @@ -16,8 +16,8 @@ namespace sym { * Symbolic function: compute_mag_innov_innov_var_and_hx * * Args: - * state: Matrix25_1 - * P: Matrix24_24 + * state: Matrix26_1 + * P: Matrix25_25 * meas: Matrix31 * R: Scalar * epsilon: Scalar @@ -25,16 +25,16 @@ namespace sym { * Outputs: * innov: Matrix31 * innov_var: Matrix31 - * Hx: Matrix24_1 + * Hx: Matrix25_1 */ template -void ComputeMagInnovInnovVarAndHx(const matrix::Matrix& state, - const matrix::Matrix& P, +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) { + matrix::Matrix* const Hx = nullptr) { // Total ops: 314 // Unused inputs @@ -146,7 +146,7 @@ void ComputeMagInnovInnovVarAndHx(const matrix::Matrix& state, } if (Hx != nullptr) { - matrix::Matrix& _hx = (*Hx); + matrix::Matrix& _hx = (*Hx); _hx.setZero(); 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 index 1934332929..0e4d3e43a1 100644 --- 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 @@ -16,20 +16,20 @@ namespace sym { * Symbolic function: compute_mag_y_innov_var_and_h * * Args: - * state: Matrix25_1 - * P: Matrix24_24 + * state: Matrix26_1 + * P: Matrix25_25 * R: Scalar * epsilon: Scalar * * Outputs: * innov_var: Scalar - * H: Matrix24_1 + * H: Matrix25_1 */ template -void ComputeMagYInnovVarAndH(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar R, +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) { + matrix::Matrix* const H = nullptr) { // Total ops: 110 // Unused inputs @@ -78,7 +78,7 @@ void ComputeMagYInnovVarAndH(const matrix::Matrix& state, } if (H != nullptr) { - matrix::Matrix& _h = (*H); + matrix::Matrix& _h = (*H); _h.setZero(); 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 index 0a3b6bd031..1f7aa8ea44 100644 --- 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 @@ -16,20 +16,20 @@ namespace sym { * Symbolic function: compute_mag_z_innov_var_and_h * * Args: - * state: Matrix25_1 - * P: Matrix24_24 + * state: Matrix26_1 + * P: Matrix25_25 * R: Scalar * epsilon: Scalar * * Outputs: * innov_var: Scalar - * H: Matrix24_1 + * H: Matrix25_1 */ template -void ComputeMagZInnovVarAndH(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar R, +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) { + matrix::Matrix* const H = nullptr) { // Total ops: 110 // Unused inputs @@ -78,7 +78,7 @@ void ComputeMagZInnovVarAndH(const matrix::Matrix& state, } if (H != nullptr) { - matrix::Matrix& _h = (*H); + matrix::Matrix& _h = (*H); _h.setZero(); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h index 0b6d290976..155e1cda4a 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h @@ -16,21 +16,21 @@ namespace sym { * Symbolic function: compute_sideslip_h_and_k * * Args: - * state: Matrix25_1 - * P: Matrix24_24 + * state: Matrix26_1 + * P: Matrix25_25 * innov_var: Scalar * epsilon: Scalar * * Outputs: - * H: Matrix24_1 - * K: Matrix24_1 + * H: Matrix25_1 + * K: Matrix25_1 */ template -void ComputeSideslipHAndK(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar innov_var, - const Scalar epsilon, matrix::Matrix* const H = nullptr, - matrix::Matrix* const K = nullptr) { - // Total ops: 485 +void ComputeSideslipHAndK(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar innov_var, + const Scalar epsilon, matrix::Matrix* const H = nullptr, + matrix::Matrix* const K = nullptr) { + // Total ops: 501 // Input arrays @@ -42,20 +42,20 @@ void ComputeSideslipHAndK(const matrix::Matrix& state, const Scalar _tmp4 = -state(22, 0) + state(4, 0); const Scalar _tmp5 = 2 * state(0, 0); const Scalar _tmp6 = _tmp5 * state(3, 0); - const Scalar _tmp7 = 2 * state(1, 0); - const Scalar _tmp8 = _tmp7 * state(2, 0); + const Scalar _tmp7 = 2 * state(2, 0); + const Scalar _tmp8 = _tmp7 * state(1, 0); const Scalar _tmp9 = _tmp6 + _tmp8; const Scalar _tmp10 = -state(23, 0) + state(5, 0); - const Scalar _tmp11 = _tmp5 * state(2, 0); + const Scalar _tmp11 = _tmp7 * state(0, 0); const Scalar _tmp12 = -_tmp11; - const Scalar _tmp13 = _tmp7 * state(3, 0); + const Scalar _tmp13 = 2 * state(1, 0) * state(3, 0); const Scalar _tmp14 = _tmp12 + _tmp13; const Scalar _tmp15 = _tmp10 * _tmp9 + _tmp14 * state(6, 0) + _tmp3 * _tmp4; const Scalar _tmp16 = _tmp15 + epsilon * (2 * math::min(0, (((_tmp15) > 0) - ((_tmp15) < 0))) + 1); const Scalar _tmp17 = Scalar(1.0) / (_tmp16); - const Scalar _tmp18 = 2 * state(2, 0) * state(3, 0); - const Scalar _tmp19 = _tmp7 * state(0, 0); + const Scalar _tmp18 = _tmp7 * state(3, 0); + const Scalar _tmp19 = _tmp5 * state(1, 0); const Scalar _tmp20 = std::pow(state(0, 0), Scalar(2)); const Scalar _tmp21 = std::pow(state(1, 0), Scalar(2)); const Scalar _tmp22 = -_tmp21; @@ -90,7 +90,7 @@ void ComputeSideslipHAndK(const matrix::Matrix& state, // Output terms (2) if (H != nullptr) { - matrix::Matrix& _h = (*H); + matrix::Matrix& _h = (*H); _h.setZero(); @@ -105,7 +105,7 @@ void ComputeSideslipHAndK(const matrix::Matrix& state, } if (K != nullptr) { - matrix::Matrix& _k = (*K); + matrix::Matrix& _k = (*K); _k(0, 0) = _tmp45 * (P(0, 0) * _tmp24 - P(0, 1) * _tmp34 + P(0, 2) * _tmp35 + P(0, 21) * _tmp43 + @@ -179,6 +179,9 @@ void ComputeSideslipHAndK(const matrix::Matrix& state, _k(23, 0) = _tmp45 * (P(23, 0) * _tmp24 - P(23, 1) * _tmp34 + P(23, 2) * _tmp35 + P(23, 21) * _tmp43 + P(23, 22) * _tmp44 + P(23, 3) * _tmp38 + P(23, 4) * _tmp41 + P(23, 5) * _tmp42); + _k(24, 0) = + _tmp45 * (P(24, 0) * _tmp24 - P(24, 1) * _tmp34 + P(24, 2) * _tmp35 + P(24, 21) * _tmp43 + + P(24, 22) * _tmp44 + P(24, 3) * _tmp38 + P(24, 4) * _tmp41 + P(24, 5) * _tmp42); } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h index 8f92427b86..77e31514de 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h @@ -16,8 +16,8 @@ namespace sym { * Symbolic function: compute_sideslip_innov_and_innov_var * * Args: - * state: Matrix25_1 - * P: Matrix24_24 + * state: Matrix26_1 + * P: Matrix25_25 * R: Scalar * epsilon: Scalar * @@ -26,8 +26,8 @@ namespace sym { * innov_var: Scalar */ template -void ComputeSideslipInnovAndInnovVar(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar R, +void ComputeSideslipInnovAndInnovVar(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar R, const Scalar epsilon, Scalar* const innov = nullptr, Scalar* const innov_var = nullptr) { // Total ops: 235 diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h index 000dc4cf5b..83121aa1b9 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h @@ -16,20 +16,20 @@ namespace sym { * Symbolic function: compute_yaw_312_innov_var_and_h * * Args: - * state: Matrix25_1 - * P: Matrix24_24 + * state: Matrix26_1 + * P: Matrix25_25 * R: Scalar * epsilon: Scalar * * Outputs: * innov_var: Scalar - * H: Matrix24_1 + * H: Matrix25_1 */ template -void ComputeYaw312InnovVarAndH(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar R, +void ComputeYaw312InnovVarAndH(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar R, const Scalar epsilon, Scalar* const innov_var = nullptr, - matrix::Matrix* const H = nullptr) { + matrix::Matrix* const H = nullptr) { // Total ops: 53 // Input arrays @@ -63,7 +63,7 @@ void ComputeYaw312InnovVarAndH(const matrix::Matrix& state, } if (H != nullptr) { - matrix::Matrix& _h = (*H); + matrix::Matrix& _h = (*H); _h.setZero(); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h_alternate.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h_alternate.h index c8de0e6a8e..39bea6503a 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h_alternate.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h_alternate.h @@ -16,20 +16,20 @@ namespace sym { * Symbolic function: compute_yaw_312_innov_var_and_h_alternate * * Args: - * state: Matrix25_1 - * P: Matrix24_24 + * state: Matrix26_1 + * P: Matrix25_25 * R: Scalar * epsilon: Scalar * * Outputs: * innov_var: Scalar - * H: Matrix24_1 + * H: Matrix25_1 */ template -void ComputeYaw312InnovVarAndHAlternate(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar R, +void ComputeYaw312InnovVarAndHAlternate(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar R, const Scalar epsilon, Scalar* const innov_var = nullptr, - matrix::Matrix* const H = nullptr) { + matrix::Matrix* const H = nullptr) { // Total ops: 57 // Input arrays @@ -63,7 +63,7 @@ void ComputeYaw312InnovVarAndHAlternate(const matrix::Matrix& sta } if (H != nullptr) { - matrix::Matrix& _h = (*H); + matrix::Matrix& _h = (*H); _h.setZero(); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h index 0ad1e85d96..d35e2e4415 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h @@ -16,20 +16,20 @@ namespace sym { * Symbolic function: compute_yaw_321_innov_var_and_h * * Args: - * state: Matrix25_1 - * P: Matrix24_24 + * state: Matrix26_1 + * P: Matrix25_25 * R: Scalar * epsilon: Scalar * * Outputs: * innov_var: Scalar - * H: Matrix24_1 + * H: Matrix25_1 */ template -void ComputeYaw321InnovVarAndH(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar R, +void ComputeYaw321InnovVarAndH(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar R, const Scalar epsilon, Scalar* const innov_var = nullptr, - matrix::Matrix* const H = nullptr) { + matrix::Matrix* const H = nullptr) { // Total ops: 53 // Input arrays @@ -63,7 +63,7 @@ void ComputeYaw321InnovVarAndH(const matrix::Matrix& state, } if (H != nullptr) { - matrix::Matrix& _h = (*H); + matrix::Matrix& _h = (*H); _h.setZero(); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h_alternate.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h_alternate.h index c744498b53..01c22720ed 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h_alternate.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h_alternate.h @@ -16,20 +16,20 @@ namespace sym { * Symbolic function: compute_yaw_321_innov_var_and_h_alternate * * Args: - * state: Matrix25_1 - * P: Matrix24_24 + * state: Matrix26_1 + * P: Matrix25_25 * R: Scalar * epsilon: Scalar * * Outputs: * innov_var: Scalar - * H: Matrix24_1 + * H: Matrix25_1 */ template -void ComputeYaw321InnovVarAndHAlternate(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar R, +void ComputeYaw321InnovVarAndHAlternate(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar R, const Scalar epsilon, Scalar* const innov_var = nullptr, - matrix::Matrix* const H = nullptr) { + matrix::Matrix* const H = nullptr) { // Total ops: 57 // Input arrays @@ -63,7 +63,7 @@ void ComputeYaw321InnovVarAndHAlternate(const matrix::Matrix& sta } if (H != nullptr) { - matrix::Matrix& _h = (*H); + matrix::Matrix& _h = (*H); _h.setZero(); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/predict_covariance.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/predict_covariance.h index d101b41ce2..a6137b75c2 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/predict_covariance.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/predict_covariance.h @@ -16,8 +16,8 @@ namespace sym { * Symbolic function: predict_covariance * * Args: - * state: Matrix25_1 - * P: Matrix24_24 + * state: Matrix26_1 + * P: Matrix25_25 * d_vel: Matrix31 * d_vel_dt: Scalar * d_vel_var: Matrix31 @@ -26,21 +26,21 @@ namespace sym { * d_ang_var: Scalar * * Outputs: - * res: Matrix24_24 + * res: Matrix25_25 */ template -matrix::Matrix PredictCovariance(const matrix::Matrix& state, - const matrix::Matrix& P, +matrix::Matrix PredictCovariance(const matrix::Matrix& state, + const matrix::Matrix& P, const matrix::Matrix& d_vel, const Scalar d_vel_dt, const matrix::Matrix& d_vel_var, const matrix::Matrix& d_ang, const Scalar d_ang_dt, const Scalar d_ang_var) { - // Total ops: 2519 + // Total ops: 2592 // Input arrays - // Intermediate terms (246) + // Intermediate terms (244) const Scalar _tmp0 = d_ang(1, 0) - d_ang_dt * state(11, 0); const Scalar _tmp1 = Scalar(0.5) * state(2, 0); const Scalar _tmp2 = d_ang(0, 0) - d_ang_dt * state(10, 0); @@ -48,310 +48,308 @@ matrix::Matrix PredictCovariance(const matrix::Matrix _res; + matrix::Matrix _res; - _res(0, 0) = _tmp19 * _tmp82 + _tmp49 * _tmp88 + _tmp58 * _tmp89 + _tmp63 * _tmp87 + + _res(0, 0) = _tmp19 * _tmp82 + _tmp50 * _tmp88 + _tmp58 * _tmp89 + _tmp63 * _tmp87 + _tmp72 * _tmp90 + _tmp81 * _tmp91 + std::pow(_tmp83, Scalar(2)) * d_ang_var + std::pow(_tmp84, Scalar(2)) * d_ang_var + _tmp86; _res(1, 0) = 0; @@ -377,11 +375,12 @@ matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix mag_B{}; matrix::Vector2 wind_vel{}; float flow_scale{}; + float flow_exp{}; - matrix::Vector Data() const { - matrix::Vector state; + matrix::Vector Data() const { + matrix::Vector state; state.slice<4, 1>(0, 0) = quat_nominal; state.slice<3, 1>(4, 0) = vel; state.slice<3, 1>(7, 0) = pos; @@ -31,15 +32,16 @@ struct StateSample { state.slice<3, 1>(19, 0) = mag_B; state.slice<2, 1>(22, 0) = wind_vel; state.slice<1, 1>(24, 0) = flow_scale; + state.slice<1, 1>(25, 0) = flow_exp; return state; }; - const matrix::Vector& vector() const { - return *reinterpret_cast*>(const_cast(reinterpret_cast(&quat_nominal))); + const matrix::Vector& vector() const { + return *reinterpret_cast*>(const_cast(reinterpret_cast(&quat_nominal))); }; }; -static_assert(sizeof(matrix::Vector) == sizeof(StateSample), "state vector doesn't match StateSample size"); +static_assert(sizeof(matrix::Vector) == sizeof(StateSample), "state vector doesn't match StateSample size"); struct IdxDof { unsigned idx; unsigned dof; }; namespace State { @@ -52,7 +54,8 @@ namespace State { static constexpr IdxDof mag_B{18, 3}; static constexpr IdxDof wind_vel{21, 2}; static constexpr IdxDof flow_scale{23, 1}; - static constexpr uint8_t size{24}; + static constexpr IdxDof flow_exp{24, 1}; + static constexpr uint8_t size{25}; }; } #endif // !EKF_STATE_H