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

lpos accuracy: bump variance if deadreckon time exceeded

This commit is contained in:
bresch 2021-01-28 10:32:27 +01:00 committed by Daniel Agar
parent 46251db4a1
commit 5aaa6c6dde
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
EKF/ekf_helper.cpp
View File

@ -752,10 +752,10 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const
// TODO - allow for baro drift in vertical position error
float hpos_err = sqrtf(P(7, 7) + P(8, 8));
// If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal position error
// If we are dead-reckoning for too long, use the innovations as a conservative alternate measure of the horizontal position error
// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
// and using state variances for accuracy reporting is overly optimistic in these situations
if (_is_dead_reckoning && _control_status.flags.gps) {
if (_deadreckon_time_exceeded && _control_status.flags.gps) {
hpos_err = math::max(hpos_err, sqrtf(sq(_gps_pos_innov(0)) + sq(_gps_pos_innov(1))));
}
@ -768,10 +768,10 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
{
float hvel_err = sqrtf(P(4, 4) + P(5, 5));
// If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal velocity error
// If we are dead-reckoning for too long, use the innovations as a conservative alternate measure of the horizontal velocity error
// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
// and using state variances for accuracy reporting is overly optimistic in these situations
if (_is_dead_reckoning) {
if (_deadreckon_time_exceeded) {
float vel_err_conservative = 0.0f;
if (_control_status.flags.opt_flow) {