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: prevent race condition between global position validity and eph reporting

Browse Source
This commit is contained in:
Paul Riseborough 2018-04-19 11:03:50 +10:00 committed by Daniel Agar
parent fdabb9277f
commit 6e3403ce28
No known key found for this signature in database
GPG Key ID: FD3CBA98017A69DE
3 changed files with 40 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
EKF/control.cpp
View File

@ -1356,39 +1356,45 @@ void Ekf::controlVelPosFusion()
if (!_control_status.flags.gps &&
!_control_status.flags.opt_flow &&
!_control_status.flags.ev_pos &&
!(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta) &&
((_time_last_imu - _time_last_fake_gps > (uint64_t)2e5) || _fuse_height))
{
// Reset position and velocity states if we re-commence this aiding method
if ((_time_last_imu - _time_last_fake_gps) > (uint64_t)4e5) {
resetPosition();
resetVelocity();
_fuse_hpos_as_odom = false;
if (_time_last_fake_gps != 0) {
ECL_WARN("EKF stopping navigation");
!(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta)) {
// We now need to use a synthetic positon observation to prevent unconstrained drift of the INS states.
_using_synthetic_position = true;
// Fuse synthetic position observations every 200msec
if ((_time_last_imu - _time_last_fake_gps > (uint64_t)2e5) || _fuse_height) {
// Reset position and velocity states if we re-commence this aiding method
if ((_time_last_imu - _time_last_fake_gps) > (uint64_t)4e5) {
resetPosition();
resetVelocity();
_fuse_hpos_as_odom = false;
if (_time_last_fake_gps != 0) {
ECL_WARN("EKF stopping navigation");
}
}
_fuse_pos = true;
_fuse_hor_vel = false;
_fuse_vert_vel = false;
_time_last_fake_gps = _time_last_imu;
if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
_posObsNoiseNE = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise);
} else {
_posObsNoiseNE = 0.5f;
}
_vel_pos_innov[0] = 0.0f;
_vel_pos_innov[1] = 0.0f;
_vel_pos_innov[2] = 0.0f;
_vel_pos_innov[3] = _state.pos(0) - _last_known_posNE(0);
_vel_pos_innov[4] = _state.pos(1) - _last_known_posNE(1);
// glitch protection is not required so set gate to a large value
_posInnovGateNE = 100.0f;
}
_fuse_pos = true;
_fuse_hor_vel = false;
_fuse_vert_vel = false;
_time_last_fake_gps = _time_last_imu;
if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
_posObsNoiseNE = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise);
} else {
_posObsNoiseNE = 0.5f;
}
_vel_pos_innov[0] = 0.0f;
_vel_pos_innov[1] = 0.0f;
_vel_pos_innov[2] = 0.0f;
_vel_pos_innov[3] = _state.pos(0) - _last_known_posNE(0);
_vel_pos_innov[4] = _state.pos(1) - _last_known_posNE(1);
// glitch protection is not required so set gate to a large value
_posInnovGateNE = 100.0f;
} else {
_using_synthetic_position = false;
}
// Fuse available NED velocity and position data into the main filter

1
EKF/ekf.h
View File

@ -292,6 +292,7 @@ private:
uint64_t _time_last_fake_gps{0}; ///< last time we faked GPS position measurements to constrain tilt errors during operation without external aiding (uSec)
uint64_t _time_ins_deadreckon_start{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec)
bool _using_synthetic_position{false}; ///< true if we are using a synthetic position to constrain drift
uint64_t _time_last_pos_fuse{0}; ///< time the last fusion of horizontal position measurements was performed (uSec)
uint64_t _time_last_delpos_fuse{0}; ///< time the last fusion of incremental horizontal position measurements was performed (uSec)

5
EKF/ekf_helper.cpp
View File

@ -1291,8 +1291,9 @@ void Ekf::setDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first,
bool Ekf::global_position_is_valid()
{
// return true if we are not doing unconstrained free inertial navigation and the origin is set
return (_NED_origin_initialised && !_deadreckon_time_exceeded);
// return true if the origin is set we are not doing unconstrained free inertial navigation
// and have not started using synthetic position observations to constrain drift
return (_NED_origin_initialised && !_deadreckon_time_exceeded && !_using_synthetic_position);
}
// return true if we are totally reliant on inertial dead-reckoning for position