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

EKF: capture innovation checks and reset events in separate variables

Browse Source 
rename the innovation check status class variable and remove the reset flags from it.
This commit is contained in:
Paul Riseborough 2016-05-23 17:33:48 +10:00
parent 79705da7e6
commit 65da9173b9
8 changed files with 22 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
EKF/airspeed_fusion.cpp
View File

@ -138,11 +138,11 @@ void Ekf::fuseAirspeed()
// If the innovation consistency check fails then don't fuse the sample and indicate bad airspeed health
if (_tas_test_ratio > 1.0f) {
_sensor_fault_status.flags.reject_airspeed = true;
_innov_check_fail_status.flags.reject_airspeed = true;
return;
}
else {
_sensor_fault_status.flags.reject_airspeed = false;
_innov_check_fail_status.flags.reject_airspeed = false;
}
// Airspeed measurement sample has passed check so record it

11
EKF/common.h
View File

@ -388,8 +388,8 @@ union fault_status_u {
};
// define structure used to communicate innovation test failures and state resets
union sensor_fault_status_u {
// define structure used to communicate innovation test failures
union innovation_fault_status_u {
struct {
bool reject_vel_NED: 1; // 0 - true if velocity observations have been rejected
bool reject_pos_NE: 1; // 1 - true if horizontal position observations have been rejected
@ -402,13 +402,8 @@ union sensor_fault_status_u {
bool reject_hagl: 1; // 8 - true if the height above ground observation has been rejected
bool reject_optflow_X: 1; // 9 - true if the X optical flow observation has been rejected
bool reject_optflow_Y: 1; // 10 - true if the Y optical flow observation has been rejected
bool reset_pos_NE: 1; // 11 - true if the horizontal positoin has been reset
bool reset_pos_D: 1; // 12 - true if the vertical position has been reset
bool reset_vel_NE: 1; // 13 - true if the horizontal velocity has been reset
bool reset_vel_D: 1; // 14 - true if the vertical velocity has been reset
bool reset_yaw: 1; // 15 - true if teh yaw angle has been reset
} flags;
uint32_t value;
uint16_t value;
};

2
EKF/ekf.cpp
View File

@ -180,7 +180,7 @@ bool Ekf::init(uint64_t timestamp)
_dt_ekf_avg = 0.001f * (float)(FILTER_UPDATE_PERIOD_MS);
_fault_status.value = 0;
_sensor_fault_status.value = 0;
_innov_check_fail_status.value = 0;
return ret;
}

3
EKF/estimator_interface.h
View File

@ -259,10 +259,12 @@ protected:
float _gps_origin_epv; // vertical position uncertainty of the GPS origin
struct map_projection_reference_s _pos_ref; // Contains WGS-84 position latitude and longitude (radians)
// innovation consistency check monitoring ratios
float _yaw_test_ratio; // yaw innovation consistency check ratio
float _mag_test_ratio[3]; // magnetometer XYZ innovation consistency check ratios
float _vel_pos_test_ratio[6]; // velocity and position innovation consistency check ratios
float _tas_test_ratio; // tas innovation consistency check ratio
innovation_fault_status_u _innov_check_fail_status;
// data buffer instances
RingBuffer<imuSample> _imu_buffer;
@ -285,7 +287,6 @@ protected:
uint64_t _time_last_optflow;
fault_status_u _fault_status;
sensor_fault_status_u _sensor_fault_status;
// allocate data buffers and intialise interface variables
bool initialise_interface(uint64_t timestamp);

8
EKF/mag_fusion.cpp
View File

@ -182,9 +182,9 @@ void Ekf::fuseMag()
if (_mag_test_ratio[index] > 1.0f) {
mag_fail = false;
_sensor_fault_status.value |= (1 << (index + 3));
_innov_check_fail_status.value |= (1 << (index + 3));
} else {
_sensor_fault_status.value &= !(1 << (index + 3));
_innov_check_fail_status.value &= !(1 << (index + 3));
}
}
@ -605,7 +605,7 @@ void Ekf::fuseHeading()
// set the magnetometer unhealthy if the test fails
if (_yaw_test_ratio > 1.0f) {
_sensor_fault_status.flags.reject_yaw = true;
_innov_check_fail_status.flags.reject_yaw = true;
// if we are in air we don't want to fuse the measurement
// we allow to use it when on the ground because the large innovation could be caused
@ -620,7 +620,7 @@ void Ekf::fuseHeading()
}
} else {
_sensor_fault_status.flags.reject_yaw = false;
_innov_check_fail_status.flags.reject_yaw = false;
}
// apply covariance correction via P_new = (I -K*H)*P

4
EKF/optflow_fusion.cpp
View File

@ -407,10 +407,10 @@ void Ekf::fuseOptFlow()
for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
if (optflow_test_ratio[obs_index] > 1.0f) {
flow_fail = true;
_sensor_fault_status.value |= (1 << (obs_index + 9));
_innov_check_fail_status.value |= (1 << (obs_index + 9));
} else {
_sensor_fault_status.value &= ~(1 << (obs_index + 9));
_innov_check_fail_status.value &= ~(1 << (obs_index + 9));
}
}

4
EKF/terrain_estimator.cpp
View File

@ -117,10 +117,10 @@ void Ekf::fuseHagl()
_terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f);
// record last successful fusion event
_time_last_hagl_fuse = _time_last_imu;
_sensor_fault_status.flags.reject_hagl = false;
_innov_check_fail_status.flags.reject_hagl = false;
} else {
_sensor_fault_status.flags.reject_hagl = true;
_innov_check_fail_status.flags.reject_hagl = true;
}

12
EKF/vel_pos_fusion.cpp
View File

@ -204,25 +204,25 @@ void Ekf::fuseVelPosHeight()
// record the successful velocity fusion event
if (vel_check_pass && _fuse_hor_vel) {
_time_last_vel_fuse = _time_last_imu;
_sensor_fault_status.flags.reject_vel_NED = false;
_innov_check_fail_status.flags.reject_vel_NED = false;
} else if (!vel_check_pass) {
_sensor_fault_status.flags.reject_vel_NED = true;
_innov_check_fail_status.flags.reject_vel_NED = true;
}
// record the successful position fusion event
if (pos_check_pass && _fuse_pos) {
_time_last_pos_fuse = _time_last_imu;
_sensor_fault_status.flags.reject_pos_NE = false;
_innov_check_fail_status.flags.reject_pos_NE = false;
} else if (!pos_check_pass) {
_sensor_fault_status.flags.reject_pos_NE = true;
_innov_check_fail_status.flags.reject_pos_NE = true;
}
// record the successful height fusion event
if (innov_check_pass_map[5] && _fuse_height) {
_time_last_hgt_fuse = _time_last_imu;
_sensor_fault_status.flags.reject_pos_D = false;
_innov_check_fail_status.flags.reject_pos_D = false;
} else if (!innov_check_pass_map[5]) {
_sensor_fault_status.flags.reject_pos_D = true;
_innov_check_fail_status.flags.reject_pos_D = true;
}
for (unsigned obs_index = 0; obs_index < 6; obs_index++) {