From d2407c34630d41e954d52de8d9d94d6e322c43c4 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 13 Mar 2016 20:14:16 +1100 Subject: [PATCH] EKF: code style updates --- EKF/control.cpp | 7 ++++--- EKF/gps_checks.cpp | 4 +++- EKF/terrain_estimator.cpp | 3 ++- 3 files changed, 9 insertions(+), 5 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 026cb7a1de..8ca8eae32c 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -125,6 +125,7 @@ void Ekf::controlFusionModes() if (_control_status.flags.yaw_align) { _control_status.flags.gps = true; _time_last_gps = _time_last_imu; + // if we are not already aiding with optical flow, then we need to reset the position and velocity if (!_control_status.flags.opt_flow) { _control_status.flags.gps = resetPosition(); @@ -248,9 +249,9 @@ void Ekf::controlFusionModes() // Control the soure of height measurements for the main filter if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) { - _control_status.flags.baro_hgt = true; - _control_status.flags.rng_hgt = false; - _control_status.flags.gps_hgt = false; + _control_status.flags.baro_hgt = true; + _control_status.flags.rng_hgt = false; + _control_status.flags.gps_hgt = false; } else if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) { _control_status.flags.baro_hgt = false; diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index 3a2a5a220c..e30c676b22 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -56,7 +56,7 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps) { // If we have defined the WGS-84 position of the NED origin, run gps quality checks until they pass, then define the origins WGS-84 position using the last GPS fix - if (!_NED_origin_initialised ) { + if (!_NED_origin_initialised) { // we have good GPS data so can now set the origin's WGS-84 position if (gps_is_good(gps) && !_NED_origin_initialised) { printf("gps is good - setting EKF origin\n"); @@ -64,12 +64,14 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps) double lat = gps->lat / 1.0e7; double lon = gps->lon / 1.0e7; map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu); + // if we are already doing aiding, corect for the change in posiiton since the EKF started navigating if (_control_status.flags.opt_flow || _control_status.flags.gps) { double est_lat, est_lon; map_projection_reproject(&_pos_ref, -_state.pos(0), -_state.pos(1), &est_lat, &est_lon); map_projection_init_timestamped(&_pos_ref, est_lat, est_lon, _time_last_imu); } + // Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin _gps_alt_ref = 1e-3f * (float)gps->alt + _state.pos(2); _NED_origin_initialised = true; diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index c4f3655d71..9879bbecb4 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -78,7 +78,8 @@ void Ekf::predictHagl() _terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_p_noise); // process noise due to terrain gradient - _terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel(1))); + _terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel( + 1))); // limit the variance to prevent it becoming badly conditioned _terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);