From 8693ad15a7490aecef5d99a8c3dcb46fea914ab1 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 21 Mar 2022 11:24:26 +0100 Subject: [PATCH] ekf: add logging of range finder consistency check --- msg/estimator_innovations.msg | 1 + src/modules/ekf2/EKF/ekf.h | 4 ++++ src/modules/ekf2/EKF/range_finder_consistency_check.cpp | 4 ++++ src/modules/ekf2/EKF/range_finder_consistency_check.hpp | 4 ++++ src/modules/ekf2/EKF2.cpp | 3 +++ 5 files changed, 16 insertions(+) diff --git a/msg/estimator_innovations.msg b/msg/estimator_innovations.msg index cb3e85a1c4..56a741b37d 100644 --- a/msg/estimator_innovations.msg +++ b/msg/estimator_innovations.msg @@ -31,6 +31,7 @@ float32[2] drag # drag specific force innovation (m/sec**2) and innovation vari float32 airspeed # airspeed innovation (m/sec) and innovation variance ((m/sec)**2) float32 beta # synthetic sideslip innovation (rad) and innovation variance (rad**2) float32 hagl # height of ground innovation (m) and innovation variance (m**2) +float32 hagl_rate # height of ground rate innovation (m/s) and innovation variance ((m/s)**2) # The innovation test ratios are scalar values. In case the field is a vector, # the test ratio will be put in the first component of the vector. diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 07e308a4e9..b59488117d 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -128,6 +128,10 @@ public: void getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _hagl_innov_var; } void getHaglInnovRatio(float &hagl_innov_ratio) const { hagl_innov_ratio = _hagl_test_ratio; } + void getHaglRateInnov(float &hagl_rate_innov) const { hagl_rate_innov = _rng_consistency_check.getInnov(); } + 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(); } + // get the state vector at the delayed time horizon matrix::Vector getStateAtFusionHorizonAsVector() const; diff --git a/src/modules/ekf2/EKF/range_finder_consistency_check.cpp b/src/modules/ekf2/EKF/range_finder_consistency_check.cpp index a8a54151a2..f1e5b61056 100644 --- a/src/modules/ekf2/EKF/range_finder_consistency_check.cpp +++ b/src/modules/ekf2/EKF/range_finder_consistency_check.cpp @@ -63,6 +63,10 @@ void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_va _time_last_update_us = time_us; _dist_bottom_prev = dist_bottom; + + // Save for logging + _vel_bottom_innov = innov; + _vel_bottom_innov_var = innov_var; } void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us) diff --git a/src/modules/ekf2/EKF/range_finder_consistency_check.hpp b/src/modules/ekf2/EKF/range_finder_consistency_check.hpp index dc8cef91f9..792f54af2b 100644 --- a/src/modules/ekf2/EKF/range_finder_consistency_check.hpp +++ b/src/modules/ekf2/EKF/range_finder_consistency_check.hpp @@ -51,6 +51,8 @@ public: float getTestRatio() const { return _vel_bottom_test_ratio; } float getSignedTestRatioLpf() const { return _vel_bottom_signed_test_ratio_lpf.getState(); } + float getInnov() const { return _vel_bottom_innov; } + float getInnovVar() const { return _vel_bottom_innov_var; } bool isKinematicallyConsistent() const { return _is_kinematically_consistent; } private: @@ -61,6 +63,8 @@ private: float _vel_bottom_test_ratio{}; AlphaFilter _vel_bottom_signed_test_ratio_lpf{}; // average signed test ratio used to detect a bias in the data + float _vel_bottom_innov{}; + float _vel_bottom_innov_var{}; bool _is_kinematically_consistent{true}; uint64_t _time_last_inconsistent_us{}; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index b7c41fc343..206863f78d 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -838,6 +838,7 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) _ekf.getAirspeedInnov(innovations.airspeed); _ekf.getBetaInnov(innovations.beta); _ekf.getHaglInnov(innovations.hagl); + _ekf.getHaglRateInnov(innovations.hagl_rate); // Not yet supported innovations.aux_vvel = NAN; @@ -878,6 +879,7 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) _ekf.getAirspeedInnovRatio(test_ratios.airspeed); _ekf.getBetaInnovRatio(test_ratios.beta); _ekf.getHaglInnovRatio(test_ratios.hagl); + _ekf.getHaglRateInnovRatio(test_ratios.hagl_rate); // Not yet supported test_ratios.aux_vvel = NAN; @@ -902,6 +904,7 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp) _ekf.getAirspeedInnovVar(variances.airspeed); _ekf.getBetaInnovVar(variances.beta); _ekf.getHaglInnovVar(variances.hagl); + _ekf.getHaglRateInnovVar(variances.hagl_rate); // Not yet supported variances.aux_vvel = NAN;