fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-20 11:59:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added external vision noise parameters etc and position offset

Browse Source
This commit is contained in:
devbharat 2016-04-19 14:24:45 +02:00 committed by Paul Riseborough
parent ff8f03b5dd
commit b681c9a5d0
2 changed files with 9 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
EKF/common.h
View File

@ -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;
}
};

2
EKF/ekf.cpp
View File

@ -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);