From 454ded1b25b4e5fec9bf4b5bc3668d3905b65839 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth <58551738+haumarco@users.noreply.github.com> Date: Tue, 24 Sep 2024 14:09:11 +0200 Subject: [PATCH] EKF2: ev-vel refactoring (#23577) --- .../airframes/1013_gazebo-classic_iris_vision | 2 +- .../external_vision/ev_control.cpp | 27 +- .../EKF/aid_sources/external_vision/ev_vel.h | 176 +++++++++++ .../external_vision/ev_vel_control.cpp | 283 +++++------------- src/modules/ekf2/EKF/ekf.h | 17 +- .../EKF/python/ekf_derivation/derivation.py | 51 ++-- .../generated/compute_body_vel_innov_var_h.h | 192 ++++++++++++ .../generated/compute_body_vel_y_innov_var.h | 81 +++++ .../generated/compute_body_vel_z_innov_var.h | 82 +++++ .../generated/compute_ev_body_vel_hx.h | 63 ---- .../generated/compute_ev_body_vel_hy.h | 67 ----- .../generated/compute_ev_body_vel_hz.h | 63 ---- 12 files changed, 674 insertions(+), 430 deletions(-) create mode 100644 src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel.h create mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_body_vel_innov_var_h.h create mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_body_vel_y_innov_var.h create mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_body_vel_z_innov_var.h delete mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hx.h delete mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hy.h delete mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hz.h diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1013_gazebo-classic_iris_vision b/ROMFS/px4fmu_common/init.d-posix/airframes/1013_gazebo-classic_iris_vision index de17714dff..e33a00a3de 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1013_gazebo-classic_iris_vision +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1013_gazebo-classic_iris_vision @@ -5,7 +5,7 @@ # @type Quadrotor Wide # -. ${R}etc/init.d-posix/airframes/10016_gazebo-classic_iris +. ${R}etc/init.d-posix/airframes/10015_gazebo-classic_iris # EKF2: Vision position and heading, no GPS param set-default EKF2_EV_DELAY 5 diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp index 68022677ba..e4d64d1ea9 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp @@ -37,6 +37,7 @@ */ #include "ekf.h" +#include "aid_sources/external_vision/ev_vel.h" void Ekf::controlExternalVisionFusion(const imuSample &imu_sample) { @@ -62,8 +63,32 @@ void Ekf::controlExternalVisionFusion(const imuSample &imu_sample) && isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL); updateEvAttitudeErrorFilter(ev_sample, ev_reset); + controlEvYawFusion(imu_sample, ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_yaw); - controlEvVelFusion(imu_sample, ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel); + + switch (ev_sample.vel_frame) { + case VelocityFrame::BODY_FRAME_FRD: { + EvVelBodyFrameFrd ev_vel_body(*this, ev_sample, _params.ev_vel_noise, imu_sample); + controlEvVelFusion(ev_vel_body, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel); + break; + } + + case VelocityFrame::LOCAL_FRAME_NED: { + EvVelLocalFrameNed ev_vel_ned(*this, ev_sample, _params.ev_vel_noise, imu_sample); + controlEvVelFusion(ev_vel_ned, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel); + break; + } + + case VelocityFrame::LOCAL_FRAME_FRD: { + EvVelLocalFrameFrd ev_vel_frd(*this, ev_sample, _params.ev_vel_noise, imu_sample); + controlEvVelFusion(ev_vel_frd, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel); + break; + } + + default: + return; + } + controlEvPosFusion(imu_sample, ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_pos); controlEvHeightFusion(imu_sample, ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_hgt); diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel.h b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel.h new file mode 100644 index 0000000000..2adadbac83 --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel.h @@ -0,0 +1,176 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ev_vel.h + * Handling different types of External Vision velocity measurements + */ + +#ifndef ExternalVisionVel_H +#define ExternalVisionVel_H + +#include "ekf.h" +#include + +class ExternalVisionVel +{ +public: + ExternalVisionVel(Ekf &ekf_instance, const extVisionSample &vision_sample, const float ev_vel_noise, + const imuSample &imu_sample) : _ekf(ekf_instance), _sample(vision_sample) + { + _min_variance = sq(ev_vel_noise); + const Vector3f angular_velocity = imu_sample.delta_ang / imu_sample.delta_ang_dt - _ekf._state.gyro_bias; + Vector3f position_offset_body = _ekf._params.ev_pos_body - _ekf._params.imu_pos_body; + _velocity_offset_body = angular_velocity % position_offset_body; + } + + virtual ~ExternalVisionVel() = default; + virtual bool fuseVelocity(estimator_aid_source3d_s &aid_src, float gate) + { + _ekf.fuseLocalFrameVelocity(aid_src, aid_src.timestamp, _measurement, + _measurement_var, gate); + return aid_src.fused; + + } + + virtual void resetVelocity() + { + _ekf.resetVelocityTo(_measurement, _measurement_var); + } + + void resetVerticalVelocity() + { + _ekf.resetVerticalVelocityTo(_measurement(2), _measurement_var(2)); + } + + void enforceMinimumVariance() + { + _min_variance = math::max(_min_variance, sq(0.01f)); + + for (int i = 0; i < 3; ++i) { + _measurement_var(i) = math::max(_measurement_var(i), _min_variance); + } + } + + Ekf &_ekf; + const extVisionSample &_sample; + float _min_variance; + Vector3f _measurement{NAN, NAN, NAN}; + Vector3f _measurement_var; + Vector3f _velocity_offset_body; +}; + +class EvVelBodyFrameFrd : public ExternalVisionVel +{ +public: + EvVelBodyFrameFrd(Ekf &ekf_instance, extVisionSample &vision_sample, const float ev_vel_noise, + const imuSample &imu_sample) : + ExternalVisionVel(ekf_instance, vision_sample, ev_vel_noise, imu_sample) + { + _measurement = _sample.vel - _velocity_offset_body; + _measurement_var = _sample.velocity_var; + enforceMinimumVariance(); + } + + void resetVelocity() override + { + const matrix::SquareMatrix3f rotated_variance = _ekf._R_to_earth * matrix::diag( + _measurement_var) * _ekf._R_to_earth.transpose(); + // bump the variance to decrease cross-correlation and increase uncertainty of velocity + const Vector3f measurement_variance_ekf_frame = rotated_variance.diag() * 5.f; + _ekf.resetVelocityTo(_ekf._R_to_earth * _measurement, measurement_variance_ekf_frame); + } + + void resetVerticalVelocity() + { + const matrix::SquareMatrix3f rotated_variance = _ekf._R_to_earth * matrix::diag( + _measurement_var) * _ekf._R_to_earth.transpose(); + // bump the variance to decrease cross-correlation and increase uncertainty of velocity + const Vector3f measurement_variance_ekf_frame = rotated_variance.diag() * 5.f; + _ekf.resetVerticalVelocityTo((_ekf._R_to_earth * _measurement)(2, 0), + measurement_variance_ekf_frame(2)); + } + + bool fuseVelocity(estimator_aid_source3d_s &aid_src, float gate) override + { + _ekf.fuseBodyFrameVelocity(aid_src, _sample.time_us, _measurement, + _measurement_var, gate); + return aid_src.fused; + } +}; + +class EvVelLocalFrameNed : public ExternalVisionVel +{ +public: + EvVelLocalFrameNed(Ekf &ekf_instance, extVisionSample &vision_sample, const float ev_vel_noise, + const imuSample &imu_sample) : + ExternalVisionVel(ekf_instance, vision_sample, ev_vel_noise, imu_sample) + { + const Vector3f velocity_offset_earth = _ekf._R_to_earth * _velocity_offset_body; + + if (_ekf._control_status.flags.yaw_align) { + _measurement = _sample.vel - velocity_offset_earth; + _measurement_var = _sample.velocity_var; + enforceMinimumVariance(); + } + } +}; + +class EvVelLocalFrameFrd : public ExternalVisionVel +{ +public: + EvVelLocalFrameFrd(Ekf &ekf_instance, extVisionSample &vision_sample, const float ev_vel_noise, + const imuSample &imu_sample) : + ExternalVisionVel(ekf_instance, vision_sample, ev_vel_noise, imu_sample) + { + const Vector3f velocity_offset_earth = _ekf._R_to_earth * _velocity_offset_body; + + if (_ekf._control_status.flags.ev_yaw) { + // Using EV frame + _measurement = _sample.vel - velocity_offset_earth; + _measurement_var = _sample.velocity_var; + + } else { + // Rotate EV to the EKF reference frame + const Dcmf rotation_ev_to_ekf = Dcmf(_ekf._ev_q_error_filt.getState()); + _measurement = rotation_ev_to_ekf * _sample.vel - velocity_offset_earth; + _measurement_var = matrix::SquareMatrix3f(rotation_ev_to_ekf * matrix::diag( + _sample.velocity_var) * rotation_ev_to_ekf.transpose()).diag(); + _min_variance = math::max(_min_variance, _sample.orientation_var.max()); + } + + enforceMinimumVariance(); + } +}; + +#endif // ExternalVisionVel_H 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 b94f24f45e..abef15db33 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,13 +37,13 @@ */ #include "ekf.h" -#include -#include -#include +#include "aid_sources/external_vision/ev_vel.h" +#include "ekf_derivation/generated/compute_body_vel_innov_var_h.h" +#include "ekf_derivation/generated/compute_body_vel_y_innov_var.h" +#include "ekf_derivation/generated/compute_body_vel_z_innov_var.h" -void Ekf::controlEvVelFusion(const imuSample &imu_sample, 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 Ekf::controlEvVelFusion(ExternalVisionVel &ev, const bool common_starting_conditions_passing, const bool ev_reset, + const bool quality_sufficient, estimator_aid_source3d_s &aid_src) { static constexpr const char *AID_SRC_NAME = "EV velocity"; @@ -53,110 +53,12 @@ void Ekf::controlEvVelFusion(const imuSample &imu_sample, const extVisionSample // determine if we should use EV velocity aiding bool continuing_conditions_passing = (_params.ev_ctrl & static_cast(EvCtrl::VEL)) && _control_status.flags.tilt_align - && ev_sample.vel.isAllFinite(); - - // correct velocity for offset relative to IMU - const Vector3f angular_velocity = imu_sample.delta_ang / imu_sample.delta_ang_dt - _state.gyro_bias; - const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; - const Vector3f vel_offset_body = angular_velocity % pos_offset_body; - const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; - - // rotate measurement into correct earth frame if required - 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) { - measurement = ev_sample.vel - vel_offset_earth; - measurement_var = ev_sample.velocity_var; - - } else { - continuing_conditions_passing = false; - } - - break; - - case VelocityFrame::LOCAL_FRAME_FRD: - if (_control_status.flags.ev_yaw) { - // using EV frame - 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()); - - 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: { - - // 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; - break; - } - -#if defined(CONFIG_EKF2_GNSS) - - // increase minimum variance if GPS active (position reference) - if (_control_status.flags.gps) { - for (int i = 0; i < 2; i++) { - measurement_var(i) = math::max(measurement_var(i), sq(_params.gps_vel_noise)); - } - } - -#endif // CONFIG_EKF2_GNSS - - 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(); - - 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); - - } 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 - } + && ev._sample.vel.isAllFinite(); - const bool starting_conditions_passing = common_starting_conditions_passing - && continuing_conditions_passing - && ((Vector3f(aid_src.test_ratio).max() < 0.1f) || !isHorizontalAidingActive()); + continuing_conditions_passing &= ev._measurement.isAllFinite() && ev._measurement_var.isAllFinite(); + + float gate = math::max(_params.ev_vel_innov_gate, 1.f); if (_control_status.flags.ev_vel) { if (continuing_conditions_passing) { @@ -164,7 +66,7 @@ void Ekf::controlEvVelFusion(const imuSample &imu_sample, const extVisionSample if (quality_sufficient) { ECL_INFO("reset to %s", AID_SRC_NAME); _information_events.flags.reset_vel_to_vision = true; - resetVelocityToEV(measurement, measurement_var, ev_sample.vel_frame); + ev.resetVelocity(); resetAidSourceStatusZeroInnovation(aid_src); } else { @@ -175,7 +77,7 @@ void Ekf::controlEvVelFusion(const imuSample &imu_sample, const extVisionSample } } else if (quality_sufficient) { - fuseEvVelocity(aid_src, ev_sample); + ev.fuseVelocity(aid_src, gate); } else { aid_src.innovation_rejected = true; @@ -186,10 +88,9 @@ void Ekf::controlEvVelFusion(const imuSample &imu_sample, const extVisionSample if (is_fusion_failing) { if ((_nb_ev_vel_reset_available > 0) && quality_sufficient) { - // Data seems good, attempt a reset _information_events.flags.reset_vel_to_vision = true; ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME); - resetVelocityToEV(measurement, measurement_var, ev_sample.vel_frame); + ev.resetVelocity(); resetAidSourceStatusZeroInnovation(aid_src); if (_control_status.flags.in_air) { @@ -197,32 +98,14 @@ void Ekf::controlEvVelFusion(const imuSample &imu_sample, const extVisionSample } } else { - // 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); + ECL_WARN("stopping %s, fusion 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(); } } else if (isHeightResetRequired()) { - // reset vertical velocity if height is failing - if (ev_sample.vel_frame == VelocityFrame::BODY_FRAME_FRD) { - const Vector3f measurement_ekf_frame = _R_to_earth * measurement; - const Vector3f measurement_var_ekf_frame = rotateVarianceToEkf(measurement_var); - resetVerticalVelocityTo(measurement_ekf_frame(2), measurement_var_ekf_frame(2)); - - } else { - resetVerticalVelocityTo(measurement(2), measurement_var(2)); - } + ev.resetVerticalVelocity(); } } else { @@ -232,18 +115,23 @@ void Ekf::controlEvVelFusion(const imuSample &imu_sample, const extVisionSample } } else { - if (starting_conditions_passing) { + + if (common_starting_conditions_passing && continuing_conditions_passing) { + // make starting condition more sensitive if horizontal aiding is active + gate = isHorizontalAidingActive() ? (float)sqrt(10.f) * gate : gate; + // activate fusion, only reset if necessary 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)); + (double)ev._measurement(0), (double)ev._measurement(1), + (double)ev._measurement(2)); _information_events.flags.reset_vel_to_vision = true; - resetVelocityToEV(measurement, measurement_var, ev_sample.vel_frame); + ev.resetVelocity(); resetAidSourceStatusZeroInnovation(aid_src); _control_status.flags.ev_vel = true; - } else if (fuseEvVelocity(aid_src, ev_sample)) { + } else if (ev.fuseVelocity(aid_src, gate)) { ECL_INFO("starting %s fusion", AID_SRC_NAME); _control_status.flags.ev_vel = true; } @@ -256,92 +144,65 @@ void Ekf::controlEvVelFusion(const imuSample &imu_sample, const extVisionSample } } -bool Ekf::fuseEvVelocity(estimator_aid_source3d_s &aid_src, const extVisionSample &ev_sample) +void Ekf::stopEvVelFusion() { - if (ev_sample.vel_frame == VelocityFrame::BODY_FRAME_FRD) { + if (_control_status.flags.ev_vel) { + _control_status.flags.ev_vel = false; + } +} - VectorState H; - estimator_aid_source1d_s current_aid_src; - const auto state_vector = _state.vector(); +void Ekf::fuseLocalFrameVelocity(estimator_aid_source3d_s &aid_src, const uint64_t ×tamp, + const Vector3f &measurement, const Vector3f &measurement_var, const float &innovation_gate) +{ + updateAidSourceStatus(aid_src, + timestamp, // sample timestamp + measurement, // observation + measurement_var, // observation variance + _state.vel - measurement, // innovation + getVelocityVariance() + measurement_var, // innovation variance + innovation_gate); // innovation gate + fuseVelocity(aid_src); +} + +void Ekf::fuseBodyFrameVelocity(estimator_aid_source3d_s &aid_src, const uint64_t ×tamp, + const Vector3f &measurement, const Vector3f &measurement_var, const float &innovation_gate) +{ + VectorState H[3]; + Vector3f innov_var; + Vector3f innov = _R_to_earth.transpose() * _state.vel - measurement; + const auto state_vector = _state.vector(); + sym::ComputeBodyVelInnovVarH(state_vector, P, measurement_var, &innov_var, &H[0], &H[1], &H[2]); + + updateAidSourceStatus(aid_src, + timestamp, // sample timestamp + measurement, // observation + measurement_var, // observation variance + innov, // innovation + innov_var, // innovation variance + innovation_gate); // innovation gate + + if (!aid_src.innovation_rejected) { + aid_src.fused = true; 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; - } + if (index == 1) { + sym::ComputeBodyVelYInnovVar(state_vector, P, measurement_var(index), &aid_src.innovation_variance[index]); + } else if (index == 2) { + sym::ComputeBodyVelZInnovVar(state_vector, P, measurement_var(index), &aid_src.innovation_variance[index]); + } + + aid_src.innovation[index] = Vector3f(_R_to_earth.transpose().row(index)) * _state.vel - measurement(index); + VectorState Kfusion = P * H[index] / aid_src.innovation_variance[index]; + aid_src.fused &= measurementUpdate(Kfusion, H[index], aid_src.observation_variance[index], aid_src.innovation[index]); + } } if (aid_src.fused) { _time_last_hor_vel_fuse = _time_delayed_us; _time_last_ver_vel_fuse = _time_delayed_us; + 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) { - - _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 7fa1f2992e..6eff399de7 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -67,6 +67,7 @@ #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION enum class Likelihood { LOW, MEDIUM, HIGH }; +class ExternalVisionVel; class Ekf final : public EstimatorInterface { @@ -422,6 +423,11 @@ public: private: + friend class ExternalVisionVel; + friend class EvVelBodyFrameFrd; + friend class EvVelLocalFrameNed; + friend class EvVelLocalFrameFrd; + // set the internal states and status to their default value void reset(); @@ -807,14 +813,15 @@ private: void controlEvPosFusion(const imuSample &imu_sample, 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 imuSample &imu_sample, 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 controlEvVelFusion(ExternalVisionVel &ev, const bool common_starting_conditions_passing, const bool ev_reset, + const bool quality_sufficient, estimator_aid_source3d_s &aid_src); void controlEvYawFusion(const imuSample &imu_sample, 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 fuseLocalFrameVelocity(estimator_aid_source3d_s &aid_src, const uint64_t ×tamp, const Vector3f &measurement, + const Vector3f &measurement_var, const float &innovation_gate); + void fuseBodyFrameVelocity(estimator_aid_source3d_s &aid_src, const uint64_t ×tamp, const Vector3f &measurement, + const Vector3f &measurement_var, const float &innovation_gate); 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, diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index df08b048ee..36e2f34373 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -383,32 +383,45 @@ def predict_vel_body( R_to_body = state["quat_nominal"].inverse() return R_to_body * vel -def compute_ev_body_vel_hx( +def compute_body_vel_innov_var_h( state: VState, -) -> (VTangent): - + P: MTangent, + R: sf.V3, +) -> (sf.V3, VTangent, VTangent, VTangent): state = vstate_to_state(state) meas_pred = predict_vel_body(state) Hx = jacobian_chain_rule(meas_pred[0], state) - return (Hx.T) + Hy = jacobian_chain_rule(meas_pred[1], state) + Hz = jacobian_chain_rule(meas_pred[2], state) + innov_var = sf.V3((Hx * P * Hx.T + R[0])[0,0], + (Hy * P * Hy.T + R[1])[0,0], + (Hz * P * Hz.T + R[2])[0,0]) -def compute_ev_body_vel_hy( + return (innov_var, Hx.T, Hy.T, Hz.T) + +def compute_body_vel_y_innov_var( state: VState, -) -> (VTangent): - + P: MTangent, + R: sf.Scalar +) -> (sf.Scalar): state = vstate_to_state(state) - meas_pred = predict_vel_body(state)[1] - Hy = jacobian_chain_rule(meas_pred, state) - return (Hy.T) + meas_pred = predict_vel_body(state) + Hy = jacobian_chain_rule(meas_pred[1], state) + innov_var = (Hy * P * Hy.T + R)[0,0] -def compute_ev_body_vel_hz( + return (innov_var) + +def compute_body_vel_z_innov_var( state: VState, -) -> (VTangent): - + P: MTangent, + R: sf.Scalar +) -> (sf.Scalar): state = vstate_to_state(state) - meas_pred = predict_vel_body(state)[2] - Hz = jacobian_chain_rule(meas_pred, state) - return (Hz.T) + meas_pred = predict_vel_body(state) + Hz = jacobian_chain_rule(meas_pred[2], state) + innov_var = (Hz * P * Hz.T + R)[0,0] + + return (innov_var) def predict_mag_body(state) -> sf.V3: mag_field_earth = state["mag_I"] @@ -746,8 +759,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_function(compute_body_vel_innov_var_h, output_names=["innov_var", "Hx", "Hy", "Hz"]) +generate_px4_function(compute_body_vel_y_innov_var, output_names=["innov_var"]) +generate_px4_function(compute_body_vel_z_innov_var, output_names=["innov_var"]) generate_px4_state(State, tangent_idx) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_body_vel_innov_var_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_body_vel_innov_var_h.h new file mode 100644 index 0000000000..66c6051549 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_body_vel_innov_var_h.h @@ -0,0 +1,192 @@ +// ----------------------------------------------------------------------------- +// 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_body_vel_innov_var_h + * + * Args: + * state: Matrix25_1 + * P: Matrix24_24 + * R: Matrix31 + * + * Outputs: + * innov_var: Matrix31 + * Hx: Matrix24_1 + * Hy: Matrix24_1 + * Hz: Matrix24_1 + */ +template +void ComputeBodyVelInnovVarH(const matrix::Matrix& state, + const matrix::Matrix& P, + const matrix::Matrix& R, + matrix::Matrix* const innov_var = nullptr, + matrix::Matrix* const Hx = nullptr, + matrix::Matrix* const Hy = nullptr, + matrix::Matrix* const Hz = nullptr) { + // Total ops: 371 + + // Input arrays + + // Intermediate terms (63) + const Scalar _tmp0 = 2 * state(5, 0); + const Scalar _tmp1 = _tmp0 * state(3, 0); + const Scalar _tmp2 = 2 * state(6, 0); + const Scalar _tmp3 = _tmp2 * state(2, 0); + const Scalar _tmp4 = _tmp1 - _tmp3; + const Scalar _tmp5 = (Scalar(1) / Scalar(2)) * state(2, 0); + const Scalar _tmp6 = _tmp0 * state(2, 0); + const Scalar _tmp7 = _tmp2 * state(3, 0); + const Scalar _tmp8 = _tmp6 + _tmp7; + const Scalar _tmp9 = (Scalar(1) / Scalar(2)) * state(3, 0); + const Scalar _tmp10 = 4 * state(4, 0); + const Scalar _tmp11 = _tmp0 * state(0, 0); + const Scalar _tmp12 = 2 * state(1, 0); + const Scalar _tmp13 = _tmp12 * state(6, 0); + const Scalar _tmp14 = -_tmp10 * state(3, 0) + _tmp11 + _tmp13; + const Scalar _tmp15 = (Scalar(1) / Scalar(2)) * state(1, 0); + const Scalar _tmp16 = _tmp0 * state(1, 0); + const Scalar _tmp17 = _tmp2 * state(0, 0); + const Scalar _tmp18 = -_tmp10 * state(2, 0) + _tmp16 - _tmp17; + const Scalar _tmp19 = (Scalar(1) / Scalar(2)) * state(0, 0); + const Scalar _tmp20 = -_tmp14 * _tmp15 + _tmp18 * _tmp19 - _tmp4 * _tmp5 + _tmp8 * _tmp9; + const Scalar _tmp21 = 2 * state(2, 0); + const Scalar _tmp22 = _tmp21 * state(0, 0); + const Scalar _tmp23 = _tmp12 * state(3, 0); + const Scalar _tmp24 = -_tmp22 + _tmp23; + const Scalar _tmp25 = 2 * state(0, 0) * state(3, 0); + const Scalar _tmp26 = _tmp12 * state(2, 0); + const Scalar _tmp27 = _tmp25 + _tmp26; + const Scalar _tmp28 = -2 * std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp29 = 1 - 2 * std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp30 = _tmp28 + _tmp29; + const Scalar _tmp31 = _tmp14 * _tmp19 + _tmp15 * _tmp18 - _tmp4 * _tmp9 - _tmp5 * _tmp8; + const Scalar _tmp32 = _tmp14 * _tmp5 - _tmp15 * _tmp4 - _tmp18 * _tmp9 + _tmp19 * _tmp8; + const Scalar _tmp33 = -_tmp25 + _tmp26; + const Scalar _tmp34 = _tmp21 * state(3, 0); + const Scalar _tmp35 = _tmp12 * state(0, 0); + const Scalar _tmp36 = _tmp34 + _tmp35; + const Scalar _tmp37 = -2 * std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp38 = _tmp28 + _tmp37 + 1; + const Scalar _tmp39 = 2 * state(4, 0); + const Scalar _tmp40 = _tmp39 * state(3, 0); + const Scalar _tmp41 = _tmp13 - _tmp40; + const Scalar _tmp42 = _tmp39 * state(1, 0); + const Scalar _tmp43 = _tmp42 + _tmp7; + const Scalar _tmp44 = _tmp39 * state(0, 0); + const Scalar _tmp45 = 4 * state(5, 0); + const Scalar _tmp46 = _tmp3 - _tmp44 - _tmp45 * state(3, 0); + const Scalar _tmp47 = _tmp39 * state(2, 0); + const Scalar _tmp48 = _tmp17 - _tmp45 * state(1, 0) + _tmp47; + const Scalar _tmp49 = _tmp15 * _tmp43 + _tmp19 * _tmp46 - _tmp41 * _tmp9 - _tmp48 * _tmp5; + const Scalar _tmp50 = -_tmp15 * _tmp46 + _tmp19 * _tmp43 - _tmp41 * _tmp5 + _tmp48 * _tmp9; + const Scalar _tmp51 = -_tmp15 * _tmp41 + _tmp19 * _tmp48 - _tmp43 * _tmp9 + _tmp46 * _tmp5; + const Scalar _tmp52 = _tmp42 + _tmp6; + const Scalar _tmp53 = -_tmp16 + _tmp47; + const Scalar _tmp54 = 4 * state(6, 0); + const Scalar _tmp55 = _tmp1 + _tmp44 - _tmp54 * state(2, 0); + const Scalar _tmp56 = -_tmp11 + _tmp40 - _tmp54 * state(1, 0); + const Scalar _tmp57 = -_tmp15 * _tmp52 + _tmp19 * _tmp55 - _tmp5 * _tmp53 + _tmp56 * _tmp9; + const Scalar _tmp58 = _tmp34 - _tmp35; + const Scalar _tmp59 = _tmp22 + _tmp23; + const Scalar _tmp60 = _tmp29 + _tmp37; + const Scalar _tmp61 = _tmp15 * _tmp55 + _tmp19 * _tmp52 - _tmp5 * _tmp56 - _tmp53 * _tmp9; + const Scalar _tmp62 = -_tmp15 * _tmp53 + _tmp19 * _tmp56 + _tmp5 * _tmp52 - _tmp55 * _tmp9; + + // Output terms (4) + if (innov_var != nullptr) { + matrix::Matrix& _innov_var = (*innov_var); + + _innov_var(0, 0) = R(0, 0) + + _tmp20 * (P(0, 1) * _tmp32 + P(1, 1) * _tmp20 + P(2, 1) * _tmp31 + + P(3, 1) * _tmp30 + P(4, 1) * _tmp27 + P(5, 1) * _tmp24) + + _tmp24 * (P(0, 5) * _tmp32 + P(1, 5) * _tmp20 + P(2, 5) * _tmp31 + + P(3, 5) * _tmp30 + P(4, 5) * _tmp27 + P(5, 5) * _tmp24) + + _tmp27 * (P(0, 4) * _tmp32 + P(1, 4) * _tmp20 + P(2, 4) * _tmp31 + + P(3, 4) * _tmp30 + P(4, 4) * _tmp27 + P(5, 4) * _tmp24) + + _tmp30 * (P(0, 3) * _tmp32 + P(1, 3) * _tmp20 + P(2, 3) * _tmp31 + + P(3, 3) * _tmp30 + P(4, 3) * _tmp27 + P(5, 3) * _tmp24) + + _tmp31 * (P(0, 2) * _tmp32 + P(1, 2) * _tmp20 + P(2, 2) * _tmp31 + + P(3, 2) * _tmp30 + P(4, 2) * _tmp27 + P(5, 2) * _tmp24) + + _tmp32 * (P(0, 0) * _tmp32 + P(1, 0) * _tmp20 + P(2, 0) * _tmp31 + + P(3, 0) * _tmp30 + P(4, 0) * _tmp27 + P(5, 0) * _tmp24); + _innov_var(1, 0) = R(1, 0) + + _tmp33 * (P(0, 3) * _tmp51 + P(1, 3) * _tmp50 + P(2, 3) * _tmp49 + + P(3, 3) * _tmp33 + P(4, 3) * _tmp38 + P(5, 3) * _tmp36) + + _tmp36 * (P(0, 5) * _tmp51 + P(1, 5) * _tmp50 + P(2, 5) * _tmp49 + + P(3, 5) * _tmp33 + P(4, 5) * _tmp38 + P(5, 5) * _tmp36) + + _tmp38 * (P(0, 4) * _tmp51 + P(1, 4) * _tmp50 + P(2, 4) * _tmp49 + + P(3, 4) * _tmp33 + P(4, 4) * _tmp38 + P(5, 4) * _tmp36) + + _tmp49 * (P(0, 2) * _tmp51 + P(1, 2) * _tmp50 + P(2, 2) * _tmp49 + + P(3, 2) * _tmp33 + P(4, 2) * _tmp38 + P(5, 2) * _tmp36) + + _tmp50 * (P(0, 1) * _tmp51 + P(1, 1) * _tmp50 + P(2, 1) * _tmp49 + + P(3, 1) * _tmp33 + P(4, 1) * _tmp38 + P(5, 1) * _tmp36) + + _tmp51 * (P(0, 0) * _tmp51 + P(1, 0) * _tmp50 + P(2, 0) * _tmp49 + + P(3, 0) * _tmp33 + P(4, 0) * _tmp38 + P(5, 0) * _tmp36); + _innov_var(2, 0) = R(2, 0) + + _tmp57 * (P(0, 1) * _tmp62 + P(1, 1) * _tmp57 + P(2, 1) * _tmp61 + + P(3, 1) * _tmp59 + P(4, 1) * _tmp58 + P(5, 1) * _tmp60) + + _tmp58 * (P(0, 4) * _tmp62 + P(1, 4) * _tmp57 + P(2, 4) * _tmp61 + + P(3, 4) * _tmp59 + P(4, 4) * _tmp58 + P(5, 4) * _tmp60) + + _tmp59 * (P(0, 3) * _tmp62 + P(1, 3) * _tmp57 + P(2, 3) * _tmp61 + + P(3, 3) * _tmp59 + P(4, 3) * _tmp58 + P(5, 3) * _tmp60) + + _tmp60 * (P(0, 5) * _tmp62 + P(1, 5) * _tmp57 + P(2, 5) * _tmp61 + + P(3, 5) * _tmp59 + P(4, 5) * _tmp58 + P(5, 5) * _tmp60) + + _tmp61 * (P(0, 2) * _tmp62 + P(1, 2) * _tmp57 + P(2, 2) * _tmp61 + + P(3, 2) * _tmp59 + P(4, 2) * _tmp58 + P(5, 2) * _tmp60) + + _tmp62 * (P(0, 0) * _tmp62 + P(1, 0) * _tmp57 + P(2, 0) * _tmp61 + + P(3, 0) * _tmp59 + P(4, 0) * _tmp58 + P(5, 0) * _tmp60); + } + + if (Hx != nullptr) { + matrix::Matrix& _hx = (*Hx); + + _hx.setZero(); + + _hx(0, 0) = _tmp32; + _hx(1, 0) = _tmp20; + _hx(2, 0) = _tmp31; + _hx(3, 0) = _tmp30; + _hx(4, 0) = _tmp27; + _hx(5, 0) = _tmp24; + } + + if (Hy != nullptr) { + matrix::Matrix& _hy = (*Hy); + + _hy.setZero(); + + _hy(0, 0) = _tmp51; + _hy(1, 0) = _tmp50; + _hy(2, 0) = _tmp49; + _hy(3, 0) = _tmp33; + _hy(4, 0) = _tmp38; + _hy(5, 0) = _tmp36; + } + + if (Hz != nullptr) { + matrix::Matrix& _hz = (*Hz); + + _hz.setZero(); + + _hz(0, 0) = _tmp62; + _hz(1, 0) = _tmp57; + _hz(2, 0) = _tmp61; + _hz(3, 0) = _tmp59; + _hz(4, 0) = _tmp58; + _hz(5, 0) = _tmp60; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_body_vel_y_innov_var.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_body_vel_y_innov_var.h new file mode 100644 index 0000000000..4fbcfb5993 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_body_vel_y_innov_var.h @@ -0,0 +1,81 @@ +// ----------------------------------------------------------------------------- +// 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_body_vel_y_innov_var + * + * Args: + * state: Matrix25_1 + * P: Matrix24_24 + * R: Scalar + * + * Outputs: + * innov_var: Scalar + */ +template +void ComputeBodyVelYInnovVar(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar R, + Scalar* const innov_var = nullptr) { + // Total ops: 138 + + // Input arrays + + // Intermediate terms (19) + const Scalar _tmp0 = 2 * state(3, 0); + const Scalar _tmp1 = 2 * state(1, 0); + const Scalar _tmp2 = -_tmp0 * state(0, 0) + _tmp1 * state(2, 0); + const Scalar _tmp3 = _tmp0 * state(2, 0) + _tmp1 * state(0, 0); + const Scalar _tmp4 = + -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1; + const Scalar _tmp5 = 2 * state(4, 0); + const Scalar _tmp6 = _tmp1 * state(6, 0) - _tmp5 * state(3, 0); + const Scalar _tmp7 = (Scalar(1) / Scalar(2)) * _tmp6; + const Scalar _tmp8 = + (Scalar(1) / Scalar(2)) * _tmp0 * state(6, 0) + (Scalar(1) / Scalar(2)) * _tmp1 * state(4, 0); + const Scalar _tmp9 = 4 * state(5, 0); + const Scalar _tmp10 = 2 * state(6, 0); + const Scalar _tmp11 = _tmp10 * state(2, 0) - _tmp5 * state(0, 0) - _tmp9 * state(3, 0); + const Scalar _tmp12 = (Scalar(1) / Scalar(2)) * _tmp11; + const Scalar _tmp13 = _tmp10 * state(0, 0) + _tmp5 * state(2, 0) - _tmp9 * state(1, 0); + const Scalar _tmp14 = (Scalar(1) / Scalar(2)) * state(2, 0); + const Scalar _tmp15 = + _tmp12 * state(0, 0) - _tmp13 * _tmp14 - _tmp7 * state(3, 0) + _tmp8 * state(1, 0); + const Scalar _tmp16 = (Scalar(1) / Scalar(2)) * _tmp13; + const Scalar _tmp17 = + -_tmp12 * state(1, 0) - _tmp14 * _tmp6 + _tmp16 * state(3, 0) + _tmp8 * state(0, 0); + const Scalar _tmp18 = + _tmp11 * _tmp14 + _tmp16 * state(0, 0) - _tmp7 * state(1, 0) - _tmp8 * state(3, 0); + + // Output terms (1) + if (innov_var != nullptr) { + Scalar& _innov_var = (*innov_var); + + _innov_var = R + + _tmp15 * (P(0, 2) * _tmp18 + P(1, 2) * _tmp17 + P(2, 2) * _tmp15 + + P(3, 2) * _tmp2 + P(4, 2) * _tmp4 + P(5, 2) * _tmp3) + + _tmp17 * (P(0, 1) * _tmp18 + P(1, 1) * _tmp17 + P(2, 1) * _tmp15 + + P(3, 1) * _tmp2 + P(4, 1) * _tmp4 + P(5, 1) * _tmp3) + + _tmp18 * (P(0, 0) * _tmp18 + P(1, 0) * _tmp17 + P(2, 0) * _tmp15 + + P(3, 0) * _tmp2 + P(4, 0) * _tmp4 + P(5, 0) * _tmp3) + + _tmp2 * (P(0, 3) * _tmp18 + P(1, 3) * _tmp17 + P(2, 3) * _tmp15 + P(3, 3) * _tmp2 + + P(4, 3) * _tmp4 + P(5, 3) * _tmp3) + + _tmp3 * (P(0, 5) * _tmp18 + P(1, 5) * _tmp17 + P(2, 5) * _tmp15 + P(3, 5) * _tmp2 + + P(4, 5) * _tmp4 + P(5, 5) * _tmp3) + + _tmp4 * (P(0, 4) * _tmp18 + P(1, 4) * _tmp17 + P(2, 4) * _tmp15 + P(3, 4) * _tmp2 + + P(4, 4) * _tmp4 + P(5, 4) * _tmp3); + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_body_vel_z_innov_var.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_body_vel_z_innov_var.h new file mode 100644 index 0000000000..c4d8e378c8 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_body_vel_z_innov_var.h @@ -0,0 +1,82 @@ +// ----------------------------------------------------------------------------- +// 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_body_vel_z_innov_var + * + * Args: + * state: Matrix25_1 + * P: Matrix24_24 + * R: Scalar + * + * Outputs: + * innov_var: Scalar + */ +template +void ComputeBodyVelZInnovVar(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar R, + Scalar* const innov_var = nullptr) { + // Total ops: 142 + + // Input arrays + + // Intermediate terms (15) + 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 = + (Scalar(1) / Scalar(2)) * _tmp0 * state(2, 0) - Scalar(1) / Scalar(2) * _tmp1 * state(1, 0); + const Scalar _tmp4 = 2 * state(0, 0); + const Scalar _tmp5 = 4 * state(6, 0); + const Scalar _tmp6 = (Scalar(1) / Scalar(2)) * _tmp1 * state(3, 0) + + (Scalar(1) / Scalar(2)) * _tmp4 * state(4, 0) - + Scalar(1) / Scalar(2) * _tmp5 * state(2, 0); + const Scalar _tmp7 = (Scalar(1) / Scalar(2)) * _tmp0 * state(3, 0) - + Scalar(1) / Scalar(2) * _tmp4 * state(5, 0) - + Scalar(1) / Scalar(2) * _tmp5 * state(1, 0); + const Scalar _tmp8 = + -_tmp2 * state(1, 0) - _tmp3 * state(2, 0) + _tmp6 * state(0, 0) + _tmp7 * state(3, 0); + const Scalar _tmp9 = 2 * state(3, 0); + const Scalar _tmp10 = -_tmp4 * state(1, 0) + _tmp9 * state(2, 0); + const Scalar _tmp11 = _tmp4 * state(2, 0) + _tmp9 * state(1, 0); + const Scalar _tmp12 = + -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(2, 0), Scalar(2)) + 1; + const Scalar _tmp13 = + _tmp2 * state(0, 0) - _tmp3 * state(3, 0) + _tmp6 * state(1, 0) - _tmp7 * state(2, 0); + const Scalar _tmp14 = + _tmp2 * state(2, 0) - _tmp3 * state(1, 0) - _tmp6 * state(3, 0) + _tmp7 * state(0, 0); + + // Output terms (1) + if (innov_var != nullptr) { + Scalar& _innov_var = (*innov_var); + + _innov_var = R + + _tmp10 * (P(0, 4) * _tmp14 + P(1, 4) * _tmp8 + P(2, 4) * _tmp13 + + P(3, 4) * _tmp11 + P(4, 4) * _tmp10 + P(5, 4) * _tmp12) + + _tmp11 * (P(0, 3) * _tmp14 + P(1, 3) * _tmp8 + P(2, 3) * _tmp13 + + P(3, 3) * _tmp11 + P(4, 3) * _tmp10 + P(5, 3) * _tmp12) + + _tmp12 * (P(0, 5) * _tmp14 + P(1, 5) * _tmp8 + P(2, 5) * _tmp13 + + P(3, 5) * _tmp11 + P(4, 5) * _tmp10 + P(5, 5) * _tmp12) + + _tmp13 * (P(0, 2) * _tmp14 + P(1, 2) * _tmp8 + P(2, 2) * _tmp13 + + P(3, 2) * _tmp11 + P(4, 2) * _tmp10 + P(5, 2) * _tmp12) + + _tmp14 * (P(0, 0) * _tmp14 + P(1, 0) * _tmp8 + P(2, 0) * _tmp13 + + P(3, 0) * _tmp11 + P(4, 0) * _tmp10 + P(5, 0) * _tmp12) + + _tmp8 * (P(0, 1) * _tmp14 + P(1, 1) * _tmp8 + P(2, 1) * _tmp13 + P(3, 1) * _tmp11 + + P(4, 1) * _tmp10 + P(5, 1) * _tmp12); + } +} // 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_hx.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hx.h deleted file mode 100644 index 72a2adf0f6..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hx.h +++ /dev/null @@ -1,63 +0,0 @@ -// ----------------------------------------------------------------------------- -// 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 deleted file mode 100644 index a4dd6f94d9..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hy.h +++ /dev/null @@ -1,67 +0,0 @@ -// ----------------------------------------------------------------------------- -// 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 deleted file mode 100644 index 395bb85b4a..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hz.h +++ /dev/null @@ -1,63 +0,0 @@ -// ----------------------------------------------------------------------------- -// 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