fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-12 10:17:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: use flow for vel test ratio if only active source of horizontal aiding

Browse Source
This commit is contained in:
Daniel Agar
2021-03-12 17:10:07 -05:00
committed by Mathieu Bresciani
parent bb950a1550
commit e3d1ade660
2 changed files with 17 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+16 -5
View File
@@ -927,13 +927,22 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
{
// return the integer bitmask containing the consistency check pass/fail status
status = _innov_check_fail_status.value;
// return the largest magnetometer innovation test ratio
mag = sqrtf(math::max(_yaw_test_ratio, _mag_test_ratio.max()));
// return the largest velocity innovation test ratio
vel = math::max(sqrtf(math::max(_gps_vel_test_ratio(0), _gps_vel_test_ratio(1))),
sqrtf(math::max(_ev_vel_test_ratio(0), _ev_vel_test_ratio(1))));
// return the largest position innovation test ratio
pos = math::max(sqrtf(_gps_pos_test_ratio(0)), sqrtf(_ev_pos_test_ratio(0)));
// return the largest velocity and position innovation test ratio
if (_control_status.flags.gps) {
vel = sqrtf(math::max(_gps_vel_test_ratio(0), _gps_vel_test_ratio(1)));
pos = sqrtf(_gps_pos_test_ratio(0));
}
if (_control_status.flags.ev_vel) {
vel = math::max(vel, sqrtf(math::max(_ev_vel_test_ratio(0), _ev_vel_test_ratio(1))));
pos = math::max(pos, sqrtf(_ev_pos_test_ratio(0)));
}
if (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)) {
vel = sqrtf(_optflow_test_ratio);
}
// return the vertical position innovation test ratio
if (_control_status.flags.baro_hgt) {
@@ -951,8 +960,10 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
// return the airspeed fusion innovation test ratio
tas = sqrtf(_tas_test_ratio);
// return the terrain height innovation test ratio
hagl = sqrtf(_hagl_test_ratio);
// return the synthetic sideslip innovation test ratio
beta = sqrtf(_beta_test_ratio);
}
EKF/estimator_interface.h
+1 -1
View File
@@ -321,7 +321,7 @@ protected:
Vector2f _gps_pos_test_ratio; // GPS position innovation consistency check ratios
Vector2f _ev_vel_test_ratio; // EV velocity innovation consistency check ratios
Vector2f _ev_pos_test_ratio ; // EV position innovation consistency check ratios
Vector2f _aux_vel_test_ratio; // Auxiliray horizontal velocity innovation consistency check ratio
Vector2f _aux_vel_test_ratio; // Auxiliary horizontal velocity innovation consistency check ratio
Vector2f _baro_hgt_test_ratio; // baro height innovation consistency check ratios
Vector2f _rng_hgt_test_ratio; // range finder height innovation consistency check ratios
float _optflow_test_ratio{}; // Optical flow innovation consistency check ratio