From 436f168188c6daabbb8d1091333278d8279d3672 Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Wed, 1 Feb 2023 10:30:55 -0500 Subject: [PATCH] EKF2: Added gravity innovations for logging. Signed-off-by: Daniel M. Sahu --- msg/EstimatorInnovations.msg | 1 + src/modules/ekf2/EKF/ekf.h | 2 ++ src/modules/ekf2/EKF2.cpp | 1 + 3 files changed, 4 insertions(+) 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/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 0a66108542..135791c041 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -191,6 +191,8 @@ 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); } + // get the state vector at the delayed time horizon matrix::Vector getStateAtFusionHorizonAsVector() const; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index a6b52477fb..39cd4bbd18 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1077,6 +1077,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); // Not yet supported innovations.aux_vvel = NAN;