From 84dcb32bd8a339b2a5e118367b8a738b9d7e552f Mon Sep 17 00:00:00 2001 From: kamilritz Date: Mon, 9 Dec 2019 14:27:51 +0100 Subject: [PATCH] Extend auxVel interface to support 3d velocity --- EKF/common.h | 4 ++-- EKF/control.cpp | 10 ++++++---- EKF/estimator_interface.cpp | 6 +++--- EKF/estimator_interface.h | 2 +- 4 files changed, 12 insertions(+), 10 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 8fe107f047..4478c2c962 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -167,8 +167,8 @@ struct dragSample { }; struct auxVelSample { - Vector2f velNE; ///< measured NE velocity relative to the local origin (m/sec) - Vector2f velVarNE; ///< estimated error variance of the NE velocity (m/sec)**2 + Vector3f vel; ///< measured NE velocity relative to the local origin (m/sec) + Vector3f velVar; ///< estimated error variance of the NE velocity (m/sec)**2 uint64_t time_us; ///< timestamp of the measurement (uSec) }; diff --git a/EKF/control.cpp b/EKF/control.cpp index 415500e640..553bc9d608 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -1398,14 +1398,16 @@ void Ekf::controlAuxVelFusion() Vector2f aux_vel_innov_gate; Vector3f aux_vel_obs_var; - _aux_vel_innov(0) = _state.vel(0) - _auxvel_sample_delayed.velNE(0); - _aux_vel_innov(1) = _state.vel(1) - _auxvel_sample_delayed.velNE(1); + _aux_vel_innov = _state.vel - _auxvel_sample_delayed.vel; + aux_vel_obs_var = _auxvel_sample_delayed.velVar; aux_vel_innov_gate(0) = _params.auxvel_gate; - aux_vel_obs_var(0) = _auxvel_sample_delayed.velVarNE(0); - aux_vel_obs_var(1) = _auxvel_sample_delayed.velVarNE(1); fuseHorizontalVelocity(_aux_vel_innov, aux_vel_innov_gate, aux_vel_obs_var, _aux_vel_innov_var, _aux_vel_test_ratio); + // Can be enabled after bit for this is added to EKF_AID_MASK + // fuseVerticalVelocity(_aux_vel_innov, aux_vel_innov_gate, aux_vel_obs_var, + // _aux_vel_innov_var, _aux_vel_test_ratio); + } } diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 590c6e9bcc..792a8e7946 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -447,7 +447,7 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message } } -void EstimatorInterface::setAuxVelData(uint64_t time_usec, float (&data)[2], float (&variance)[2]) +void EstimatorInterface::setAuxVelData(uint64_t time_usec, const Vector3f &velocity, const Vector3f &variance) { if (!_initialised || _auxvel_buffer_fail) { return; @@ -473,8 +473,8 @@ void EstimatorInterface::setAuxVelData(uint64_t time_usec, float (&data)[2], flo auxvel_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2; _time_last_auxvel = time_usec; - auxvel_sample_new.velNE = Vector2f(data); - auxvel_sample_new.velVarNE = Vector2f(variance); + auxvel_sample_new.vel = velocity; + auxvel_sample_new.velVar = variance; _auxvel_buffer.push(auxvel_sample_new); } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 04c7d7dd80..d6f23509e0 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -203,7 +203,7 @@ public: void setExtVisionData(uint64_t time_usec, ext_vision_message *evdata); // set auxiliary velocity data - void setAuxVelData(uint64_t time_usec, float (&data)[2], float (&variance)[2]); + void setAuxVelData(uint64_t time_usec, const Vector3f &vel, const Vector3f &variance); // return a address to the parameters struct // in order to give access to the application