diff --git a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu index 95df14d54ec..27bd8397758 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu +++ b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu @@ -7,6 +7,7 @@ # # @maintainer # @board px4_fmu-v2 exclude +# @board px4_fmu-v6x exclude # . ${R}etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 b/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 index d16230d3583..4833d17af23 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 +++ b/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 @@ -13,6 +13,7 @@ # @board px4_fmu-v4pro exclude # @board px4_fmu-v5 exclude # @board px4_fmu-v5x exclude +# @board px4_fmu-v6x exclude # @board diatone_mamba-f405-mk2 exclude # . ${R}etc/init.d/rc.mc_defaults diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp index e612452b762..c338b75ba13 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp @@ -37,6 +37,9 @@ */ #include "ekf.h" +#include +#include +#include void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src) @@ -57,14 +60,16 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; // rotate measurement into correct earth frame if required - Vector3f vel{NAN, NAN, NAN}; - Matrix3f vel_cov{}; + Vector3f measurement{}; + Vector3f measurement_var{}; + + float minimum_variance = math::max(sq(0.01f), sq(_params.ev_vel_noise)); switch (ev_sample.vel_frame) { case VelocityFrame::LOCAL_FRAME_NED: if (_control_status.flags.yaw_align) { - vel = ev_sample.vel - vel_offset_earth; - vel_cov = matrix::diag(ev_sample.velocity_var); + measurement = ev_sample.vel - vel_offset_earth; + measurement_var = ev_sample.velocity_var; } else { continuing_conditions_passing = false; @@ -75,31 +80,28 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common case VelocityFrame::LOCAL_FRAME_FRD: if (_control_status.flags.ev_yaw) { // using EV frame - vel = ev_sample.vel - vel_offset_earth; - vel_cov = matrix::diag(ev_sample.velocity_var); + measurement = ev_sample.vel - vel_offset_earth; + measurement_var = ev_sample.velocity_var; } else { // rotate EV to the EKF reference frame const Dcmf R_ev_to_ekf = Dcmf(_ev_q_error_filt.getState()); - vel = R_ev_to_ekf * ev_sample.vel - vel_offset_earth; - vel_cov = R_ev_to_ekf * matrix::diag(ev_sample.velocity_var) * R_ev_to_ekf.transpose(); - - // increase minimum variance to include EV orientation variance - // TODO: do this properly - const float orientation_var_max = ev_sample.orientation_var.max(); - - for (int i = 0; i < 2; i++) { - vel_cov(i, i) = math::max(vel_cov(i, i), orientation_var_max); - } + measurement = R_ev_to_ekf * ev_sample.vel - vel_offset_earth; + measurement_var = matrix::SquareMatrix3f(R_ev_to_ekf * matrix::diag(ev_sample.velocity_var) * + R_ev_to_ekf.transpose()).diag(); + minimum_variance = math::max(minimum_variance, ev_sample.orientation_var.max()); } break; - case VelocityFrame::BODY_FRAME_FRD: - vel = _R_to_earth * (ev_sample.vel - vel_offset_body); - vel_cov = _R_to_earth * matrix::diag(ev_sample.velocity_var) * _R_to_earth.transpose(); - break; + case VelocityFrame::BODY_FRAME_FRD: { + + // currently it is assumed that the orientation of the EV frame and the body frame are the same + measurement = ev_sample.vel - vel_offset_body; + measurement_var = ev_sample.velocity_var; + break; + } default: continuing_conditions_passing = false; @@ -111,48 +113,56 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common // increase minimum variance if GPS active (position reference) if (_control_status.flags.gps) { for (int i = 0; i < 2; i++) { - vel_cov(i, i) = math::max(vel_cov(i, i), sq(_params.gps_vel_noise)); + measurement_var(i) = math::max(measurement_var(i), sq(_params.gps_vel_noise)); } } #endif // CONFIG_EKF2_GNSS - const Vector3f measurement{vel}; - - const Vector3f measurement_var{ - math::max(vel_cov(0, 0), sq(_params.ev_vel_noise), sq(0.01f)), - math::max(vel_cov(1, 1), sq(_params.ev_vel_noise), sq(0.01f)), - math::max(vel_cov(2, 2), sq(_params.ev_vel_noise), sq(0.01f)) + measurement_var = Vector3f{ + math::max(measurement_var(0), minimum_variance), + math::max(measurement_var(1), minimum_variance), + math::max(measurement_var(2), minimum_variance) }; + continuing_conditions_passing &= measurement.isAllFinite() && measurement_var.isAllFinite(); - const bool measurement_valid = measurement.isAllFinite() && measurement_var.isAllFinite(); + if (ev_sample.vel_frame == VelocityFrame::BODY_FRAME_FRD) { + const Vector3f measurement_var_ekf_frame = rotateVarianceToEkf(measurement_var); + const Vector3f measurement_ekf_frame = _R_to_earth * measurement; + const uint64_t t = aid_src.timestamp_sample; + updateAidSourceStatus(aid_src, + ev_sample.time_us, // sample timestamp + measurement_ekf_frame, // observation + measurement_var_ekf_frame, // observation variance + _state.vel - measurement_ekf_frame, // innovation + getVelocityVariance() + measurement_var_ekf_frame, // innovation variance + math::max(_params.ev_vel_innov_gate, 1.f)); // innovation gate + aid_src.timestamp_sample = t; + measurement.copyTo(aid_src.observation); + measurement_var.copyTo(aid_src.observation_variance); - updateAidSourceStatus(aid_src, - ev_sample.time_us, // sample timestamp - measurement, // observation - measurement_var, // observation variance - _state.vel - measurement, // innovation - getVelocityVariance() + measurement_var, // innovation variance - math::max(_params.ev_vel_innov_gate, 1.f)); // innovation gate - - if (!measurement_valid) { - continuing_conditions_passing = false; + } else { + updateAidSourceStatus(aid_src, + ev_sample.time_us, // sample timestamp + measurement, // observation + measurement_var, // observation variance + _state.vel - measurement, // innovation + getVelocityVariance() + measurement_var, // innovation variance + math::max(_params.ev_vel_innov_gate, 1.f)); // innovation gate } + const bool starting_conditions_passing = common_starting_conditions_passing && continuing_conditions_passing && ((Vector3f(aid_src.test_ratio).max() < 0.1f) || !isHorizontalAidingActive()); if (_control_status.flags.ev_vel) { - if (continuing_conditions_passing) { - if ((ev_reset && isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.ev_vel)) || yaw_alignment_changed) { - if (quality_sufficient) { ECL_INFO("reset to %s", AID_SRC_NAME); _information_events.flags.reset_vel_to_vision = true; - resetVelocityTo(measurement, measurement_var); + resetVelocityToEV(measurement, measurement_var, ev_sample.vel_frame); resetAidSourceStatusZeroInnovation(aid_src); } else { @@ -163,7 +173,7 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common } } else if (quality_sufficient) { - fuseVelocity(aid_src); + fuseEvVelocity(aid_src, ev_sample); } else { aid_src.innovation_rejected = true; @@ -177,24 +187,27 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common // Data seems good, attempt a reset _information_events.flags.reset_vel_to_vision = true; ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME); - resetVelocityTo(measurement, measurement_var); + resetVelocityToEV(measurement, measurement_var, ev_sample.vel_frame); resetAidSourceStatusZeroInnovation(aid_src); if (_control_status.flags.in_air) { _nb_ev_vel_reset_available--; } - } else if (starting_conditions_passing) { - // Data seems good, but previous reset did not fix the issue - // something else must be wrong, declare the sensor faulty and stop the fusion - //_control_status.flags.ev_vel_fault = true; - ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME); - stopEvVelFusion(); - } else { - // A reset did not fix the issue but all the starting checks are not passing - // This could be a temporary issue, stop the fusion without declaring the sensor faulty - ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME); + // differ warning message based on whether the starting conditions are passing + if (starting_conditions_passing) { + // Data seems good, but previous reset did not fix the issue + // something else must be wrong, declare the sensor faulty and stop the fusion + //_control_status.flags.ev_vel_fault = true; + ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME); + + } else { + // A reset did not fix the issue but all the starting checks are not passing + // This could be a temporary issue, stop the fusion without declaring the sensor faulty + ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME); + } + stopEvVelFusion(); } } @@ -211,15 +224,13 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common if (!isHorizontalAidingActive() || yaw_alignment_changed) { ECL_INFO("starting %s fusion, resetting velocity to (%.3f, %.3f, %.3f)", AID_SRC_NAME, (double)measurement(0), (double)measurement(1), (double)measurement(2)); - _information_events.flags.reset_vel_to_vision = true; - - resetVelocityTo(measurement, measurement_var); + resetVelocityToEV(measurement, measurement_var, ev_sample.vel_frame); resetAidSourceStatusZeroInnovation(aid_src); _control_status.flags.ev_vel = true; - } else if (fuseVelocity(aid_src)) { + } else if (fuseEvVelocity(aid_src, ev_sample)) { ECL_INFO("starting %s fusion", AID_SRC_NAME); _control_status.flags.ev_vel = true; } @@ -232,6 +243,63 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common } } +bool Ekf::fuseEvVelocity(estimator_aid_source3d_s &aid_src, const extVisionSample &ev_sample) +{ + if (ev_sample.vel_frame == VelocityFrame::BODY_FRAME_FRD) { + + VectorState H; + estimator_aid_source1d_s current_aid_src; + const auto state_vector = _state.vector(); + + for (uint8_t index = 0; index <= 2; index++) { + current_aid_src.timestamp_sample = aid_src.timestamp_sample; + + if (index == 0) { + sym::ComputeEvBodyVelHx(state_vector, &H); + + } else if (index == 1) { + sym::ComputeEvBodyVelHy(state_vector, &H); + + } else { + sym::ComputeEvBodyVelHz(state_vector, &H); + } + + const float innov_var = (H.T() * P * H)(0, 0) + aid_src.observation_variance[index]; + const float innov = (_R_to_earth.transpose() * _state.vel - Vector3f(aid_src.observation))(index, 0); + + updateAidSourceStatus(current_aid_src, + ev_sample.time_us, // sample timestamp + aid_src.observation[index], // observation + aid_src.observation_variance[index], // observation variance + innov, // innovation + innov_var, // innovation variance + math::max(_params.ev_vel_innov_gate, 1.f)); // innovation gate + + if (!current_aid_src.innovation_rejected) { + fuseBodyVelocity(current_aid_src, current_aid_src.innovation_variance, H); + + } + + aid_src.innovation[index] = current_aid_src.innovation; + aid_src.innovation_variance[index] = current_aid_src.innovation_variance; + aid_src.test_ratio[index] = current_aid_src.test_ratio; + aid_src.fused = current_aid_src.fused; + aid_src.innovation_rejected |= current_aid_src.innovation_rejected; + + if (aid_src.fused) { + aid_src.time_last_fuse = _time_delayed_us; + } + + } + + aid_src.timestamp_sample = current_aid_src.timestamp_sample; + return !aid_src.innovation_rejected; + + } else { + return fuseVelocity(aid_src); + } +} + void Ekf::stopEvVelFusion() { if (_control_status.flags.ev_vel) { @@ -239,3 +307,23 @@ void Ekf::stopEvVelFusion() _control_status.flags.ev_vel = false; } } + +void Ekf::resetVelocityToEV(const Vector3f &measurement, const Vector3f &measurement_var, + const VelocityFrame &vel_frame) +{ + if (vel_frame == VelocityFrame::BODY_FRAME_FRD) { + const Vector3f measurement_var_ekf_frame = rotateVarianceToEkf(measurement_var); + resetVelocityTo(_R_to_earth * measurement, measurement_var_ekf_frame); + + } else { + resetVelocityTo(measurement, measurement_var); + } + +} + +Vector3f Ekf::rotateVarianceToEkf(const Vector3f &measurement_var) +{ + // rotate the covariance matrix into the EKF frame + const matrix::SquareMatrix R_cov = _R_to_earth * matrix::diag(measurement_var) * _R_to_earth.transpose(); + return R_cov.diag(); +} diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 2a23d1d283b..f7c4ee2dcfc 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -921,6 +921,8 @@ private: void controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src); void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src); void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src); + void resetVelocityToEV(const Vector3f &measurement, const Vector3f &measurement_var, const VelocityFrame &vel_frame); + Vector3f rotateVarianceToEkf(const Vector3f &measurement_var); void startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src); void updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src); @@ -928,6 +930,12 @@ private: void stopEvHgtFusion(); void stopEvVelFusion(); void stopEvYawFusion(); + bool fuseEvVelocity(estimator_aid_source3d_s &aid_src, const extVisionSample &ev_sample); + void fuseBodyVelocity(estimator_aid_source1d_s &aid_src, float &innov_var, VectorState &H) + { + VectorState Kfusion = P * H / innov_var; + aid_src.fused = measurementUpdate(Kfusion, H, aid_src.observation_variance, aid_src.innovation); + } #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_GNSS) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 4d8b5278dbc..a206ea62954 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -362,6 +362,40 @@ def compute_sideslip_h_and_k( return (H.T, K) +def predict_vel_body( + state: VState +) -> (sf.V3): + vel = state["vel"] + R_to_body = state["quat_nominal"].inverse() + return R_to_body * vel + +def compute_ev_body_vel_hx( + state: VState, +) -> (VTangent): + + state = vstate_to_state(state) + meas_pred = predict_vel_body(state) + Hx = jacobian_chain_rule(meas_pred[0], state) + return (Hx.T) + +def compute_ev_body_vel_hy( + state: VState, +) -> (VTangent): + + state = vstate_to_state(state) + meas_pred = predict_vel_body(state)[1] + Hy = jacobian_chain_rule(meas_pred, state) + return (Hy.T) + +def compute_ev_body_vel_hz( + state: VState, +) -> (VTangent): + + state = vstate_to_state(state) + meas_pred = predict_vel_body(state)[2] + Hz = jacobian_chain_rule(meas_pred, state) + return (Hz.T) + def predict_mag_body(state) -> sf.V3: mag_field_earth = state["mag_I"] mag_bias_body = state["mag_B"] @@ -697,5 +731,8 @@ generate_px4_function(compute_gnss_yaw_pred_innov_var_and_h, output_names=["meas generate_px4_function(compute_gravity_xyz_innov_var_and_hx, output_names=["innov_var", "Hx"]) generate_px4_function(compute_gravity_y_innov_var_and_h, output_names=["innov_var", "Hy"]) generate_px4_function(compute_gravity_z_innov_var_and_h, output_names=["innov_var", "Hz"]) +generate_px4_function(compute_ev_body_vel_hx, output_names=["H"]) +generate_px4_function(compute_ev_body_vel_hy, output_names=["H"]) +generate_px4_function(compute_ev_body_vel_hz, output_names=["H"]) generate_px4_state(State, tangent_idx) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hx.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hx.h new file mode 100644 index 00000000000..72a2adf0f67 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hx.h @@ -0,0 +1,63 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// 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_ev_body_vel_hx + * + * Args: + * state: Matrix25_1 + * + * Outputs: + * H: Matrix24_1 + */ +template +void ComputeEvBodyVelHx(const matrix::Matrix& state, + matrix::Matrix* const H = nullptr) { + // Total ops: 60 + + // Input arrays + + // Intermediate terms (13) + const Scalar _tmp0 = 2 * state(5, 0); + const Scalar _tmp1 = 2 * state(6, 0); + const Scalar _tmp2 = _tmp0 * state(3, 0) - _tmp1 * state(2, 0); + const Scalar _tmp3 = (Scalar(1) / Scalar(2)) * state(1, 0); + const Scalar _tmp4 = + (Scalar(1) / Scalar(2)) * _tmp0 * state(2, 0) + (Scalar(1) / Scalar(2)) * _tmp1 * state(3, 0); + const Scalar _tmp5 = 4 * state(4, 0); + const Scalar _tmp6 = 2 * state(1, 0); + const Scalar _tmp7 = _tmp0 * state(0, 0) - _tmp5 * state(3, 0) + _tmp6 * state(6, 0); + const Scalar _tmp8 = (Scalar(1) / Scalar(2)) * _tmp7; + const Scalar _tmp9 = 2 * state(0, 0); + const Scalar _tmp10 = _tmp0 * state(1, 0) - _tmp5 * state(2, 0) - _tmp9 * state(6, 0); + const Scalar _tmp11 = (Scalar(1) / Scalar(2)) * _tmp10; + const Scalar _tmp12 = (Scalar(1) / Scalar(2)) * _tmp2; + + // Output terms (1) + if (H != nullptr) { + matrix::Matrix& _h = (*H); + + _h.setZero(); + + _h(0, 0) = -_tmp11 * state(3, 0) - _tmp2 * _tmp3 + _tmp4 * state(0, 0) + _tmp8 * state(2, 0); + _h(1, 0) = _tmp11 * state(0, 0) - _tmp12 * state(2, 0) - _tmp3 * _tmp7 + _tmp4 * state(3, 0); + _h(2, 0) = _tmp10 * _tmp3 - _tmp12 * state(3, 0) - _tmp4 * state(2, 0) + _tmp8 * state(0, 0); + _h(3, 0) = -2 * std::pow(state(2, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1; + _h(4, 0) = _tmp6 * state(2, 0) + _tmp9 * state(3, 0); + _h(5, 0) = _tmp6 * state(3, 0) - _tmp9 * state(2, 0); + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hy.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hy.h new file mode 100644 index 00000000000..a4dd6f94d94 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hy.h @@ -0,0 +1,67 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// 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_ev_body_vel_hy + * + * Args: + * state: Matrix25_1 + * + * Outputs: + * H: Matrix24_1 + */ +template +void ComputeEvBodyVelHy(const matrix::Matrix& state, + matrix::Matrix* const H = nullptr) { + // Total ops: 64 + + // Input arrays + + // Intermediate terms (9) + const Scalar _tmp0 = 2 * state(4, 0); + const Scalar _tmp1 = 2 * state(1, 0); + const Scalar _tmp2 = + -Scalar(1) / Scalar(2) * _tmp0 * state(3, 0) + (Scalar(1) / Scalar(2)) * _tmp1 * state(6, 0); + const Scalar _tmp3 = 2 * state(3, 0); + const Scalar _tmp4 = + (Scalar(1) / Scalar(2)) * _tmp0 * state(1, 0) + (Scalar(1) / Scalar(2)) * _tmp3 * state(6, 0); + const Scalar _tmp5 = 4 * state(5, 0); + const Scalar _tmp6 = 2 * state(6, 0); + const Scalar _tmp7 = -Scalar(1) / Scalar(2) * _tmp0 * state(0, 0) - + Scalar(1) / Scalar(2) * _tmp5 * state(3, 0) + + (Scalar(1) / Scalar(2)) * _tmp6 * state(2, 0); + const Scalar _tmp8 = (Scalar(1) / Scalar(2)) * _tmp0 * state(2, 0) - + Scalar(1) / Scalar(2) * _tmp5 * state(1, 0) + + (Scalar(1) / Scalar(2)) * _tmp6 * state(0, 0); + + // Output terms (1) + if (H != nullptr) { + matrix::Matrix& _h = (*H); + + _h.setZero(); + + _h(0, 0) = + -_tmp2 * state(1, 0) - _tmp4 * state(3, 0) + _tmp7 * state(2, 0) + _tmp8 * state(0, 0); + _h(1, 0) = + -_tmp2 * state(2, 0) + _tmp4 * state(0, 0) - _tmp7 * state(1, 0) + _tmp8 * state(3, 0); + _h(2, 0) = + -_tmp2 * state(3, 0) + _tmp4 * state(1, 0) + _tmp7 * state(0, 0) - _tmp8 * state(2, 0); + _h(3, 0) = _tmp1 * state(2, 0) - _tmp3 * state(0, 0); + _h(4, 0) = -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1; + _h(5, 0) = _tmp1 * state(0, 0) + _tmp3 * state(2, 0); + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hz.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hz.h new file mode 100644 index 00000000000..395bb85b4a2 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hz.h @@ -0,0 +1,63 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// 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_ev_body_vel_hz + * + * Args: + * state: Matrix25_1 + * + * Outputs: + * H: Matrix24_1 + */ +template +void ComputeEvBodyVelHz(const matrix::Matrix& state, + matrix::Matrix* const H = nullptr) { + // Total ops: 60 + + // Input arrays + + // Intermediate terms (13) + const Scalar _tmp0 = 2 * state(4, 0); + const Scalar _tmp1 = 2 * state(5, 0); + const Scalar _tmp2 = + (Scalar(1) / Scalar(2)) * _tmp0 * state(1, 0) + (Scalar(1) / Scalar(2)) * _tmp1 * state(2, 0); + const Scalar _tmp3 = _tmp0 * state(2, 0) - _tmp1 * state(1, 0); + const Scalar _tmp4 = (Scalar(1) / Scalar(2)) * _tmp3; + const Scalar _tmp5 = 4 * state(6, 0); + const Scalar _tmp6 = _tmp0 * state(3, 0) - _tmp1 * state(0, 0) - _tmp5 * state(1, 0); + const Scalar _tmp7 = (Scalar(1) / Scalar(2)) * _tmp6; + const Scalar _tmp8 = _tmp0 * state(0, 0) + _tmp1 * state(3, 0) - _tmp5 * state(2, 0); + const Scalar _tmp9 = (Scalar(1) / Scalar(2)) * state(3, 0); + const Scalar _tmp10 = (Scalar(1) / Scalar(2)) * _tmp8; + const Scalar _tmp11 = 2 * state(2, 0); + const Scalar _tmp12 = 2 * state(1, 0); + + // Output terms (1) + if (H != nullptr) { + matrix::Matrix& _h = (*H); + + _h.setZero(); + + _h(0, 0) = _tmp2 * state(2, 0) - _tmp4 * state(1, 0) + _tmp7 * state(0, 0) - _tmp8 * _tmp9; + _h(1, 0) = _tmp10 * state(0, 0) - _tmp2 * state(1, 0) - _tmp4 * state(2, 0) + _tmp6 * _tmp9; + _h(2, 0) = _tmp10 * state(1, 0) + _tmp2 * state(0, 0) - _tmp3 * _tmp9 - _tmp7 * state(2, 0); + _h(3, 0) = _tmp11 * state(0, 0) + _tmp12 * state(3, 0); + _h(4, 0) = _tmp11 * state(3, 0) - _tmp12 * state(0, 0); + _h(5, 0) = -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(2, 0), Scalar(2)) + 1; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/test/test_EKF_externalVision.cpp b/src/modules/ekf2/test/test_EKF_externalVision.cpp index 734d485fbcd..5b1439edd9f 100644 --- a/src/modules/ekf2/test/test_EKF_externalVision.cpp +++ b/src/modules/ekf2/test/test_EKF_externalVision.cpp @@ -259,37 +259,24 @@ TEST_F(EkfExternalVisionTest, visionAlignment) TEST_F(EkfExternalVisionTest, velocityFrameBody) { - // GIVEN: Drone is turned 90 degrees - const Quatf quat_sim(Eulerf(0.0f, 0.0f, math::radians(90.0f))); - _sensor_simulator.simulateOrientation(quat_sim); + _ekf_wrapper.setMagFuseTypeNone(); _sensor_simulator.runSeconds(_tilt_align_time); - _ekf->set_vehicle_at_rest(false); - // Without any measurement x and y velocity variance are close - const Vector3f velVar_init = _ekf->getVelocityVariance(); - EXPECT_NEAR(velVar_init(0), velVar_init(1), 0.0001); - - // WHEN: measurement is given in BODY-FRAME and - // x variance is bigger than y variance - _sensor_simulator._vio.setVelocityFrameToBody(); + float yaw_var0 = _ekf->getYawVar(); const Vector3f vel_cov_body(2.0f, 0.01f, 0.01f); const Vector3f vel_body(1.0f, 0.0f, 0.0f); + _sensor_simulator._vio.setVelocityFrameToBody(); _sensor_simulator._vio.setVelocityVariance(vel_cov_body); _sensor_simulator._vio.setVelocity(vel_body); _ekf_wrapper.enableExternalVisionVelocityFusion(); _sensor_simulator.startExternalVision(); _sensor_simulator.runSeconds(4); - // THEN: As the drone is turned 90 degrees, velocity variance - // along local y axis is expected to be bigger - const Vector3f velVar_new = _ekf->getVelocityVariance(); - EXPECT_NEAR(velVar_new(1) / velVar_new(0), 30.f, 15.f); - - const Vector3f vel_earth_est = _ekf->getVelocity(); - EXPECT_NEAR(vel_earth_est(0), 0.0f, 0.1f); - EXPECT_NEAR(vel_earth_est(1), 1.0f, 0.1f); + const Vector3f vel_var = _ekf->getVelocityVariance(); + EXPECT_TRUE(yaw_var0 < _ekf->getYawVar()); + EXPECT_TRUE(vel_var(1) < vel_var(0)); } TEST_F(EkfExternalVisionTest, velocityFrameLocal)