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

EKF: improve detection of bad vert accel data

Browse Source 
Improve ability to detect if bad vertical accel data has caused loss of height accuracy by using historical data.
This commit is contained in:
Paul Riseborough 2016-04-22 09:24:04 +10:00
parent 469a7d1711
commit 874558d194
3 changed files with 23 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
EKF/control.cpp
View File

@ -170,6 +170,20 @@ void Ekf::controlFusionModes()
* measurement source, continue using it after the reset and declare the current
* source failed if we have switched.
*/
// check for inertial sensing errors as evidenced by the vertical innovations having the same sign and not stale
bool bad_vert_accel = (_control_status.flags.baro_hgt && // we can only run this check if vertical position and velocity observations are indepedant
(_vel_pos_innov[5] * _vel_pos_innov[2] > 0.0f) && // vertical position and velocity sensors are in agreement
((_imu_sample_delayed.time_us - _baro_sample_delayed.time_us) < 2 * BARO_MAX_INTERVAL) && // vertical position data is fresh
((_imu_sample_delayed.time_us - _gps_sample_delayed.time_us) < 2 * GPS_MAX_INTERVAL) && // vertical velocity data is freshs
_vel_pos_test_ratio[2] > 1.0f && // vertical velocty innovations have failed innovation consistency checks
_vel_pos_test_ratio[5] > 1.0f); // vertical position innovations have failed innovation consistency checks
// record time of last bad vert accel
if (bad_vert_accel) {
_time_bad_vert_accel = _time_last_imu;
}
if ((P[8][8] > sq(_params.hgt_reset_lim)) && ((_time_last_imu - _time_last_hgt_fuse) > 5e6)) {
// handle the case where we are using baro for height
if (_control_status.flags.baro_hgt) {
@ -180,16 +194,14 @@ void Ekf::controlFusionModes()
baroSample baro_init = _baro_buffer.get_newest();
bool baro_hgt_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
// check for inertial sensing errors as evidenced by the vertical innovations having the same sign and not stale
bool bad_imu = ((_vel_pos_innov[5] * _vel_pos_innov[2] > 0.0f) &&
((_imu_sample_delayed.time_us - _baro_sample_delayed.time_us) < 2 * BARO_MAX_INTERVAL) &&
((_imu_sample_delayed.time_us - _gps_sample_delayed.time_us) < 2 * GPS_MAX_INTERVAL));
// check for inertial sensing errors in the last 10 seconds
bool prev_bad_vert_accel = (_time_last_imu - _time_bad_vert_accel < 10E6);
// continue to use baro if it is available and we have detected bad IMU data or inadequate GPS
// switch to GPS if GPS data is available or we do not have Baro
if (baro_hgt_available && (bad_imu || !gps_hgt_available || !gps_hgt_accurate)) {
if (baro_hgt_available && (prev_bad_vert_accel || !gps_hgt_available || !gps_hgt_accurate)) {
printf("EKF baro hgt timeout - reset to baro\n");
} else if (gps_hgt_available && !bad_imu) {
} else if (gps_hgt_available && !prev_bad_vert_accel) {
// declare the baro as unhealthy
_baro_hgt_faulty = true;
// set the height mode to the GPS

3
EKF/ekf.cpp
View File

@ -89,7 +89,8 @@ Ekf::Ekf():
_vert_pos_reset_delta(0.0f),
_time_vert_pos_reset(0),
_vert_vel_reset_delta(0.0f),
_time_vert_vel_reset(0)
_time_vert_vel_reset(0),
_time_bad_vert_accel(0)
{
_state = {};
_last_known_posNE.setZero();

3
EKF/ekf.h
View File

@ -240,6 +240,9 @@ private:
float _vert_vel_reset_delta; // increase in vertical position velocity at the last reset(m)
uint64_t _time_vert_vel_reset; // last system time in usec that the vertical velocity state was reset
// imu fault status
uint64_t _time_bad_vert_accel; // last time a bad vertical accel was detected (usec)
// update the real time complementary filter states. This includes the prediction
// and the correction step
void calculateOutputStates();