dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Fix bug causing large yaw innovations when GPS is lost

This commit is contained in:
Paul Riseborough 2018-05-17 14:13:20 +10:00 committed by Lorenz Meier
parent f59cd0f5b0
commit adb4a09beb
2 changed files with 4 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
EKF/ekf.h
View File

@ -324,6 +324,7 @@ private:
uint8_t _num_bad_flight_yaw_events{0}; ///< number of times a bad heading has been detected in flight and required a yaw reset
uint64_t _mag_use_not_inhibit_us{0}; ///< last system time in usec before magnetomer use was inhibited
bool _mag_use_inhibit{false}; ///< true when magnetomer use is being inhibited
bool _mag_use_inhibit_prev{false}; ///< true when magnetomer use was being inhibited the previous frame
bool _mag_inhibit_yaw_reset_req{false}; ///< true when magnetomer inhibit has been active for long enough to require a yaw reset when conditons improve.
float _last_static_yaw{0.0f}; ///< last yaw angle recorded when on ground motion checks were passing (rad)
bool _vehicle_at_rest_prev{false}; ///< true when the vehicle was at rest the previous time the status was checked

4
EKF/mag_fusion.cpp
View File

@ -629,7 +629,7 @@ void Ekf::fuseHeading()
// Vehicle is at rest so use the last moving prediciton as an observation
// to prevent the heading from drifting and to enable yaw gyro bias learning
// before takeoff.
if (!_vehicle_at_rest_prev) {
if (!_vehicle_at_rest_prev || !_mag_use_inhibit_prev) {
_last_static_yaw = predicted_hdg;
_vehicle_at_rest_prev = true;
}
@ -639,7 +639,9 @@ void Ekf::fuseHeading()
}
} else {
_heading_innov = predicted_hdg - measured_hdg;
_last_static_yaw = predicted_hdg;
}
_mag_use_inhibit_prev = _mag_use_inhibit;
// wrap the innovation to the interval between +-pi
_heading_innov = wrap_pi(_heading_innov);