Skip to content
This repository has been archived by the owner on May 1, 2024. It is now read-only.

EKF: Reduce effect of GPS noise when using other aiding sources #403

Merged
merged 1 commit into from
Mar 27, 2018
Merged

EKF: Reduce effect of GPS noise when using other aiding sources #403

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 10 additions & 2 deletions EKF/control.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -523,8 +523,16 @@ void Ekf::controlGpsFusion()

// calculate observation process noise
float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
_posObsNoiseNE = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit);
if (_control_status.flags.opt_flow || _control_status.flags.ev_pos) {
// if we are using other sources of aiding, then relax the upper observation
// noise limit which prevents bad GPS perturbing the position estimate
_posObsNoiseNE = fmaxf(_gps_sample_delayed.hacc, lower_limit);
} else {
// if we are not using another source of aiding, then we are reliant on the GPS
// observations to constrain attitude errors and must limit the observation noise value.
float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
_posObsNoiseNE = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit);
}
_velObsVarNE(1) = _velObsVarNE(0) = sq(fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise));

// calculate innovations
Expand Down