From 149233a9ab73e5ed08379a835a565f7cf4755613 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Tue, 17 Sep 2019 08:58:59 +0200 Subject: [PATCH] Add velocity observations to external vision interface --- EKF/common.h | 12 ++++++++---- EKF/estimator_interface.cpp | 2 ++ 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index dc3a73e53b..c559441f8f 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -80,10 +80,12 @@ struct flow_message { }; struct ext_vision_message { - Vector3f posNED; ///< measured NED position relative to the local origin (m) - Quatf quat; ///< measured quaternion orientation defining rotation from NED to body frame + Vector3f posNED; ///< XYZ position in earth frame (m) - Z must be aligned with down axis + Vector3f velNED; ///< XYZ velocity in earth frame (m/sec) - Z must be aligned with down axis + Quatf quat; ///< quaternion defining rotation from body to earth frame float posErr; ///< 1-Sigma horizontal position accuracy (m) float hgtErr; ///< 1-Sigma height accuracy (m) + float velErr; ///< 1-Sigma velocity accuracy (m/sec) float angErr; ///< 1-Sigma angular error (rad) }; @@ -151,10 +153,12 @@ struct flowSample { }; struct extVisionSample { - Vector3f posNED; ///< measured NED position relative to the local origin (m) - Quatf quat; ///< measured quaternion orientation defining rotation from NED to body frame + Vector3f posNED; ///< XYZ position in earth frame (m) - Z must be aligned with down axis + Vector3f velNED; ///< XYZ velocity in earth frame (m/sec) - Z must be aligned with down axis + Quatf quat; ///< quaternion defining rotation from body to earth frame float posErr; ///< 1-Sigma horizontal position accuracy (m) float hgtErr; ///< 1-Sigma height accuracy (m) + float velErr; ///< 1-Sigma velocity accuracy (m/sec) float angErr; ///< 1-Sigma angular error (rad) uint64_t time_us; ///< timestamp of the measurement (uSec) }; diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 48c7782a25..2aad6881ba 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -442,9 +442,11 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message // copy required data ev_sample_new.angErr = evdata->angErr; ev_sample_new.posErr = evdata->posErr; + ev_sample_new.velErr = evdata->velErr; ev_sample_new.hgtErr = evdata->hgtErr; ev_sample_new.quat = evdata->quat; ev_sample_new.posNED = evdata->posNED; + ev_sample_new.velNED = evdata->velNED; // record time for comparison next measurement _time_last_ext_vision = time_usec;