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: Don't use EKF origin in GPS drift check calculation

Browse Source 
The GPS drift calculations need to be able to run independently of the EKF origin.
This commit is contained in:
Paul Riseborough 2017-04-07 20:04:23 +10:00 committed by Lorenz Meier
parent a1a5734443
commit 0d7e7e9d81
3 changed files with 18 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
EKF/estimator_interface.cpp
View File

@ -67,6 +67,8 @@ EstimatorInterface::EstimatorInterface():
_gps_origin_eph(0.0f),
_gps_origin_epv(0.0f),
_pos_ref{},
_gps_pos_prev{},
_gps_alt_prev(0.0f),
_yaw_test_ratio(0.0f),
_mag_test_ratio{},
_vel_pos_test_ratio{},

4
EKF/estimator_interface.h
View File

@ -328,7 +328,9 @@ protected:
bool _gps_speed_valid;
float _gps_origin_eph; // horizontal position uncertainty of the GPS origin
float _gps_origin_epv; // vertical position uncertainty of the GPS origin
struct map_projection_reference_s _pos_ref; // Contains WGS-84 position latitude and longitude (radians)
struct map_projection_reference_s _pos_ref; // Contains WGS-84 position latitude and longitude (radians) of the EKF origin
struct map_projection_reference_s _gps_pos_prev; // Contains WGS-84 position latitude and longitude (radians) of the previous GPS message
float _gps_alt_prev; // height from the previous GPS message (m)
// innovation consistency check monitoring ratios
float _yaw_test_ratio; // yaw innovation consistency check ratio

23
EKF/gps_checks.cpp
View File

@ -135,19 +135,23 @@ bool Ekf::gps_is_good(struct gps_message *gps)
double lat = gps->lat * 1.0e-7;
double lon = gps->lon * 1.0e-7;
if (_pos_ref.init_done) {
map_projection_project(&_pos_ref, lat, lon, &delta_posN, &delta_PosE);
// calculate position movement since last GPS fix
if (_gps_pos_prev.timestamp > 0) {
map_projection_project(&_gps_pos_prev, lat, lon, &delta_posN, &delta_PosE);
} else {
map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu);
_gps_alt_ref = 1e-3f * (float)gps->alt;
// no previous position has been set
map_projection_init_timestamped(&_gps_pos_prev, lat, lon, _time_last_imu);
_gps_alt_prev = 1e-3f * (float)gps->alt;
}
// Calculate time lapsed since last update, limit to prevent numerical errors and calculate the lowpass filter coefficient
const float filt_time_const = 10.0f;
float dt = fminf(fmaxf(float(_time_last_imu - _last_gps_origin_time_us) * 1e-6f, 0.001f), filt_time_const);
float dt = fminf(fmaxf(float(_time_last_imu - _gps_pos_prev.timestamp) * 1e-6f, 0.001f), filt_time_const);
float filter_coef = dt / filt_time_const;
// save GPS fix for next time
map_projection_init_timestamped(&_gps_pos_prev, lat, lon, _time_last_imu);
// Calculate the horizontal drift velocity components and limit to 10x the threshold
float vel_limit = 10.0f * _params.req_hdrift;
float velN = fminf(fmaxf(delta_posN / dt, -vel_limit), vel_limit);
@ -169,10 +173,9 @@ bool Ekf::gps_is_good(struct gps_message *gps)
// Calculate the vertical drift velocity and limit to 10x the threshold
vel_limit = 10.0f * _params.req_vdrift;
float velD = fminf(fmaxf((_gps_alt_ref - 1e-3f * (float)gps->alt) / dt, -vel_limit), vel_limit);
// Save the current height as the reference for next time
_gps_alt_ref = 1e-3f * (float)gps->alt;
float gps_alt_m = 1e-3f * (float)gps->alt;
float velD = math::constrain(((_gps_alt_prev - gps_alt_m) / dt), -vel_limit, vel_limit);
_gps_alt_prev = gps_alt_m;
// Apply a low pass filter to the vertical velocity
_gps_drift_velD = velD * filter_coef + _gps_drift_velD * (1.0f - filter_coef);