From c6e5d516f9250c5edd1af14df7ec5e1417353ed5 Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Wed, 1 Feb 2023 10:01:20 -0500 Subject: [PATCH] EKF2: Adding gravity fusion observation hooks into the EKF. Signed-off-by: Daniel M. Sahu --- src/modules/ekf2/EKF/common.h | 3 ++ src/modules/ekf2/EKF/control.cpp | 10 ++++ src/modules/ekf2/EKF/ekf.h | 7 +++ src/modules/ekf2/EKF/gravity_fusion.cpp | 70 +++++++++++++++++++++++++ src/modules/ekf2/EKF2.cpp | 1 + src/modules/ekf2/EKF2.hpp | 3 ++ src/modules/ekf2/ekf2_params.c | 11 ++++ 7 files changed, 105 insertions(+) create mode 100644 src/modules/ekf2/EKF/gravity_fusion.cpp diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 949f4ef042..a3c4bd34fc 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -386,6 +386,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) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 460f4c8371..500f7ce273 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(); // Additional data odometry data from an external estimator can be fused. controlExternalVisionFusion(); @@ -428,3 +429,12 @@ void Ekf::controlAuxVelFusion() } } } + +void Ekf::controlGravityFusion() +{ + // fuse gravity observation if our overall acceleration isn't too big + const float gravity_scale = _accel_vec_filt.norm() / CONSTANTS_ONE_G; + if (gravity_scale >= 0.9f && gravity_scale <= 1.1f) { + fuseGravity(); + } +} diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index e79eea97e6..0a66108542 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -540,6 +540,8 @@ private: float _hagl_innov{0.0f}; ///< innovation of the last height above terrain measurement (m) float _hagl_innov_var{0.0f}; ///< innovation variance for the last height above terrain measurement (m**2) + Vector3f _gravity_innov{}; ///< innovation of the last gravity fusion observation (m/s**2) + // optical flow processing Vector3f _flow_gyro_bias{}; ///< bias errors in optical flow sensor rate gyro outputs (rad/sec) Vector2f _flow_vel_body{}; ///< velocity from corrected flow measurement (body frame)(m/s) @@ -918,6 +920,11 @@ private: void updateGroundEffect(); + // gravity fusion + void fuseGravity(); + // heuristically enable / disable gravity fusion + void controlGravityFusion(); + // 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..52d5db5be3 --- /dev/null +++ b/src/modules/ekf2/EKF/gravity_fusion.cpp @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (c) 2018 Estimation and Control Library (ECL). 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::fuseGravity() +{ + // get raw accelerometer reading at delayed horizon and expected measurement noise (gaussian) + const imuSample imu = get_imu_sample_delayed(); + const Vector3f measurement = imu.delta_vel / imu.delta_vel_dt - getAccelBias(); + const float acc_measurement_noise = sq(_params.gravity_noise); + + // initialize fusion variables + Vector24f Kx, Ky, Kz; // Kalman gain vectors + Vector3f innovation_variance; + + // calculate kalman gains and innovation variances + sym::ComputyGravityInnovVarAndKAndH( + getStateAtFusionHorizonAsVector(), P, measurement, acc_measurement_noise, FLT_EPSILON, + &_gravity_innov, &innovation_variance, &Kx, &Ky, &Kz); + + // perform fusion for each axis + if (!measurementUpdate(Kx, innovation_variance(0), _gravity_innov(0))) + PX4_ERR("Gravity fusion (X axis) unsuccessful."); + if (!measurementUpdate(Ky, innovation_variance(1), _gravity_innov(1))) + PX4_ERR("Gravity fusion (y axis) unsuccessful."); + if (!measurementUpdate(Kz, innovation_variance(2), _gravity_innov(2))) + PX4_ERR("Gravity fusion (Z axis) unsuccessful."); +} diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 7e90b56d93..a6b52477fb 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -130,6 +130,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), diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 5dc81c51f0..aa8d4bd0f2 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -538,6 +538,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 705871ce98..482df4c1e6 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -843,6 +843,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 *