diff --git a/msg/EstimatorAidSource3d.msg b/msg/EstimatorAidSource3d.msg index bb53ae9823..5d143bc04d 100644 --- a/msg/EstimatorAidSource3d.msg +++ b/msg/EstimatorAidSource3d.msg @@ -18,6 +18,4 @@ bool fusion_enabled # true when measurements are being fused bool innovation_rejected # true if the observation has been rejected bool fused # true if the sample was successfully fused -# TOPICS estimator_aid_src_ev_vel -# TOPICS estimator_aid_src_gnss_vel -# TOPICS estimator_aid_src_mag +# TOPICS estimator_aid_src_ev_vel estimator_aid_src_gnss_vel estimator_aid_src_gravity estimator_aid_src_mag diff --git a/msg/EstimatorInnovations.msg b/msg/EstimatorInnovations.msg index e8b430669e..c8500d9e83 100644 --- a/msg/EstimatorInnovations.msg +++ b/msg/EstimatorInnovations.msg @@ -28,6 +28,7 @@ float32[2] terr_flow # flow innvoation (rad/sec) and innovation variance compute # Various float32 heading # heading innovation (rad) and innovation variance (rad**2) float32[3] mag_field # earth magnetic field innovation (Gauss) and innovation variance (Gauss**2) +float32[3] gravity # gravity innovation from accelerometerr vector (m/s**2) float32[2] drag # drag specific force innovation (m/sec**2) and innovation variance ((m/sec)**2) float32 airspeed # airspeed innovation (m/sec) and innovation variance ((m/sec)**2) float32 beta # synthetic sideslip innovation (rad) and innovation variance (rad**2) diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index 2905011582..9d1f77403a 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -38,6 +38,7 @@ bool cs_wind_dead_reckoning # 30 - true if we are navigationg reliant on win bool cs_rng_kin_consistent # 31 - true when the range finder kinematic consistency check is passing bool cs_fake_pos # 32 - true when fake position measurements are being fused bool cs_fake_hgt # 33 - true when fake height measurements are being fused +bool cs_gravity_vector # 34 - true when gravity vector measurements are being fused # fault status uint32 fault_status_changes # number of filter fault status (fs) changes diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index e8e6d16f31..53690c7d2e 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -102,6 +102,7 @@ px4_add_module( EKF/gps_checks.cpp EKF/gps_control.cpp EKF/gps_yaw_fusion.cpp + EKF/gravity_fusion.cpp EKF/height_control.cpp EKF/imu_down_sampler.cpp EKF/mag_control.cpp diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 7ee0ed1d04..ea273a4f85 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -53,6 +53,7 @@ add_library(ecl_EKF gps_checks.cpp gps_control.cpp gps_yaw_fusion.cpp + gravity_fusion.cpp height_control.cpp imu_down_sampler.cpp mag_control.cpp diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index ebd249b679..c5fb499b69 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -127,8 +127,9 @@ enum class PositionSensor : uint8_t { }; enum class ImuCtrl : uint8_t { - GyroBias = (1<<0), - AccelBias = (1<<1), + GyroBias = (1<<0), + AccelBias = (1<<1), + GravityVector = (1<<2), }; enum GnssCtrl : uint8_t { @@ -393,6 +394,9 @@ struct parameters { float ev_pos_innov_gate{5.0f}; ///< vision position fusion innovation consistency gate size (STD) float ev_hgt_bias_nsd{0.13f}; ///< process noise for vision height bias estimation (m/s/sqrt(Hz)) + // gravity fusion + float gravity_noise{1.0f}; ///< accelerometer measurement gaussian noise (m/s**2) + // optical flow fusion float flow_noise{0.15f}; ///< observation noise for optical flow LOS rate measurements (rad/sec) float flow_noise_qual_min{0.5f}; ///< observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec) @@ -564,6 +568,7 @@ union filter_control_status_u { uint64_t rng_kin_consistent : 1; ///< 31 - true when the range finder kinematic consistency check is passing uint64_t fake_pos : 1; ///< 32 - true when fake position measurements are being fused uint64_t fake_hgt : 1; ///< 33 - true when fake height measurements are being fused + uint64_t gravity_vector : 1; ///< 34 - true when gravity vector measurements are being fused } flags; uint64_t value; }; diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 460f4c8371..bb40ef49df 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -198,6 +198,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) controlBetaFusion(imu_delayed); controlDragFusion(); controlHeightFusion(imu_delayed); + controlGravityFusion(imu_delayed); // Additional data odometry data from an external estimator can be fused. controlExternalVisionFusion(); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index d787a3dde9..a992d63480 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -191,6 +191,10 @@ public: void getHaglRateInnovVar(float &hagl_rate_innov_var) const { hagl_rate_innov_var = _rng_consistency_check.getInnovVar(); } void getHaglRateInnovRatio(float &hagl_rate_innov_ratio) const { hagl_rate_innov_ratio = _rng_consistency_check.getSignedTestRatioLpf(); } + void getGravityInnov(float grav_innov[3]) const { memcpy(grav_innov, _aid_src_gravity.innovation, sizeof(_aid_src_gravity.innovation)); } + void getGravityInnovVar(float grav_innov_var[3]) const { memcpy(grav_innov_var, _aid_src_gravity.innovation_variance, sizeof(_aid_src_gravity.innovation_variance)); } + void getGravityInnovRatio(float &grav_innov_ratio) const { grav_innov_ratio = Vector3f(_aid_src_gravity.test_ratio).max(); } + // get the state vector at the delayed time horizon matrix::Vector getStateAtFusionHorizonAsVector() const; @@ -433,6 +437,8 @@ public: const auto &aid_src_mag_heading() const { return _aid_src_mag_heading; } const auto &aid_src_mag() const { return _aid_src_mag; } + const auto &aid_src_gravity() const { return _aid_src_gravity; } + const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; } const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; } @@ -573,6 +579,8 @@ private: estimator_aid_source1d_s _aid_src_mag_heading{}; estimator_aid_source3d_s _aid_src_mag{}; + estimator_aid_source3d_s _aid_src_gravity{}; + estimator_aid_source2d_s _aid_src_aux_vel{}; estimator_aid_source2d_s _aid_src_optical_flow{}; @@ -927,6 +935,9 @@ private: void updateGroundEffect(); + // gravity fusion: heuristically enable / disable gravity fusion + void controlGravityFusion(const imuSample &imu_delayed); + // calculate the measurement variance for the optical flow sensor float calcOptFlowMeasVar(const flowSample &flow_sample); diff --git a/src/modules/ekf2/EKF/gravity_fusion.cpp b/src/modules/ekf2/EKF/gravity_fusion.cpp new file mode 100644 index 0000000000..8a09b72f79 --- /dev/null +++ b/src/modules/ekf2/EKF/gravity_fusion.cpp @@ -0,0 +1,95 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 ECL 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 gravity_fusion.cpp + * Fuse observations from the gravity vector to constrain roll + * and pitch (a la complementary filter). + * + * @author Daniel M. Sahu + */ + +#include "ekf.h" +#include "python/ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h" + +#include + +void Ekf::controlGravityFusion(const imuSample &imu) +{ + // fuse gravity observation if our overall acceleration isn't too big + const float gravity_scale = _accel_vec_filt.norm() / CONSTANTS_ONE_G; + + _control_status.flags.gravity_vector = (_params.imu_ctrl & static_cast(ImuCtrl::GravityVector)) + && (((gravity_scale >= 0.9f && gravity_scale <= 1.1f)) || _control_status.flags.vehicle_at_rest) + && !isHorizontalAidingActive(); + + // get raw accelerometer reading at delayed horizon and expected measurement noise (gaussian) + const Vector3f measurement = imu.delta_vel / imu.delta_vel_dt - getAccelBias(); + const float measurement_var = sq(_params.gravity_noise); + + // calculate kalman gains and innovation variances + Vector3f innovation; // innovation of the last gravity fusion observation (m/s**2) + Vector3f innovation_variance; + Vector24f Kx, Ky, Kz; // Kalman gain vectors + sym::ComputeGravityInnovVarAndKAndH( + getStateAtFusionHorizonAsVector(), P, measurement, measurement_var, FLT_EPSILON, + &innovation, &innovation_variance, &Kx, &Ky, &Kz); + + // fill estimator aid source status + resetEstimatorAidStatus(_aid_src_gravity); + _aid_src_gravity.timestamp_sample = imu.time_us; + measurement.copyTo(_aid_src_gravity.observation); + + for (auto &var : _aid_src_gravity.observation_variance) { + var = measurement_var; + } + + innovation.copyTo(_aid_src_gravity.innovation); + innovation_variance.copyTo(_aid_src_gravity.innovation_variance); + + float innovation_gate = 1.f; + setEstimatorAidStatusTestRatio(_aid_src_gravity, innovation_gate); + + _aid_src_gravity.fusion_enabled = _control_status.flags.gravity_vector; + + if (_aid_src_gravity.fusion_enabled && !_aid_src_gravity.innovation_rejected) { + // perform fusion for each axis + _aid_src_gravity.fused = measurementUpdate(Kx, innovation_variance(0), innovation(0)) + && measurementUpdate(Ky, innovation_variance(1), innovation(1)) + && measurementUpdate(Kz, innovation_variance(2), innovation(2)); + + if (_aid_src_gravity.fused) { + _aid_src_gravity.time_last_fuse = imu.time_us; + } + } +} diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index ed8cf5e99f..3d15be6d82 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -477,6 +477,36 @@ def compute_drag_y_innov_var_and_k( return (innov_var, K) +def compute_gravity_innov_var_and_k_and_h( + state: VState, + P: MState, + meas: sf.V3, + R: sf.Scalar, + epsilon: sf.Scalar +) -> (sf.V3, sf.V3, VState, VState, VState): + + # get transform from earth to body frame + q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz]) + R_to_body = quat_to_rot(q_att).T + + # the innovation is the error between measured acceleration + # and predicted (body frame), assuming no body acceleration + meas_pred = R_to_body * sf.Matrix([0,0,-9.80665]) + innov = meas_pred - 9.80665 * meas.normalized(epsilon=epsilon) + + # initialize outputs + innov_var = sf.V3() + K = [None] * 3 + + # calculate observation jacobian (H), kalman gain (K), and innovation variance (S) + # for each axis + for i in range(3): + H = sf.V1(meas_pred[i]).jacobian(state) + innov_var[i] = (H * P * H.T + R)[0,0] + K[i] = P * H.T / innov_var[i] + + return (innov, innov_var, K[0], K[1], K[2]) + print("Derive EKF2 equations...") generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"]) generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"]) @@ -497,3 +527,4 @@ generate_px4_function(compute_flow_y_innov_var_and_h, output_names=["innov_var", generate_px4_function(compute_gnss_yaw_innon_innov_var_and_h, output_names=["innov", "innov_var", "H"]) generate_px4_function(compute_drag_x_innov_var_and_k, output_names=["innov_var", "K"]) generate_px4_function(compute_drag_y_innov_var_and_k, output_names=["innov_var", "K"]) +generate_px4_function(compute_gravity_innov_var_and_k_and_h, output_names=["innov", "innov_var", "Kx", "Ky", "Kz"]) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h new file mode 100644 index 0000000000..5ced3fdb16 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h @@ -0,0 +1,276 @@ +// ----------------------------------------------------------------------------- +// 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_gravity_innov_var_and_k_and_h + * + * Args: + * state: Matrix24_1 + * P: Matrix24_24 + * meas: Matrix31 + * R: Scalar + * epsilon: Scalar + * + * Outputs: + * innov: Matrix31 + * innov_var: Matrix31 + * Kx: Matrix24_1 + * Ky: Matrix24_1 + * Kz: Matrix24_1 + */ +template +void ComputeGravityInnovVarAndKAndH(const matrix::Matrix& state, + const matrix::Matrix& P, + const matrix::Matrix& meas, const Scalar R, + const Scalar epsilon, + matrix::Matrix* const innov = nullptr, + matrix::Matrix* const innov_var = nullptr, + matrix::Matrix* const Kx = nullptr, + matrix::Matrix* const Ky = nullptr, + matrix::Matrix* const Kz = nullptr) { + // Total ops: 736 + + // Input arrays + + // Intermediate terms (54) + const Scalar _tmp0 = + Scalar(9.8066499999999994) / + std::sqrt(Scalar(epsilon + std::pow(meas(0, 0), Scalar(2)) + std::pow(meas(1, 0), Scalar(2)) + + std::pow(meas(2, 0), Scalar(2)))); + const Scalar _tmp1 = Scalar(19.613299999999999) * state(1, 0); + const Scalar _tmp2 = -P(3, 0) * _tmp1; + const Scalar _tmp3 = Scalar(19.613299999999999) * state(2, 0); + const Scalar _tmp4 = P(0, 0) * _tmp3; + const Scalar _tmp5 = Scalar(19.613299999999999) * state(0, 0); + const Scalar _tmp6 = P(2, 0) * _tmp5; + const Scalar _tmp7 = Scalar(19.613299999999999) * state(3, 0); + const Scalar _tmp8 = P(3, 1) * _tmp1; + const Scalar _tmp9 = P(2, 1) * _tmp5; + const Scalar _tmp10 = -P(1, 1) * _tmp7; + const Scalar _tmp11 = P(0, 2) * _tmp3; + const Scalar _tmp12 = P(2, 2) * _tmp5; + const Scalar _tmp13 = -P(1, 2) * _tmp7; + const Scalar _tmp14 = -P(3, 3) * _tmp1; + const Scalar _tmp15 = P(0, 3) * _tmp3; + const Scalar _tmp16 = -P(1, 3) * _tmp7; + const Scalar _tmp17 = R - _tmp1 * (P(2, 3) * _tmp5 + _tmp14 + _tmp15 + _tmp16) + + _tmp3 * (-P(1, 0) * _tmp7 + _tmp2 + _tmp4 + _tmp6) + + _tmp5 * (-P(3, 2) * _tmp1 + _tmp11 + _tmp12 + _tmp13) - + _tmp7 * (P(0, 1) * _tmp3 + _tmp10 - _tmp8 + _tmp9); + const Scalar _tmp18 = P(3, 0) * _tmp3; + const Scalar _tmp19 = -P(0, 0) * _tmp1; + const Scalar _tmp20 = -P(1, 0) * _tmp5; + const Scalar _tmp21 = P(3, 2) * _tmp3; + const Scalar _tmp22 = -P(2, 2) * _tmp7; + const Scalar _tmp23 = P(1, 2) * _tmp5; + const Scalar _tmp24 = P(0, 1) * _tmp1; + const Scalar _tmp25 = -P(2, 1) * _tmp7; + const Scalar _tmp26 = -P(1, 1) * _tmp5; + const Scalar _tmp27 = -P(3, 3) * _tmp3; + const Scalar _tmp28 = -P(0, 3) * _tmp1; + const Scalar _tmp29 = -P(2, 3) * _tmp7; + const Scalar _tmp30 = R - _tmp1 * (-P(2, 0) * _tmp7 - _tmp18 + _tmp19 + _tmp20) - + _tmp3 * (-P(1, 3) * _tmp5 + _tmp27 + _tmp28 + _tmp29) - + _tmp5 * (-P(3, 1) * _tmp3 - _tmp24 + _tmp25 + _tmp26) - + _tmp7 * (-P(0, 2) * _tmp1 - _tmp21 + _tmp22 - _tmp23); + const Scalar _tmp31 = -P(0, 0) * _tmp5; + const Scalar _tmp32 = P(2, 0) * _tmp3; + const Scalar _tmp33 = P(1, 0) * _tmp1; + const Scalar _tmp34 = -P(3, 2) * _tmp7; + const Scalar _tmp35 = P(0, 2) * _tmp5; + const Scalar _tmp36 = P(2, 2) * _tmp3; + const Scalar _tmp37 = -P(3, 1) * _tmp7; + const Scalar _tmp38 = -P(0, 1) * _tmp5; + const Scalar _tmp39 = P(1, 1) * _tmp1; + const Scalar _tmp40 = -P(3, 3) * _tmp7; + const Scalar _tmp41 = P(2, 3) * _tmp3; + const Scalar _tmp42 = P(1, 3) * _tmp1; + const Scalar _tmp43 = R + _tmp1 * (P(2, 1) * _tmp3 + _tmp37 + _tmp38 + _tmp39) + + _tmp3 * (P(1, 2) * _tmp1 + _tmp34 - _tmp35 + _tmp36) - + _tmp5 * (-P(3, 0) * _tmp7 + _tmp31 + _tmp32 + _tmp33) - + _tmp7 * (-P(0, 3) * _tmp5 + _tmp40 + _tmp41 + _tmp42); + const Scalar _tmp44 = Scalar(1.0) / (_tmp17); + const Scalar _tmp45 = Scalar(19.613299999999999) * P(8, 3); + const Scalar _tmp46 = Scalar(19.613299999999999) * P(8, 0); + const Scalar _tmp47 = Scalar(19.613299999999999) * P(8, 2); + const Scalar _tmp48 = Scalar(19.613299999999999) * P(9, 3); + const Scalar _tmp49 = Scalar(19.613299999999999) * P(9, 2); + const Scalar _tmp50 = Scalar(19.613299999999999) * P(9, 0); + const Scalar _tmp51 = Scalar(1.0) / (_tmp30); + const Scalar _tmp52 = Scalar(19.613299999999999) * P(4, 0); + const Scalar _tmp53 = Scalar(1.0) / (_tmp43); + + // Output terms (5) + if (innov != nullptr) { + matrix::Matrix& _innov = (*innov); + + _innov(0, 0) = -_tmp0 * meas(0, 0) + Scalar(19.613299999999999) * state(0, 0) * state(2, 0) - + Scalar(19.613299999999999) * state(1, 0) * state(3, 0); + _innov(1, 0) = -_tmp0 * meas(1, 0) - Scalar(19.613299999999999) * state(0, 0) * state(1, 0) - + Scalar(19.613299999999999) * state(2, 0) * state(3, 0); + _innov(2, 0) = -_tmp0 * meas(2, 0) - + Scalar(9.8066499999999994) * std::pow(state(0, 0), Scalar(2)) + + Scalar(9.8066499999999994) * std::pow(state(1, 0), Scalar(2)) + + Scalar(9.8066499999999994) * std::pow(state(2, 0), Scalar(2)) - + Scalar(9.8066499999999994) * std::pow(state(3, 0), Scalar(2)); + } + + if (innov_var != nullptr) { + matrix::Matrix& _innov_var = (*innov_var); + + _innov_var(0, 0) = _tmp17; + _innov_var(1, 0) = _tmp30; + _innov_var(2, 0) = _tmp43; + } + + if (Kx != nullptr) { + matrix::Matrix& _kx = (*Kx); + + _kx(0, 0) = _tmp44 * (-P(0, 1) * _tmp7 + _tmp28 + _tmp35 + _tmp4); + _kx(1, 0) = _tmp44 * (P(1, 0) * _tmp3 + _tmp10 + _tmp23 - _tmp42); + _kx(2, 0) = _tmp44 * (-P(2, 3) * _tmp1 + _tmp12 + _tmp25 + _tmp32); + _kx(3, 0) = _tmp44 * (P(3, 2) * _tmp5 + _tmp14 + _tmp18 + _tmp37); + _kx(4, 0) = _tmp44 * (P(4, 0) * _tmp3 - P(4, 1) * _tmp7 + P(4, 2) * _tmp5 - P(4, 3) * _tmp1); + _kx(5, 0) = _tmp44 * (P(5, 0) * _tmp3 - P(5, 1) * _tmp7 + P(5, 2) * _tmp5 - P(5, 3) * _tmp1); + _kx(6, 0) = _tmp44 * (P(6, 0) * _tmp3 - P(6, 1) * _tmp7 + P(6, 2) * _tmp5 - P(6, 3) * _tmp1); + _kx(7, 0) = _tmp44 * (P(7, 0) * _tmp3 - P(7, 1) * _tmp7 + P(7, 2) * _tmp5 - P(7, 3) * _tmp1); + _kx(8, 0) = _tmp44 * (-P(8, 1) * _tmp7 - _tmp45 * state(1, 0) + _tmp46 * state(2, 0) + + _tmp47 * state(0, 0)); + _kx(9, 0) = _tmp44 * (-P(9, 1) * _tmp7 - _tmp48 * state(1, 0) + _tmp49 * state(0, 0) + + _tmp50 * state(2, 0)); + _kx(10, 0) = + _tmp44 * (P(10, 0) * _tmp3 - P(10, 1) * _tmp7 + P(10, 2) * _tmp5 - P(10, 3) * _tmp1); + _kx(11, 0) = + _tmp44 * (P(11, 0) * _tmp3 - P(11, 1) * _tmp7 + P(11, 2) * _tmp5 - P(11, 3) * _tmp1); + _kx(12, 0) = + _tmp44 * (P(12, 0) * _tmp3 - P(12, 1) * _tmp7 + P(12, 2) * _tmp5 - P(12, 3) * _tmp1); + _kx(13, 0) = + _tmp44 * (P(13, 0) * _tmp3 - P(13, 1) * _tmp7 + P(13, 2) * _tmp5 - P(13, 3) * _tmp1); + _kx(14, 0) = + _tmp44 * (P(14, 0) * _tmp3 - P(14, 1) * _tmp7 + P(14, 2) * _tmp5 - P(14, 3) * _tmp1); + _kx(15, 0) = + _tmp44 * (P(15, 0) * _tmp3 - P(15, 1) * _tmp7 + P(15, 2) * _tmp5 - P(15, 3) * _tmp1); + _kx(16, 0) = + _tmp44 * (P(16, 0) * _tmp3 - P(16, 1) * _tmp7 + P(16, 2) * _tmp5 - P(16, 3) * _tmp1); + _kx(17, 0) = + _tmp44 * (P(17, 0) * _tmp3 - P(17, 1) * _tmp7 + P(17, 2) * _tmp5 - P(17, 3) * _tmp1); + _kx(18, 0) = + _tmp44 * (P(18, 0) * _tmp3 - P(18, 1) * _tmp7 + P(18, 2) * _tmp5 - P(18, 3) * _tmp1); + _kx(19, 0) = + _tmp44 * (P(19, 0) * _tmp3 - P(19, 1) * _tmp7 + P(19, 2) * _tmp5 - P(19, 3) * _tmp1); + _kx(20, 0) = + _tmp44 * (P(20, 0) * _tmp3 - P(20, 1) * _tmp7 + P(20, 2) * _tmp5 - P(20, 3) * _tmp1); + _kx(21, 0) = + _tmp44 * (P(21, 0) * _tmp3 - P(21, 1) * _tmp7 + P(21, 2) * _tmp5 - P(21, 3) * _tmp1); + _kx(22, 0) = + _tmp44 * (P(22, 0) * _tmp3 - P(22, 1) * _tmp7 + P(22, 2) * _tmp5 - P(22, 3) * _tmp1); + _kx(23, 0) = + _tmp44 * (P(23, 0) * _tmp3 - P(23, 1) * _tmp7 + P(23, 2) * _tmp5 - P(23, 3) * _tmp1); + } + + if (Ky != nullptr) { + matrix::Matrix& _ky = (*Ky); + + _ky(0, 0) = _tmp51 * (-P(0, 2) * _tmp7 - _tmp15 + _tmp19 + _tmp38); + _ky(1, 0) = _tmp51 * (-P(1, 3) * _tmp3 + _tmp13 + _tmp26 - _tmp33); + _ky(2, 0) = _tmp51 * (-P(2, 0) * _tmp1 + _tmp22 - _tmp41 - _tmp9); + _ky(3, 0) = _tmp51 * (-P(3, 1) * _tmp5 + _tmp2 + _tmp27 + _tmp34); + _ky(4, 0) = + _tmp51 * (-P(4, 1) * _tmp5 - P(4, 2) * _tmp7 - P(4, 3) * _tmp3 - _tmp52 * state(1, 0)); + _ky(5, 0) = _tmp51 * (-P(5, 0) * _tmp1 - P(5, 1) * _tmp5 - P(5, 2) * _tmp7 - P(5, 3) * _tmp3); + _ky(6, 0) = _tmp51 * (-P(6, 0) * _tmp1 - P(6, 1) * _tmp5 - P(6, 2) * _tmp7 - P(6, 3) * _tmp3); + _ky(7, 0) = _tmp51 * (-P(7, 0) * _tmp1 - P(7, 1) * _tmp5 - P(7, 2) * _tmp7 - P(7, 3) * _tmp3); + _ky(8, 0) = + _tmp51 * (-P(8, 1) * _tmp5 - P(8, 2) * _tmp7 - _tmp45 * state(2, 0) - _tmp46 * state(1, 0)); + _ky(9, 0) = + _tmp51 * (-P(9, 1) * _tmp5 - P(9, 2) * _tmp7 - _tmp48 * state(2, 0) - _tmp50 * state(1, 0)); + _ky(10, 0) = + _tmp51 * (-P(10, 0) * _tmp1 - P(10, 1) * _tmp5 - P(10, 2) * _tmp7 - P(10, 3) * _tmp3); + _ky(11, 0) = + _tmp51 * (-P(11, 0) * _tmp1 - P(11, 1) * _tmp5 - P(11, 2) * _tmp7 - P(11, 3) * _tmp3); + _ky(12, 0) = + _tmp51 * (-P(12, 0) * _tmp1 - P(12, 1) * _tmp5 - P(12, 2) * _tmp7 - P(12, 3) * _tmp3); + _ky(13, 0) = + _tmp51 * (-P(13, 0) * _tmp1 - P(13, 1) * _tmp5 - P(13, 2) * _tmp7 - P(13, 3) * _tmp3); + _ky(14, 0) = + _tmp51 * (-P(14, 0) * _tmp1 - P(14, 1) * _tmp5 - P(14, 2) * _tmp7 - P(14, 3) * _tmp3); + _ky(15, 0) = + _tmp51 * (-P(15, 0) * _tmp1 - P(15, 1) * _tmp5 - P(15, 2) * _tmp7 - P(15, 3) * _tmp3); + _ky(16, 0) = + _tmp51 * (-P(16, 0) * _tmp1 - P(16, 1) * _tmp5 - P(16, 2) * _tmp7 - P(16, 3) * _tmp3); + _ky(17, 0) = + _tmp51 * (-P(17, 0) * _tmp1 - P(17, 1) * _tmp5 - P(17, 2) * _tmp7 - P(17, 3) * _tmp3); + _ky(18, 0) = + _tmp51 * (-P(18, 0) * _tmp1 - P(18, 1) * _tmp5 - P(18, 2) * _tmp7 - P(18, 3) * _tmp3); + _ky(19, 0) = + _tmp51 * (-P(19, 0) * _tmp1 - P(19, 1) * _tmp5 - P(19, 2) * _tmp7 - P(19, 3) * _tmp3); + _ky(20, 0) = + _tmp51 * (-P(20, 0) * _tmp1 - P(20, 1) * _tmp5 - P(20, 2) * _tmp7 - P(20, 3) * _tmp3); + _ky(21, 0) = + _tmp51 * (-P(21, 0) * _tmp1 - P(21, 1) * _tmp5 - P(21, 2) * _tmp7 - P(21, 3) * _tmp3); + _ky(22, 0) = + _tmp51 * (-P(22, 0) * _tmp1 - P(22, 1) * _tmp5 - P(22, 2) * _tmp7 - P(22, 3) * _tmp3); + _ky(23, 0) = + _tmp51 * (-P(23, 0) * _tmp1 - P(23, 1) * _tmp5 - P(23, 2) * _tmp7 - P(23, 3) * _tmp3); + } + + if (Kz != nullptr) { + matrix::Matrix& _kz = (*Kz); + + _kz(0, 0) = _tmp53 * (-P(0, 3) * _tmp7 + _tmp11 + _tmp24 + _tmp31); + _kz(1, 0) = _tmp53 * (P(1, 2) * _tmp3 + _tmp16 + _tmp20 + _tmp39); + _kz(2, 0) = _tmp53 * (P(2, 1) * _tmp1 + _tmp29 + _tmp36 - _tmp6); + _kz(3, 0) = _tmp53 * (-P(3, 0) * _tmp5 + _tmp21 + _tmp40 + _tmp8); + _kz(4, 0) = + _tmp53 * (P(4, 1) * _tmp1 + P(4, 2) * _tmp3 - P(4, 3) * _tmp7 - _tmp52 * state(0, 0)); + _kz(5, 0) = _tmp53 * (-P(5, 0) * _tmp5 + P(5, 1) * _tmp1 + P(5, 2) * _tmp3 - P(5, 3) * _tmp7); + _kz(6, 0) = _tmp53 * (-P(6, 0) * _tmp5 + P(6, 1) * _tmp1 + P(6, 2) * _tmp3 - P(6, 3) * _tmp7); + _kz(7, 0) = _tmp53 * (-P(7, 0) * _tmp5 + P(7, 1) * _tmp1 + P(7, 2) * _tmp3 - P(7, 3) * _tmp7); + _kz(8, 0) = + _tmp53 * (P(8, 1) * _tmp1 - P(8, 3) * _tmp7 - _tmp46 * state(0, 0) + _tmp47 * state(2, 0)); + _kz(9, 0) = + _tmp53 * (P(9, 1) * _tmp1 - P(9, 3) * _tmp7 + _tmp49 * state(2, 0) - _tmp50 * state(0, 0)); + _kz(10, 0) = + _tmp53 * (-P(10, 0) * _tmp5 + P(10, 1) * _tmp1 + P(10, 2) * _tmp3 - P(10, 3) * _tmp7); + _kz(11, 0) = + _tmp53 * (-P(11, 0) * _tmp5 + P(11, 1) * _tmp1 + P(11, 2) * _tmp3 - P(11, 3) * _tmp7); + _kz(12, 0) = + _tmp53 * (-P(12, 0) * _tmp5 + P(12, 1) * _tmp1 + P(12, 2) * _tmp3 - P(12, 3) * _tmp7); + _kz(13, 0) = + _tmp53 * (-P(13, 0) * _tmp5 + P(13, 1) * _tmp1 + P(13, 2) * _tmp3 - P(13, 3) * _tmp7); + _kz(14, 0) = + _tmp53 * (-P(14, 0) * _tmp5 + P(14, 1) * _tmp1 + P(14, 2) * _tmp3 - P(14, 3) * _tmp7); + _kz(15, 0) = + _tmp53 * (-P(15, 0) * _tmp5 + P(15, 1) * _tmp1 + P(15, 2) * _tmp3 - P(15, 3) * _tmp7); + _kz(16, 0) = + _tmp53 * (-P(16, 0) * _tmp5 + P(16, 1) * _tmp1 + P(16, 2) * _tmp3 - P(16, 3) * _tmp7); + _kz(17, 0) = + _tmp53 * (-P(17, 0) * _tmp5 + P(17, 1) * _tmp1 + P(17, 2) * _tmp3 - P(17, 3) * _tmp7); + _kz(18, 0) = + _tmp53 * (-P(18, 0) * _tmp5 + P(18, 1) * _tmp1 + P(18, 2) * _tmp3 - P(18, 3) * _tmp7); + _kz(19, 0) = + _tmp53 * (-P(19, 0) * _tmp5 + P(19, 1) * _tmp1 + P(19, 2) * _tmp3 - P(19, 3) * _tmp7); + _kz(20, 0) = + _tmp53 * (-P(20, 0) * _tmp5 + P(20, 1) * _tmp1 + P(20, 2) * _tmp3 - P(20, 3) * _tmp7); + _kz(21, 0) = + _tmp53 * (-P(21, 0) * _tmp5 + P(21, 1) * _tmp1 + P(21, 2) * _tmp3 - P(21, 3) * _tmp7); + _kz(22, 0) = + _tmp53 * (-P(22, 0) * _tmp5 + P(22, 1) * _tmp1 + P(22, 2) * _tmp3 - P(22, 3) * _tmp7); + _kz(23, 0) = + _tmp53 * (-P(23, 0) * _tmp5 + P(23, 1) * _tmp1 + P(23, 2) * _tmp3 - P(23, 3) * _tmp7); + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 7d9cb1f86e..42b0cfaef3 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -131,6 +131,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_eva_noise(_params->ev_att_noise), _param_ekf2_evv_gate(_params->ev_vel_innov_gate), _param_ekf2_evp_gate(_params->ev_pos_innov_gate), + _param_ekf2_grav_noise(_params->gravity_noise), _param_ekf2_of_n_min(_params->flow_noise), _param_ekf2_of_n_max(_params->flow_noise_qual_min), _param_ekf2_of_qmin(_params->flow_qual_min), @@ -804,6 +805,9 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) // mag 3d PublishAidSourceStatus(_ekf.aid_src_mag(), _status_mag_pub_last, _estimator_aid_src_mag_pub); + // gravity + PublishAidSourceStatus(_ekf.aid_src_gravity(), _status_gravity_pub_last, _estimator_aid_src_gravity_pub); + // aux velocity PublishAidSourceStatus(_ekf.aid_src_aux_vel(), _status_aux_vel_pub_last, _estimator_aid_src_aux_vel_pub); @@ -1098,6 +1102,7 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) _ekf.getHaglInnov(innovations.hagl); _ekf.getHaglRateInnov(innovations.hagl_rate); _ekf.getTerrainFlowInnov(innovations.terr_flow); + _ekf.getGravityInnov(innovations.gravity); // Not yet supported innovations.aux_vvel = NAN; @@ -1148,6 +1153,7 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) _ekf.getHaglInnovRatio(test_ratios.hagl); _ekf.getHaglRateInnovRatio(test_ratios.hagl_rate); _ekf.getTerrainFlowInnovRatio(test_ratios.terr_flow[0]); + _ekf.getGravityInnovRatio(test_ratios.gravity[0]); // Not yet supported test_ratios.aux_vvel = NAN; @@ -1174,6 +1180,7 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp) _ekf.getHaglInnovVar(variances.hagl); _ekf.getHaglRateInnovVar(variances.hagl_rate); _ekf.getTerrainFlowInnovVar(variances.terr_flow); + _ekf.getGravityInnovVar(variances.gravity); // Not yet supported variances.aux_vvel = NAN; @@ -1507,6 +1514,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.cs_rng_kin_consistent = _ekf.control_status_flags().rng_kin_consistent; status_flags.cs_fake_pos = _ekf.control_status_flags().fake_pos; status_flags.cs_fake_hgt = _ekf.control_status_flags().fake_hgt; + status_flags.cs_gravity_vector = _ekf.control_status_flags().gravity_vector; status_flags.fault_status_changes = _filter_fault_status_changes; status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x; diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 731999ba55..e7159fcdf6 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -285,6 +285,8 @@ private: hrt_abstime _status_mag_pub_last{0}; hrt_abstime _status_mag_heading_pub_last{0}; + hrt_abstime _status_gravity_pub_last{0}; + hrt_abstime _status_aux_vel_pub_last{0}; hrt_abstime _status_optical_flow_pub_last{0}; @@ -376,6 +378,8 @@ private: uORB::PublicationMulti _estimator_aid_src_mag_heading_pub{ORB_ID(estimator_aid_src_mag_heading)}; uORB::PublicationMulti _estimator_aid_src_mag_pub{ORB_ID(estimator_aid_src_mag)}; + uORB::PublicationMulti _estimator_aid_src_gravity_pub{ORB_ID(estimator_aid_src_gravity)}; + uORB::PublicationMulti _estimator_aid_src_aux_vel_pub{ORB_ID(estimator_aid_src_aux_vel)}; uORB::PublicationMulti _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)}; @@ -540,6 +544,9 @@ private: (ParamExtFloat) _param_ekf2_evp_gate, ///< external vision position innovation consistency gate size (STD) + (ParamExtFloat) + _param_ekf2_grav_noise, ///< default accelerometer noise for gravity fusion measurements (m/s**2) + // optical flow fusion (ParamExtFloat) _param_ekf2_of_n_min, ///< best quality observation noise for optical flow LOS rate measurements (rad/sec) diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 271fb907d8..c383059ba8 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -57,11 +57,12 @@ PARAM_DEFINE_INT32(EKF2_PREDICT_US, 10000); * * @group EKF2 * @min 0 - * @max 3 + * @max 7 * @bit 0 Gyro Bias * @bit 1 Accel Bias + * @bit 2 Gravity vector fusion */ -PARAM_DEFINE_INT32(EKF2_IMU_CTRL, 3); +PARAM_DEFINE_INT32(EKF2_IMU_CTRL, 7); /** * Magnetometer measurement delay relative to IMU measurements @@ -854,6 +855,17 @@ PARAM_DEFINE_FLOAT(EKF2_EVV_NOISE, 0.1f); */ PARAM_DEFINE_FLOAT(EKF2_EVA_NOISE, 0.1f); +/** + * Accelerometer measurement noise for gravity based observations. + * + * @group EKF2 + * @min 0.1 + * @max 10.0 + * @unit m/s^2 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_GRAV_NOISE, 1.0f); + /** * Measurement noise for the optical flow sensor when it's reported quality metric is at the maximum * diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index eecf97bc93..e46c7c8dd0 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -187,6 +187,7 @@ void LoggedTopics::add_default_topics() // add_optional_topic_multi("estimator_aid_src_ev_pos", 100, MAX_ESTIMATOR_INSTANCES); // add_optional_topic_multi("estimator_aid_src_ev_vel", 100, MAX_ESTIMATOR_INSTANCES); // add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES); + // add_optional_topic_multi("estimator_aid_src_gravity", 100, MAX_ESTIMATOR_INSTANCES); // add_optional_topic_multi("estimator_aid_src_rng_hgt", 100, MAX_ESTIMATOR_INSTANCES); // add_optional_topic_multi("estimator_aid_src_fake_hgt", 100, MAX_ESTIMATOR_INSTANCES); // add_optional_topic_multi("estimator_aid_src_fake_pos", 100, MAX_ESTIMATOR_INSTANCES);