From b681c9a5d0024898b7d61844eaa2ed48f2f7e44a Mon Sep 17 00:00:00 2001 From: devbharat Date: Tue, 19 Apr 2016 14:24:45 +0200 Subject: [PATCH] Added external vision noise parameters etc and position offset --- EKF/common.h | 10 ++++++++-- EKF/ekf.cpp | 2 +- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index f589b6277f..35d2966704 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -226,6 +226,11 @@ struct parameters { float range_innov_gate; // range finder fusion innovation consistency gate size (STD) float rng_gnd_clearance; // minimum valid value for range when on ground (m) + // vision position fusion + float ev_noise; // observation noise for vision sensor estimates (m) + float ev_innov_gate; // vision estimator fusion innovation consistency gate size (STD) + float ev_gnd_clearance; // minimum valid value for vision based height estimate when on ground (m) + // optical flow fusion float flow_noise; // observation noise for optical flow LOS rate measurements (rad/sec) float flow_noise_qual_min; // observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec) @@ -249,7 +254,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) + Vector3f ev_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) @@ -334,11 +339,12 @@ struct parameters { gps_pos_body = {}; rng_pos_body = {}; flow_pos_body = {}; - visn_pos_body = {}; + ev_pos_body = {}; // output complementary filter tuning time constants vel_Tau = 0.5f; pos_Tau = 0.25f; + } }; diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index d7f9aa2e6e..3e93b58a3e 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -311,7 +311,7 @@ bool Ekf::update() _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_body = _params.ev_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);