From 7c2798269cc360ab4a39532fd2cc351fdf141faa Mon Sep 17 00:00:00 2001 From: James Goppert Date: Mon, 3 Oct 2016 02:28:46 -0400 Subject: [PATCH] Added vision delay compensation to LPE. (#5585) --- .../BlockLocalPositionEstimator.cpp | 24 +++++++++++++++++++ .../BlockLocalPositionEstimator.hpp | 6 +++-- src/modules/local_position_estimator/params.c | 11 +++++++++ .../local_position_estimator/sensors/gps.cpp | 20 +++------------- .../sensors/vision.cpp | 9 ++++++- 5 files changed, 50 insertions(+), 20 deletions(-) diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 5055b7313d..89616b2bf7 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -77,6 +77,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _gps_epv_max(this, "EPV_MAX"), _vision_xy_stddev(this, "VIS_XY"), _vision_z_stddev(this, "VIS_Z"), + _vision_delay(this, "VIS_DELAY"), _vision_on(this, "VIS_ON"), _mocap_p_stddev(this, "VIC_P"), _flow_gyro_comp(this, "FLW_GYRO_CMP"), @@ -913,3 +914,26 @@ void BlockLocalPositionEstimator::predict() _xLowPass.update(_x); _aglLowPass.update(agl()); } + +int BlockLocalPositionEstimator::getDelayPeriods(float delay, uint8_t *periods) +{ + float t_delay = 0; + uint8_t i_hist = 0; + + for (i_hist = 1; i_hist < HIST_LEN; i_hist++) { + t_delay = 1.0e-6f * (_timeStamp - _tDelay.get(i_hist)(0, 0)); + + if (t_delay > delay) { + break; + } + } + + *periods = i_hist; + + if (t_delay > DELAY_MAX) { + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] delayed data too old: %8.4f", double(t_delay)); + return -1; + } + + return OK; +} diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 3d90d0fb0f..9fd0ecfcb1 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -33,10 +33,10 @@ using namespace matrix; using namespace control; -static const float GPS_DELAY_MAX = 0.5f; // seconds +static const float DELAY_MAX = 0.5f; // seconds static const float HIST_STEP = 0.05f; // 20 hz static const float BIAS_MAX = 1e-1f; -static const size_t HIST_LEN = 10; // GPS_DELAY_MAX / HIST_STEP; +static const size_t HIST_LEN = 10; // DELAY_MAX / HIST_STEP; static const size_t N_DIST_SUBS = 4; enum fault_t { @@ -207,6 +207,7 @@ private: void correctionLogic(Vector &dx); void covPropagationLogic(Matrix &dP); void detectDistanceSensors(); + int getDelayPeriods(float delay, uint8_t *periods); // publications void publishLocalPos(); @@ -276,6 +277,7 @@ private: // vision parameters BlockParamFloat _vision_xy_stddev; BlockParamFloat _vision_z_stddev; + BlockParamFloat _vision_delay; BlockParamInt _vision_on; // mocap parameters diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c index 500c7a2894..3e7833ac3e 100644 --- a/src/modules/local_position_estimator/params.c +++ b/src/modules/local_position_estimator/params.c @@ -245,6 +245,17 @@ PARAM_DEFINE_FLOAT(LPE_EPH_MAX, 3.0f); */ PARAM_DEFINE_FLOAT(LPE_EPV_MAX, 5.0f); +/** + * Vision delay compensaton + * + * @group Local Position Estimator + * @unit sec + * @min 0 + * @max 0.1 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(LPE_VIS_DELAY, 0.1f); + /** * Vision xy standard deviation. * diff --git a/src/modules/local_position_estimator/sensors/gps.cpp b/src/modules/local_position_estimator/sensors/gps.cpp index 4a538539cb..64dae7c6ab 100644 --- a/src/modules/local_position_estimator/sensors/gps.cpp +++ b/src/modules/local_position_estimator/sensors/gps.cpp @@ -166,24 +166,10 @@ void BlockLocalPositionEstimator::gpsCorrect() R(4, 4) = var_vxy; R(5, 5) = var_vz; - // get delayed x and P - float t_delay = 0; - int i_hist = 0; + // get delayed x + uint8_t i_hist = 0; - for (i_hist = 1; i_hist < HIST_LEN; i_hist++) { - t_delay = 1.0e-6f * (_timeStamp - _tDelay.get(i_hist)(0, 0)); - - if (t_delay > _gps_delay.get()) { - break; - } - } - - // if you are 3 steps past the delay you wanted, this - // data is probably too old to use - if (t_delay > GPS_DELAY_MAX) { - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps delayed data too old: %8.4f", double(t_delay)); - return; - } + if (getDelayPeriods(_gps_delay.get(), &i_hist) < 0) { return; } Vector x0 = _xDelay.get(i_hist); diff --git a/src/modules/local_position_estimator/sensors/vision.cpp b/src/modules/local_position_estimator/sensors/vision.cpp index f2350fd606..694f2bac79 100644 --- a/src/modules/local_position_estimator/sensors/vision.cpp +++ b/src/modules/local_position_estimator/sensors/vision.cpp @@ -76,9 +76,16 @@ void BlockLocalPositionEstimator::visionCorrect() R(Y_vision_y, Y_vision_y) = _vision_xy_stddev.get() * _vision_xy_stddev.get(); R(Y_vision_z, Y_vision_z) = _vision_z_stddev.get() * _vision_z_stddev.get(); + // vision delayed x + uint8_t i_hist = 0; + + if (getDelayPeriods(_vision_delay.get(), &i_hist) < 0) { return; } + + Vector x0 = _xDelay.get(i_hist); + // residual Matrix S_I = inv((C * _P * C.transpose()) + R); - Matrix r = y - C * _x; + Matrix r = y - C * x0; // fault detection float beta = (r.transpose() * (S_I * r))(0, 0);