diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 7dd7ce481a..0550468173 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -15,6 +15,7 @@ CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_DIFFERENTIAL_DRIVE=y CONFIG_MODULES_EKF2=y CONFIG_EKF2_VERBOSE_STATUS=y +CONFIG_EKF2_AIRSPEED2D=y CONFIG_EKF2_AUX_GLOBAL_POSITION=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y diff --git a/msg/EstimatorAidSource2d.msg b/msg/EstimatorAidSource2d.msg index 98e645a3ec..77b093e6fb 100644 --- a/msg/EstimatorAidSource2d.msg +++ b/msg/EstimatorAidSource2d.msg @@ -19,4 +19,4 @@ bool fused # true if the sample was successfully fused # TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos estimator_aid_src_aux_global_position # TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow estimator_aid_src_terrain_optical_flow -# TOPICS estimator_aid_src_drag +# TOPICS estimator_aid_src_airspeed2d estimator_aid_src_drag diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 3f1f9a4c99..d875b6e3db 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -140,6 +140,10 @@ if(CONFIG_EKF2_AIRSPEED) list(APPEND EKF_SRCS EKF/airspeed_fusion.cpp) endif() +if(CONFIG_EKF2_AIRSPEED2D) + list(APPEND EKF_SRCS EKF/aid_sources/Airspeed2D.cpp) +endif() + if(CONFIG_EKF2_AUX_GLOBAL_POSITION) list(APPEND EKF_SRCS EKF/aux_global_position.cpp) endif() diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index adf922907a..10dd2ca5bf 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -57,6 +57,10 @@ if(CONFIG_EKF2_AIRSPEED) list(APPEND EKF_SRCS airspeed_fusion.cpp) endif() +if(CONFIG_EKF2_AIRSPEED2D) + list(APPEND EKF_SRCS aid_sources/Airspeed2D.cpp) +endif() + if(CONFIG_EKF2_AUX_GLOBAL_POSITION) list(APPEND EKF_SRCS aux_global_position.cpp) endif() diff --git a/src/modules/ekf2/EKF/aid_sources/Airspeed2D.cpp b/src/modules/ekf2/EKF/aid_sources/Airspeed2D.cpp new file mode 100644 index 0000000000..261ac738b4 --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/Airspeed2D.cpp @@ -0,0 +1,188 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include "Airspeed2D.hpp" + +#include "../ekf.h" + +#include +#include + +bool Airspeed2D::update(Ekf &ekf, const estimator::imuSample &imu_delayed) +{ + +#if defined(MODULE_NAME) + + if (_sensor_airflow_sub.updated()) { + sensor_airflow_s sensor_airflow; + + if (_sensor_airflow_sub.copy(&sensor_airflow)) { + + AirflowSample sample{}; + sample.time_us = sensor_airflow.timestamp; + sample.speed = sensor_airflow.speed; + sample.direction_rad = sensor_airflow.direction; + + _airflow_buffer.push(sample); + _time_last_buffer_push = imu_delayed.time_us; + } + } + +#endif // MODULE_NAME + + + + AirflowSample sample; + + if (_airflow_buffer.pop_first_older_than(imu_delayed.time_us, &sample)) { + + + + + //ECL_INFO("%lu: %lu sensor wind fusion", _time_delayed_us, sample_delayed.time_us); + + ekf.resetEstimatorAidStatus(_aid_src); + + _aid_src.timestamp_sample = sample.time_us; + + // body frame wind speed and direction + const Vector2f measurement{sample.speed * cosf(sample.direction_rad), sample.speed * sinf(sample.direction_rad)}; + + float wind_noise = 4.f; // m/s + const float R = sq(wind_noise); + + const auto state_vector = ekf.state().vector(); + const auto &P = ekf.covariances(); + + Vector2f innov{}; + Vector2f innov_var{}; + Ekf::VectorState H; + + sym::ComputeAirspeed2DInnovInnovVarAndHx(state_vector, P, measurement, R, FLT_EPSILON, + &innov, &innov_var, &H); + + _aid_src.observation[0] = measurement(0); + _aid_src.observation[1] = measurement(1); + _aid_src.innovation[0] = innov(0); + _aid_src.innovation[1] = innov(1); + + _aid_src.observation_variance[0] = R; + _aid_src.observation_variance[1] = R; + _aid_src.innovation_variance[0] = innov_var(0); + _aid_src.innovation_variance[1] = innov_var(1); + + float innov_gate = 3.f; // innovation gate + ekf.setEstimatorAidStatusTestRatio(_aid_src, innov_gate); + + bool conditions_passing = ekf.control_status_flags().wind; + + bool update_all_states = ekf.control_status_flags().wind && ekf.control_status_flags().in_air + && (_aid_src.test_ratio[0] < 0.1f) && (_aid_src.test_ratio[1] < 0.1f) + && !ekf.getWindVelocityVariance().longerThan(0.1f * R); + + // const bool update_wind_only = !ekf.control_status_flags().wind_dead_reckoning; + + bool fused[2] {false, false}; + + if (ekf.control_status_flags().wind + && conditions_passing + && (_aid_src.test_ratio[0] < 1.f) + && (_aid_src.test_ratio[1] < 1.f) + ) { + // fuse x + { + // H computed above + Ekf::VectorState Kfusion = P * H / innov_var(0); + + if (!update_all_states) { + for (unsigned row = 0; row <= 21; row++) { + Kfusion(row) = 0.f; + } + } + + fused[0] = ekf.measurementUpdate(Kfusion, innov_var(0), innov(0)); + } + + // fuse y + { + // recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes) + sym::ComputeAirspeed2DYInnovInnovVarAndHy(state_vector, P, measurement, R, FLT_EPSILON, + &innov(1), &innov_var(1), &H); + + Ekf::VectorState Kfusion = P * H / innov_var(1); + + if (!update_all_states) { + for (unsigned row = 0; row <= 21; row++) { + Kfusion(row) = 0.f; + } + } + + fused[1] = ekf.measurementUpdate(Kfusion, innov_var(1), innov(1)); + } + } + + if (fused[0] && fused[1]) { + _aid_src.time_last_fuse = imu_delayed.time_us; + _aid_src.fused = true; + } + + + if (!ekf.control_status_flags().wind && ekf.local_position_is_valid()) { + + Vector3f airflow_bf{measurement(0), measurement(1), 0.f}; + Vector3f offset_body{0.f, 0.f, 0.f}; + + const Dcmf R_to_earth{ekf.state().quat_nominal}; + + Vector3f airflow_ned = (R_to_earth * (airflow_bf - offset_body)); + + Vector2f wind_vel{ + ekf.state().vel(0) - airflow_ned(0), + ekf.state().vel(1) - airflow_ned(1) + }; + + ekf.resetWind(wind_vel, R); + + _aid_src.time_last_fuse = imu_delayed.time_us; + } + +#if defined(MODULE_NAME) + _aid_src.timestamp = hrt_absolute_time(); + _estimator_aid_src_airspeed2d_pub.publish(_aid_src); +#endif // MODULE_NAME + + return true; + } + + return false; +} diff --git a/src/modules/ekf2/EKF/aid_sources/Airspeed2D.hpp b/src/modules/ekf2/EKF/aid_sources/Airspeed2D.hpp new file mode 100644 index 0000000000..5d1bd41b2c --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/Airspeed2D.hpp @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef EKF_AIRSPEED2D_HPP +#define EKF_AIRSPEED2D_HPP + +#if defined(CONFIG_EKF2_AIRSPEED2D) && defined(MODULE_NAME) + +#include "EstimatorAidSource.hpp" + +#include "../RingBuffer.h" + +#include +#include +#include +#include +#include + +class Airspeed2D : public EstimatorAidSource, public ModuleParams +{ +public: + Airspeed2D() : ModuleParams(nullptr) {} + virtual ~Airspeed2D() = default; + + void reset() override {}; + bool update(Ekf &ekf, const estimator::imuSample &imu_delayed) override; + +private: + struct AirflowSample { + uint64_t time_us{}; + float speed{}; + float direction_rad{}; + }; + + RingBuffer _airflow_buffer{20}; // TODO: size with _obs_buffer_length and actual publication rate + uint64_t _time_last_buffer_push{0}; + + estimator_aid_source2d_s _aid_src{}; + + uORB::PublicationMulti _estimator_aid_src_airspeed2d_pub {ORB_ID(estimator_aid_src_airspeed2d)}; + uORB::Subscription _sensor_airflow_sub{ORB_ID(sensor_airflow)}; + + DEFINE_PARAMETERS( + (ParamInt) _param_ekf2_asp2d_ctrl, + (ParamFloat) _param_ekf2_asp2d_delay, + (ParamFloat) _param_ekf2_asp2d_noise, + (ParamFloat) _param_ekf2_asp2d_gate + ) + +}; + +#endif // CONFIG_EKF2_AIRSPEED2D && MODULE_NAME + +#endif // !EKF_AIRSPEED2D_HPP diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index bc087558e9..ff94f41c54 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -59,6 +59,10 @@ #include #include +#if defined(CONFIG_EKF2_AIRSPEED2D) +# include "aid_sources/Airspeed2D.hpp" +#endif // CONFIG_EKF2_AIRSPEED2D + #include "aid_sources/ZeroGyroUpdate.hpp" #include "aid_sources/ZeroVelocityUpdate.hpp" @@ -524,6 +528,99 @@ public: friend class AuxGlobalPosition; + +#if defined(CONFIG_EKF2_WIND) + // perform a reset of the wind states and related covariances + void resetWind(); + void resetWind(const Vector2f& wind_vel, float wind_vel_var = NAN); + void resetWindCov(); + void resetWindToZero(); +#endif // CONFIG_EKF2_WIND + + void resetEstimatorAidStatus(estimator_aid_source1d_s &status) const + { + // only bother resetting if timestamp_sample is set + if (status.timestamp_sample != 0) { + status.timestamp_sample = 0; + + // preserve status.time_last_fuse + + status.observation = 0; + status.observation_variance = 0; + + status.innovation = 0; + status.innovation_variance = 0; + status.test_ratio = INFINITY; + + status.innovation_rejected = true; + status.fused = false; + } + } + + template + void resetEstimatorAidStatus(T &status) const + { + // only bother resetting if timestamp_sample is set + if (status.timestamp_sample != 0) { + status.timestamp_sample = 0; + + // preserve status.time_last_fuse + + for (size_t i = 0; i < (sizeof(status.observation) / sizeof(status.observation[0])); i++) { + status.observation[i] = 0; + status.observation_variance[i] = 0; + + status.innovation[i] = 0; + status.innovation_variance[i] = 0; + status.test_ratio[i] = INFINITY; + } + + status.innovation_rejected = true; + status.fused = false; + } + } + + void setEstimatorAidStatusTestRatio(estimator_aid_source1d_s &status, float innovation_gate) const + { + if (PX4_ISFINITE(status.innovation) + && PX4_ISFINITE(status.innovation_variance) + && (status.innovation_variance > 0.f) + ) { + status.test_ratio = sq(status.innovation) / (sq(innovation_gate) * status.innovation_variance); + status.innovation_rejected = (status.test_ratio > 1.f); + + } else { + status.test_ratio = INFINITY; + status.innovation_rejected = true; + } + } + + template + void setEstimatorAidStatusTestRatio(T &status, float innovation_gate) const + { + bool innovation_rejected = false; + + for (size_t i = 0; i < (sizeof(status.test_ratio) / sizeof(status.test_ratio[0])); i++) { + if (PX4_ISFINITE(status.innovation[i]) + && PX4_ISFINITE(status.innovation_variance[i]) + && (status.innovation_variance[i] > 0.f) + ) { + status.test_ratio[i] = sq(status.innovation[i]) / (sq(innovation_gate) * status.innovation_variance[i]); + + if (status.test_ratio[i] > 1.f) { + innovation_rejected = true; + } + + } else { + status.test_ratio[i] = INFINITY; + innovation_rejected = true; + } + } + + // if any of the innovations are rejected, then the overall innovation is rejected + status.innovation_rejected = innovation_rejected; + } + private: // set the internal states and status to their default value @@ -1115,13 +1212,6 @@ private: void resetMagCov(); #endif // CONFIG_EKF2_MAGNETOMETER -#if defined(CONFIG_EKF2_WIND) - // perform a reset of the wind states and related covariances - void resetWind(); - void resetWindCov(); - void resetWindToZero(); -#endif // CONFIG_EKF2_WIND - void resetGyroBiasZCov(); // uncorrelate quaternion states from other states @@ -1160,96 +1250,21 @@ private: bool _ev_q_error_initialized{false}; #endif // CONFIG_EKF2_EXTERNAL_VISION - void resetEstimatorAidStatus(estimator_aid_source1d_s &status) const - { - // only bother resetting if timestamp_sample is set - if (status.timestamp_sample != 0) { - status.timestamp_sample = 0; - - // preserve status.time_last_fuse - - status.observation = 0; - status.observation_variance = 0; - - status.innovation = 0; - status.innovation_variance = 0; - status.test_ratio = INFINITY; - - status.innovation_rejected = true; - status.fused = false; - } - } - - template - void resetEstimatorAidStatus(T &status) const - { - // only bother resetting if timestamp_sample is set - if (status.timestamp_sample != 0) { - status.timestamp_sample = 0; - - // preserve status.time_last_fuse - - for (size_t i = 0; i < (sizeof(status.observation) / sizeof(status.observation[0])); i++) { - status.observation[i] = 0; - status.observation_variance[i] = 0; - - status.innovation[i] = 0; - status.innovation_variance[i] = 0; - status.test_ratio[i] = INFINITY; - } - - status.innovation_rejected = true; - status.fused = false; - } - } - - void setEstimatorAidStatusTestRatio(estimator_aid_source1d_s &status, float innovation_gate) const - { - if (PX4_ISFINITE(status.innovation) - && PX4_ISFINITE(status.innovation_variance) - && (status.innovation_variance > 0.f) - ) { - status.test_ratio = sq(status.innovation) / (sq(innovation_gate) * status.innovation_variance); - status.innovation_rejected = (status.test_ratio > 1.f); - - } else { - status.test_ratio = INFINITY; - status.innovation_rejected = true; - } - } - - template - void setEstimatorAidStatusTestRatio(T &status, float innovation_gate) const - { - bool innovation_rejected = false; - - for (size_t i = 0; i < (sizeof(status.test_ratio) / sizeof(status.test_ratio[0])); i++) { - if (PX4_ISFINITE(status.innovation[i]) - && PX4_ISFINITE(status.innovation_variance[i]) - && (status.innovation_variance[i] > 0.f) - ) { - status.test_ratio[i] = sq(status.innovation[i]) / (sq(innovation_gate) * status.innovation_variance[i]); - - if (status.test_ratio[i] > 1.f) { - innovation_rejected = true; - } - - } else { - status.test_ratio[i] = INFINITY; - innovation_rejected = true; - } - } - - // if any of the innovations are rejected, then the overall innovation is rejected - status.innovation_rejected = innovation_rejected; - } - ZeroGyroUpdate _zero_gyro_update{}; ZeroVelocityUpdate _zero_velocity_update{}; -#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) +#if defined(MODULE_NAME) + +# if defined(CONFIG_EKF2_AIRSPEED2D) + Airspeed2D _airspeed2d{}; +# endif // CONFIG_EKF2_AIRSPEED2D + +# if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) AuxGlobalPosition _aux_global_position{}; -#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION +# endif // CONFIG_EKF2_AUX_GLOBAL_POSITION + +#endif // MODULE_NAME + }; #endif // !EKF_EKF_H diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index bb9e901f57..1fb2b4e958 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -769,6 +769,23 @@ void Ekf::resetWind() resetWindToZero(); } +void Ekf::resetWind(const Vector2f& wind_vel, const float wind_vel_var) +{ + _control_status.flags.wind = true; + + ECL_INFO("reset wind velocity to (%.1f, %.1f) m/s", double(wind_vel(0)), double(wind_vel(1))); + + _state.wind_vel = wind_vel; + + float obs_var = sq(_params.initial_wind_uncertainty); + + if (PX4_ISFINITE(wind_vel_var)) { + obs_var = math::max(obs_var, wind_vel_var); + } + + P.uncorrelateCovarianceSetVariance(State::wind_vel.idx, obs_var); +} + void Ekf::resetWindToZero() { ECL_INFO("reset wind to zero"); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 5cbe1a127f..5fdee17df4 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -282,6 +282,64 @@ def compute_wind_init_and_cov_from_airspeed( wind = wind.subs({sideslip: 0.0}) return (wind, P) +def predict_body_airspeed(state: State) -> (sf.V2): + + wind = sf.V3(state["wind_vel"][0], state["wind_vel"][1], 0.0) + vel_rel = state["vel"] - wind + + relative_wind_body = state["quat_nominal"].inverse() * vel_rel + + return relative_wind_body + +def compute_airspeed_2d_innov_innov_var_and_hx( + state: VState, + P: MTangent, + airspeed_2d: sf.V2, + R: sf.Scalar, + epsilon: sf.Scalar +) -> (sf.V2, sf.V2, VTangent): + + state = vstate_to_state(state) + relative_wind_body = predict_body_airspeed(state) + + meas_pred = sf.V2() + meas_pred[0] = relative_wind_body[0] + meas_pred[1] = relative_wind_body[1] + + innov = meas_pred - airspeed_2d + + # initialize outputs + innov_var = sf.V2() + H = [None] * 2 + + # calculate observation jacobian (H), kalman gain (K), and innovation variance (S) + # for each axis + for i in range(2): + H[i] = sf.V1(meas_pred[i]).jacobian(state) + innov_var[i] = (H[i] * P * H[i].T + R)[0,0] + + return (innov, innov_var, H[0].T) + +def compute_airspeed_2d_y_innov_innov_var_and_hy( + state: VState, + P: MTangent, + airspeed_2d: sf.V2, + R: sf.Scalar, + epsilon: sf.Scalar +) -> (sf.Scalar, sf.Scalar, VTangent): + + state = vstate_to_state(state) + meas_pred = predict_body_airspeed(state) + + innov = meas_pred[1] - airspeed_2d[1] + + # calculate observation jacobian (H), kalman gain (K), and innovation variance (S) + # for each axis + H = sf.V1(meas_pred[1]).jacobian(state) + innov_var = (H * P * H.T + R)[0,0] + + return (innov, innov_var, H.T) + def predict_sideslip( state: State, epsilon: sf.Scalar @@ -627,6 +685,8 @@ if not args.disable_wind: generate_px4_function(compute_sideslip_h_and_k, output_names=["H", "K"]) generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"]) generate_px4_function(compute_wind_init_and_cov_from_airspeed, output_names=["wind", "P_wind"]) + generate_px4_function(compute_airspeed_2d_innov_innov_var_and_hx, output_names=["innov", "innov_var", "Hx"]) + generate_px4_function(compute_airspeed_2d_y_innov_innov_var_and_hy, output_names=["innov", "innov_var", "Hy"]) generate_px4_function(compute_yaw_innov_var_and_h, output_names=["innov_var", "H"]) generate_px4_function(compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"]) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_2d_innov_innov_var_and_hx.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_2d_innov_innov_var_and_hx.h new file mode 100644 index 0000000000..aaba379d12 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_2d_innov_innov_var_and_hx.h @@ -0,0 +1,144 @@ +// ----------------------------------------------------------------------------- +// 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_airspeed_2d_innov_innov_var_and_hx + * + * Args: + * state: Matrix24_1 + * P: Matrix23_23 + * airspeed_2d: Matrix21 + * R: Scalar + * epsilon: Scalar + * + * Outputs: + * innov: Matrix21 + * innov_var: Matrix21 + * Hx: Matrix23_1 + */ +template +void ComputeAirspeed2DInnovInnovVarAndHx(const matrix::Matrix& state, + const matrix::Matrix& P, + const matrix::Matrix& airspeed_2d, + const Scalar R, const Scalar epsilon, + matrix::Matrix* const innov = nullptr, + matrix::Matrix* const innov_var = nullptr, + matrix::Matrix* const Hx = nullptr) { + // Total ops: 287 + + // Unused inputs + (void)epsilon; + + // Input arrays + + // Intermediate terms (33) + const Scalar _tmp0 = std::pow(state(2, 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 = -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(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 = _tmp7 * state(0, 0); + const Scalar _tmp12 = -_tmp11; + const Scalar _tmp13 = 2 * state(1, 0) * state(3, 0); + const Scalar _tmp14 = _tmp12 + _tmp13; + const Scalar _tmp15 = std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp16 = -2 * _tmp15 + _tmp2; + const Scalar _tmp17 = -_tmp6; + const Scalar _tmp18 = _tmp17 + _tmp8; + const Scalar _tmp19 = _tmp7 * state(3, 0); + const Scalar _tmp20 = _tmp5 * state(1, 0); + const Scalar _tmp21 = _tmp19 + _tmp20; + const Scalar _tmp22 = _tmp18 * _tmp4 + _tmp21 * state(6, 0); + const Scalar _tmp23 = -_tmp1; + const Scalar _tmp24 = std::pow(state(0, 0), Scalar(2)); + const Scalar _tmp25 = -_tmp15; + const Scalar _tmp26 = _tmp10 * (_tmp0 + _tmp23 + _tmp24 + _tmp25) + _tmp22; + const Scalar _tmp27 = -_tmp13; + const Scalar _tmp28 = _tmp0 - _tmp24; + const Scalar _tmp29 = _tmp10 * (-_tmp19 + _tmp20) + _tmp4 * (_tmp12 + _tmp27) + + state(6, 0) * (_tmp15 + _tmp23 + _tmp28); + const Scalar _tmp30 = _tmp1 + _tmp25; + const Scalar _tmp31 = _tmp10 * (_tmp19 - _tmp20) + _tmp4 * (_tmp11 + _tmp13) + + state(6, 0) * (-_tmp0 + _tmp24 + _tmp30); + const Scalar _tmp32 = + _tmp10 * (_tmp17 - _tmp8) + _tmp4 * (_tmp28 + _tmp30) + state(6, 0) * (_tmp11 + _tmp27); + + // Output terms (3) + if (innov != nullptr) { + matrix::Matrix& _innov = (*innov); + + _innov(0, 0) = _tmp10 * _tmp9 + _tmp14 * state(6, 0) + _tmp3 * _tmp4 - airspeed_2d(0, 0); + _innov(1, 0) = _tmp10 * _tmp16 + _tmp22 - airspeed_2d(1, 0); + } + + if (innov_var != nullptr) { + matrix::Matrix& _innov_var = (*innov_var); + + _innov_var(0, 0) = + R + + _tmp14 * (P(1, 5) * _tmp29 + P(2, 5) * _tmp26 - P(21, 5) * _tmp3 - P(22, 5) * _tmp9 + + P(3, 5) * _tmp3 + P(4, 5) * _tmp9 + P(5, 5) * _tmp14) + + _tmp26 * (P(1, 2) * _tmp29 + P(2, 2) * _tmp26 - P(21, 2) * _tmp3 - P(22, 2) * _tmp9 + + P(3, 2) * _tmp3 + P(4, 2) * _tmp9 + P(5, 2) * _tmp14) + + _tmp29 * (P(1, 1) * _tmp29 + P(2, 1) * _tmp26 - P(21, 1) * _tmp3 - P(22, 1) * _tmp9 + + P(3, 1) * _tmp3 + P(4, 1) * _tmp9 + P(5, 1) * _tmp14) - + _tmp3 * (P(1, 21) * _tmp29 + P(2, 21) * _tmp26 - P(21, 21) * _tmp3 - P(22, 21) * _tmp9 + + P(3, 21) * _tmp3 + P(4, 21) * _tmp9 + P(5, 21) * _tmp14) + + _tmp3 * (P(1, 3) * _tmp29 + P(2, 3) * _tmp26 - P(21, 3) * _tmp3 - P(22, 3) * _tmp9 + + P(3, 3) * _tmp3 + P(4, 3) * _tmp9 + P(5, 3) * _tmp14) - + _tmp9 * (P(1, 22) * _tmp29 + P(2, 22) * _tmp26 - P(21, 22) * _tmp3 - P(22, 22) * _tmp9 + + P(3, 22) * _tmp3 + P(4, 22) * _tmp9 + P(5, 22) * _tmp14) + + _tmp9 * (P(1, 4) * _tmp29 + P(2, 4) * _tmp26 - P(21, 4) * _tmp3 - P(22, 4) * _tmp9 + + P(3, 4) * _tmp3 + P(4, 4) * _tmp9 + P(5, 4) * _tmp14); + _innov_var(1, 0) = + R - + _tmp16 * (P(0, 22) * _tmp31 + P(2, 22) * _tmp32 - P(21, 22) * _tmp18 - P(22, 22) * _tmp16 + + P(3, 22) * _tmp18 + P(4, 22) * _tmp16 + P(5, 22) * _tmp21) + + _tmp16 * (P(0, 4) * _tmp31 + P(2, 4) * _tmp32 - P(21, 4) * _tmp18 - P(22, 4) * _tmp16 + + P(3, 4) * _tmp18 + P(4, 4) * _tmp16 + P(5, 4) * _tmp21) - + _tmp18 * (P(0, 21) * _tmp31 + P(2, 21) * _tmp32 - P(21, 21) * _tmp18 - P(22, 21) * _tmp16 + + P(3, 21) * _tmp18 + P(4, 21) * _tmp16 + P(5, 21) * _tmp21) + + _tmp18 * (P(0, 3) * _tmp31 + P(2, 3) * _tmp32 - P(21, 3) * _tmp18 - P(22, 3) * _tmp16 + + P(3, 3) * _tmp18 + P(4, 3) * _tmp16 + P(5, 3) * _tmp21) + + _tmp21 * (P(0, 5) * _tmp31 + P(2, 5) * _tmp32 - P(21, 5) * _tmp18 - P(22, 5) * _tmp16 + + P(3, 5) * _tmp18 + P(4, 5) * _tmp16 + P(5, 5) * _tmp21) + + _tmp31 * (P(0, 0) * _tmp31 + P(2, 0) * _tmp32 - P(21, 0) * _tmp18 - P(22, 0) * _tmp16 + + P(3, 0) * _tmp18 + P(4, 0) * _tmp16 + P(5, 0) * _tmp21) + + _tmp32 * (P(0, 2) * _tmp31 + P(2, 2) * _tmp32 - P(21, 2) * _tmp18 - P(22, 2) * _tmp16 + + P(3, 2) * _tmp18 + P(4, 2) * _tmp16 + P(5, 2) * _tmp21); + } + + if (Hx != nullptr) { + matrix::Matrix& _hx = (*Hx); + + _hx.setZero(); + + _hx(1, 0) = _tmp29; + _hx(2, 0) = _tmp26; + _hx(3, 0) = _tmp3; + _hx(4, 0) = _tmp9; + _hx(5, 0) = _tmp14; + _hx(21, 0) = -_tmp3; + _hx(22, 0) = -_tmp9; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_2d_y_innov_innov_var_and_hy.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_2d_y_innov_innov_var_and_hy.h new file mode 100644 index 0000000000..7a642c85c5 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_2d_y_innov_innov_var_and_hy.h @@ -0,0 +1,113 @@ +// ----------------------------------------------------------------------------- +// 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_airspeed_2d_y_innov_innov_var_and_hy + * + * Args: + * state: Matrix24_1 + * P: Matrix23_23 + * airspeed_2d: Matrix21 + * R: Scalar + * epsilon: Scalar + * + * Outputs: + * innov: Scalar + * innov_var: Scalar + * Hy: Matrix23_1 + */ +template +void ComputeAirspeed2DYInnovInnovVarAndHy(const matrix::Matrix& state, + const matrix::Matrix& P, + const matrix::Matrix& airspeed_2d, + const Scalar R, const Scalar epsilon, + Scalar* const innov = nullptr, + Scalar* const innov_var = nullptr, + matrix::Matrix* const Hy = nullptr) { + // Total ops: 154 + + // Unused inputs + (void)epsilon; + + // Input arrays + + // Intermediate terms (20) + const Scalar _tmp0 = std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp2 = -2 * _tmp0 - 2 * _tmp1 + 1; + const Scalar _tmp3 = -state(23, 0) + state(5, 0); + const Scalar _tmp4 = 2 * state(0, 0); + const Scalar _tmp5 = -_tmp4 * state(3, 0); + const Scalar _tmp6 = 2 * state(2, 0); + const Scalar _tmp7 = _tmp6 * state(1, 0); + const Scalar _tmp8 = _tmp5 + _tmp7; + const Scalar _tmp9 = -state(22, 0) + state(4, 0); + const Scalar _tmp10 = _tmp6 * state(3, 0); + const Scalar _tmp11 = _tmp4 * state(1, 0); + const Scalar _tmp12 = _tmp10 + _tmp11; + const Scalar _tmp13 = _tmp6 * state(0, 0); + const Scalar _tmp14 = 2 * state(1, 0) * state(3, 0); + const Scalar _tmp15 = std::pow(state(0, 0), Scalar(2)); + const Scalar _tmp16 = std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp17 = _tmp0 - _tmp1; + const Scalar _tmp18 = _tmp3 * (_tmp10 - _tmp11) + _tmp9 * (_tmp13 + _tmp14) + + state(6, 0) * (_tmp15 - _tmp16 + _tmp17); + const Scalar _tmp19 = _tmp3 * (_tmp5 - _tmp7) + _tmp9 * (-_tmp15 + _tmp16 + _tmp17) + + state(6, 0) * (_tmp13 - _tmp14); + + // Output terms (3) + if (innov != nullptr) { + Scalar& _innov = (*innov); + + _innov = _tmp12 * state(6, 0) + _tmp2 * _tmp3 + _tmp8 * _tmp9 - airspeed_2d(1, 0); + } + + if (innov_var != nullptr) { + Scalar& _innov_var = (*innov_var); + + _innov_var = + R + + _tmp12 * (P(0, 5) * _tmp18 + P(2, 5) * _tmp19 - P(21, 5) * _tmp8 - P(22, 5) * _tmp2 + + P(3, 5) * _tmp8 + P(4, 5) * _tmp2 + P(5, 5) * _tmp12) + + _tmp18 * (P(0, 0) * _tmp18 + P(2, 0) * _tmp19 - P(21, 0) * _tmp8 - P(22, 0) * _tmp2 + + P(3, 0) * _tmp8 + P(4, 0) * _tmp2 + P(5, 0) * _tmp12) + + _tmp19 * (P(0, 2) * _tmp18 + P(2, 2) * _tmp19 - P(21, 2) * _tmp8 - P(22, 2) * _tmp2 + + P(3, 2) * _tmp8 + P(4, 2) * _tmp2 + P(5, 2) * _tmp12) - + _tmp2 * (P(0, 22) * _tmp18 + P(2, 22) * _tmp19 - P(21, 22) * _tmp8 - P(22, 22) * _tmp2 + + P(3, 22) * _tmp8 + P(4, 22) * _tmp2 + P(5, 22) * _tmp12) + + _tmp2 * (P(0, 4) * _tmp18 + P(2, 4) * _tmp19 - P(21, 4) * _tmp8 - P(22, 4) * _tmp2 + + P(3, 4) * _tmp8 + P(4, 4) * _tmp2 + P(5, 4) * _tmp12) - + _tmp8 * (P(0, 21) * _tmp18 + P(2, 21) * _tmp19 - P(21, 21) * _tmp8 - P(22, 21) * _tmp2 + + P(3, 21) * _tmp8 + P(4, 21) * _tmp2 + P(5, 21) * _tmp12) + + _tmp8 * (P(0, 3) * _tmp18 + P(2, 3) * _tmp19 - P(21, 3) * _tmp8 - P(22, 3) * _tmp2 + + P(3, 3) * _tmp8 + P(4, 3) * _tmp2 + P(5, 3) * _tmp12); + } + + if (Hy != nullptr) { + matrix::Matrix& _hy = (*Hy); + + _hy.setZero(); + + _hy(0, 0) = _tmp18; + _hy(2, 0) = _tmp19; + _hy(3, 0) = _tmp8; + _hy(4, 0) = _tmp2; + _hy(5, 0) = _tmp12; + _hy(21, 0) = -_tmp8; + _hy(22, 0) = -_tmp2; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/Kconfig b/src/modules/ekf2/Kconfig index a189934415..745dfe70de 100644 --- a/src/modules/ekf2/Kconfig +++ b/src/modules/ekf2/Kconfig @@ -31,6 +31,14 @@ depends on MODULES_EKF2 ---help--- EKF2 airspeed fusion support. +menuconfig EKF2_AIRSPEED2D +depends on MODULES_EKF2 + bool "airspeed 2D fusion support" + default n + depends on EKF2_WIND + ---help--- + EKF2 airspeed 2D fusion support. + menuconfig EKF2_AUX_GLOBAL_POSITION depends on MODULES_EKF2 bool "aux global position fusion support" diff --git a/src/modules/ekf2/ekf2_params_airspeed_2d.c b/src/modules/ekf2/ekf2_params_airspeed_2d.c new file mode 100644 index 0000000000..bde5e8cc03 --- /dev/null +++ b/src/modules/ekf2/ekf2_params_airspeed_2d.c @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Airspeed 2D sensor aiding + * + * @group EKF2 + * @boolean + */ +PARAM_DEFINE_INT32(EKF2_ASP2D_CTRL, 0); + +/** + * Airspeed 2D measurement delay relative to IMU measurements + * + * @group EKF2 + * @min 0 + * @max 300 + * @unit ms + * @reboot_required true + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_ASP2D_DELAY, 100); + +/** + * Airspeed 2D measurement noise. + * + * @group EKF2 + * @min 0.5 + * @max 5.0 + * @unit m/s + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_ASP2D_NOISE, 3.0f); + +/** + * Airspeed 2D gate size + * + * Sets the number of standard deviations used by the innovation consistency test. + * + * @group EKF2 + * @min 1.0 + * @unit SD + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_ASP2D_GATE, 3.0f); diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 3465d857bc..b9c5bca4eb 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -209,6 +209,7 @@ void LoggedTopics::add_default_topics() // add_optional_topic_multi("estimator_aid_src_optical_flow", 100, MAX_ESTIMATOR_INSTANCES); // add_optional_topic_multi("estimator_aid_src_terrain_optical_flow", 100, MAX_ESTIMATOR_INSTANCES); // add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_aid_src_airspeed2d", 100, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_aid_src_aux_global_position", 100, MAX_ESTIMATOR_INSTANCES); // log all raw sensors at minimal rate (at least 1 Hz)