From ff8f03b5ddffebd94ab7b66b5f15727440c8da5e Mon Sep 17 00:00:00 2001 From: devbharat Date: Tue, 19 Apr 2016 13:51:50 +0200 Subject: [PATCH] Added compensation for VI sensor offset. Check sign. --- EKF/common.h | 2 ++ EKF/ekf.cpp | 7 +++++++ 2 files changed, 9 insertions(+) diff --git a/EKF/common.h b/EKF/common.h index 53c89f3c0f..f589b6277f 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -249,6 +249,7 @@ struct parameters { Vector3f gps_pos_body; // xyz position of the GPS antenna in body frame (m) Vector3f rng_pos_body; // xyz position of range sensor in body frame (m) Vector3f flow_pos_body; // xyz position of range sensor focal point in body frame (m) + Vector3f visn_pos_body; // xyz position of VI-sensor focal point in body frame (m) // output complementary filter tuning float vel_Tau; // velocity state correction time constant (1/sec) @@ -333,6 +334,7 @@ struct parameters { gps_pos_body = {}; rng_pos_body = {}; flow_pos_body = {}; + visn_pos_body = {}; // output complementary filter tuning time constants vel_Tau = 0.5f; diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 49df81540f..d7f9aa2e6e 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -309,6 +309,13 @@ bool Ekf::update() if (_control_status.flags.ev_pos) { _fuse_pos = true; _fuse_height = true; + + // correct position and height for offset relative to IMU + Vector3f pos_offset_body = _params.visn_pos_body - _params.imu_pos_body; + Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; + _ev_sample_delayed.posNED(0) -= pos_offset_earth(0); + _ev_sample_delayed.posNED(1) -= pos_offset_earth(1); + _ev_sample_delayed.posNED(2) -= pos_offset_earth(2); // + or - ? } // use external vision yaw observation if (_control_status.flags.ev_yaw) {