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

EKF: Fix reversion from GPS to no-aiding mode (#412)

Browse Source 
* EKF: Do not delay reversion to no-aiding mode if parameter initiated

* EKF: Move no-aid reversion resets to helper functions

* EKF: Prevent unwanted fusion of velocity data during no aiding mode
This commit is contained in:
Paul Riseborough
2018-03-22 22:39:00 +11:00
committed by GitHub
parent 95c4777b35
commit 8a012436f9
2 changed files with 18 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+10 -3
View File
@@ -1339,6 +1339,9 @@ void Ekf::controlVelPosFusion()
{
// if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift
// Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz
if (!(_params.fusion_mode & MASK_USE_GPS)) {
_control_status.flags.gps = false;
}
if (!_control_status.flags.gps &&
!_control_status.flags.opt_flow &&
!_control_status.flags.ev_pos &&
@@ -1347,9 +1350,8 @@ void Ekf::controlVelPosFusion()
{
// Reset position and velocity states if we re-commence this aiding method
if ((_time_last_imu - _time_last_fake_gps) > (uint64_t)4e5) {
_last_known_posNE(0) = _state.pos(0);
_last_known_posNE(1) = _state.pos(1);
_state.vel.setZero();
resetPosition();
resetVelocity();
_fuse_hpos_as_odom = false;
if (_time_last_fake_gps != 0) {
ECL_WARN("EKF stopping navigation");
@@ -1358,6 +1360,8 @@ void Ekf::controlVelPosFusion()
}
_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) {
@@ -1365,6 +1369,9 @@ void Ekf::controlVelPosFusion()
} 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);
EKF/ekf_helper.cpp
+8 -2
View File
@@ -64,7 +64,10 @@ bool Ekf::resetVelocity()
zeroOffDiag(P,4,6);
} else {
return false;
// Used when falling back to non-aiding mode of operation
_state.vel(0) = 0.0f;
_state.vel(1) = 0.0f;
setDiag(P,4,5,25.0f);
}
// calculate the change in velocity and apply to the output predictor state history
@@ -122,7 +125,10 @@ bool Ekf::resetPosition()
setDiag(P,7,8,sq(_ev_sample_delayed.posErr));
} else {
return false;
// Used when falling back to non-aiding mode of operation
_state.pos(0) = _last_known_posNE(0);
_state.pos(1) = _last_known_posNE(1);
setDiag(P,7,8,sq(_params.pos_noaid_noise));
}
// calculate the change in position and apply to the output predictor state history