From 4a111d679dba9aa08534d9fb275013f9d748029c Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 3 Feb 2023 11:17:10 -0500 Subject: [PATCH] ekf2: gravity fusion updates - only fuse gravity if no horizontal aiding source is enabled - add estimator_aid_src_gravity for inspection and logging - estimator_aid_src_gravity is always updated and published, even if not fused - add innovation gate with conservative default (could be made configurable) - use new EKF2_IMU_CTRL parameter to enable/disable gravity vector fusion --- msg/EstimatorAidSource3d.msg | 4 +- msg/EstimatorStatusFlags.msg | 1 + src/modules/ekf2/EKF/CMakeLists.txt | 1 + src/modules/ekf2/EKF/common.h | 8 ++-- src/modules/ekf2/EKF/control.cpp | 11 +---- src/modules/ekf2/EKF/ekf.h | 16 ++++--- src/modules/ekf2/EKF/gravity_fusion.cpp | 62 +++++++++++++++++-------- src/modules/ekf2/EKF2.cpp | 8 +++- src/modules/ekf2/EKF2.hpp | 4 ++ src/modules/ekf2/ekf2_params.c | 5 +- src/modules/logger/logged_topics.cpp | 1 + 11 files changed, 75 insertions(+), 46 deletions(-) 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/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/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 48a19ac095..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 { @@ -394,7 +395,7 @@ struct parameters { 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) + 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) @@ -567,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 500f7ce273..bb40ef49df 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -198,7 +198,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) controlBetaFusion(imu_delayed); controlDragFusion(); controlHeightFusion(imu_delayed); - controlGravityFusion(); + controlGravityFusion(imu_delayed); // Additional data odometry data from an external estimator can be fused. controlExternalVisionFusion(); @@ -429,12 +429,3 @@ 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 bfb002482a..a992d63480 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -191,7 +191,9 @@ 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 getGravInnov(float grav_innov[3]) const { _gravity_innov.copyTo(grav_innov); } + 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; @@ -435,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; } @@ -543,8 +547,6 @@ 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) @@ -577,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{}; @@ -931,10 +935,8 @@ private: void updateGroundEffect(); - // gravity fusion - void fuseGravity(); - // heuristically enable / disable gravity fusion - void controlGravityFusion(); + // 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 index 58bf5ef9f0..8a09b72f79 100644 --- a/src/modules/ekf2/EKF/gravity_fusion.cpp +++ b/src/modules/ekf2/EKF/gravity_fusion.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 Estimation and Control Library (ECL). All rights reserved. + * 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 @@ -35,7 +35,7 @@ * @file gravity_fusion.cpp * Fuse observations from the gravity vector to constrain roll * and pitch (a la complementary filter). - * + * * @author Daniel M. Sahu */ @@ -44,30 +44,52 @@ #include -void Ekf::fuseGravity() +void Ekf::controlGravityFusion(const imuSample &imu) { - // 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); + // fuse gravity observation if our overall acceleration isn't too big + const float gravity_scale = _accel_vec_filt.norm() / CONSTANTS_ONE_G; - // initialize fusion variables - Vector24f Kx, Ky, Kz; // Kalman gain vectors - Vector3f innovation_variance; + _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, acc_measurement_noise, FLT_EPSILON, - &_gravity_innov, &innovation_variance, &Kx, &Ky, &Kz); + getStateAtFusionHorizonAsVector(), P, measurement, measurement_var, FLT_EPSILON, + &innovation, &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."); + // 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; } - 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."); + + 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/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index c07d44f8ce..42b0cfaef3 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -805,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); @@ -1099,7 +1102,7 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) _ekf.getHaglInnov(innovations.hagl); _ekf.getHaglRateInnov(innovations.hagl_rate); _ekf.getTerrainFlowInnov(innovations.terr_flow); - _ekf.getGravInnov(innovations.gravity); + _ekf.getGravityInnov(innovations.gravity); // Not yet supported innovations.aux_vvel = NAN; @@ -1150,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; @@ -1176,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; @@ -1509,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 57be3660fa..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)}; diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index ec4134e3a3..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 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);