From 40e174b81c9de5539a75d4ba7071294a0678ee66 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 10 Feb 2016 10:02:30 +1100 Subject: [PATCH 1/2] EKF: Correct for sensor noise and baro offset during alignment --- EKF/ekf.cpp | 124 +++++++++++++++++++++++------------------ EKF/ekf.h | 7 +++ EKF/ekf_helper.cpp | 2 +- EKF/gps_checks.cpp | 3 +- EKF/vel_pos_fusion.cpp | 2 +- 5 files changed, 81 insertions(+), 57 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 269b160a22..8a9987dfea 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -196,73 +196,89 @@ bool Ekf::update() bool Ekf::initialiseFilter(void) { - _state.ang_error.setZero(); - _state.vel.setZero(); - _state.pos.setZero(); - _state.gyro_bias.setZero(); - _state.gyro_scale(0) = _state.gyro_scale(1) = _state.gyro_scale(2) = 1.0f; - _state.accel_z_bias = 0.0f; - _state.mag_I.setZero(); - _state.mag_B.setZero(); - _state.wind_vel.setZero(); + // Keep accumulating measurements until we have a minimum of 10 samples for the baro and magnetoemter - // get initial roll and pitch estimate from accel vector, assuming vehicle is static - Vector3f accel_init = _imu_down_sampled.delta_vel / _imu_down_sampled.delta_vel_dt; + // Sum the IMU delta angle measurements + _delVel_sum += _imu_down_sampled.delta_vel; - float pitch = 0.0f; - float roll = 0.0f; - - if (accel_init.norm() > 0.001f) { - accel_init.normalize(); - - pitch = asinf(accel_init(0)); - roll = -asinf(accel_init(1) / cosf(pitch)); - } - - matrix::Euler euler_init(roll, pitch, 0.0f); - - // Get the latest magnetic field measurement. - // If we don't have a measurement then we cannot initialise the filter + // Sum the magnetometer measurements magSample mag_init = _mag_buffer.get_newest(); - if (mag_init.time_us == 0) { - return false; + if (mag_init.time_us != 0) { + _mag_counter ++; + _mag_sum += mag_init.mag; } - // rotate magnetic field into earth frame assuming zero yaw and estimate yaw angle assuming zero declination - // TODO use declination if available - matrix::Dcm R_to_earth_zeroyaw(euler_init); - Vector3f mag_ef_zeroyaw = R_to_earth_zeroyaw * mag_init.mag; - float declination = 0.0f; - euler_init(2) = declination - atan2f(mag_ef_zeroyaw(1), mag_ef_zeroyaw(0)); - - // calculate initial quaternion states - _state.quat_nominal = Quaternion(euler_init); - _output_new.quat_nominal = _state.quat_nominal; - - // TODO replace this with a conditional test based on fitered angle error states. - _control_status.flags.angle_align = true; - - // calculate initial earth magnetic field states - matrix::Dcm R_to_earth(euler_init); - _state.mag_I = R_to_earth * mag_init.mag; - - resetVelocity(); - resetPosition(); - + // Sum the barometer measurements // initialize vertical position with newest baro measurement baroSample baro_init = _baro_buffer.get_newest(); - if (baro_init.time_us == 0) { - return false; + if (baro_init.time_us != 0) { + _baro_counter ++; + _baro_sum += baro_init.hgt; } - _state.pos(2) = -baro_init.hgt; - _output_new.pos(2) = -baro_init.hgt; + // check to see if we have enough measruements and return false if not + if (_baro_counter < 10 || _mag_counter < 10) { + return false; - initialiseCovariance(); + } else { + // Zero all of the states + _state.ang_error.setZero(); + _state.vel.setZero(); + _state.pos.setZero(); + _state.gyro_bias.setZero(); + _state.gyro_scale(0) = _state.gyro_scale(1) = _state.gyro_scale(2) = 1.0f; + _state.accel_z_bias = 0.0f; + _state.mag_I.setZero(); + _state.mag_B.setZero(); + _state.wind_vel.setZero(); - return true; + // get initial roll and pitch estimate from delta velocity vector, assuming vehicle is static + float pitch = 0.0f; + float roll = 0.0f; + + if (_delVel_sum.norm() > 0.001f) { + _delVel_sum.normalize(); + pitch = asinf(_delVel_sum(0)); + roll = -asinf(_delVel_sum(1) / cosf(pitch)); + + } else { + return false; + } + + // calculate the averaged magnetometer reading + Vector3f mag_init = _mag_sum * (1.0f / (float(_mag_counter))); + + // rotate magnetic field into earth frame assuming zero yaw and estimate yaw angle assuming zero declination + // TODO use declination if available + matrix::Euler euler_init(roll, pitch, 0.0f); + matrix::Dcm R_to_earth_zeroyaw(euler_init); + Vector3f mag_ef_zeroyaw = R_to_earth_zeroyaw * mag_init; + float declination = 0.0f; + euler_init(2) = declination - atan2f(mag_ef_zeroyaw(1), mag_ef_zeroyaw(0)); + + // calculate initial quaternion states + _state.quat_nominal = Quaternion(euler_init); + _output_new.quat_nominal = _state.quat_nominal; + + // TODO replace this with a conditional test based on fitered angle error states. + _control_status.flags.angle_align = true; + + // calculate initial earth magnetic field states + matrix::Dcm R_to_earth(euler_init); + _state.mag_I = R_to_earth * mag_init; + + // calculate the averaged barometer reading + _baro_at_alignment = _baro_sum / (float)_baro_counter; + + resetVelocity(); + resetPosition(); + + initialiseCovariance(); + + return true; + } } void Ekf::predictState() diff --git a/EKF/ekf.h b/EKF/ekf.h index 3ea6449a24..74ee55932f 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -145,6 +145,13 @@ private: uint64_t _last_gps_origin_time_us = 0; // time the origin was last set (uSec) float _gps_alt_ref = 0.0f; // WGS-84 height (m) + // Variables used to initialise the filter states + uint8_t _baro_counter = 0; // number of baro samples averaged + float _baro_sum = 0.0f; // summed baro measurement + uint8_t _mag_counter = 0; // number of magnetometer samples averaged + Vector3f _mag_sum = {}; // summed magnetometer measurement + Vector3f _delVel_sum = {}; // summed delta velocity + float _baro_at_alignment; // baro offset relative to alignment position gps_check_fail_status_u _gps_check_fail_status; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 7ab35abeb3..41e66cad86 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -78,7 +78,7 @@ void Ekf::resetPosition() } baroSample baro_newest = _baro_buffer.get_newest(); - _state.pos(2) = -baro_newest.hgt; + _state.pos(2) = _baro_at_alignment - baro_newest.hgt; } #if defined(__PX4_POSIX) && !defined(__PX4_QURT) diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index 3b19d44ed3..16a3e502e7 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -63,7 +63,8 @@ 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(&_pos_ref, lat, lon); - _gps_alt_ref = gps->alt / 1e3f; + // 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; _last_gps_origin_time_us = _time_last_imu; } diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index aef41ee149..bce1fd1221 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -100,7 +100,7 @@ void Ekf::fuseVelPosHeight() if (_fuse_height) { fuse_map[5] = true; // vertical position innovation - baro measurement has opposite sign to earth z axis - _vel_pos_innov[5] = _state.pos(2) - (-_baro_sample_delayed.hgt); + _vel_pos_innov[5] = _state.pos(2) - (_baro_at_alignment -_baro_sample_delayed.hgt); // observation variance - user parameter defined R[5] = fmaxf(_params.baro_noise, 0.01f); R[5] = R[5] * R[5]; From fff2bd50f630a15ac6b5172b8fcb21b676390815 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 10 Feb 2016 12:53:24 +1100 Subject: [PATCH 2/2] EKF: Fix bugs in position and velocity resets The position reset was not being compensated for velocity and measurement delay The height was being reset with the position. It has been moved into a separate reset function The maximum accepted GPS delay of 100msec was inadequate The states was being incorrectly reset to the GPS position and Baro height on initial alignment. --- EKF/ekf.cpp | 3 ++- EKF/ekf.h | 2 ++ EKF/ekf_helper.cpp | 25 +++++++++++++++++++------ 3 files changed, 23 insertions(+), 7 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 8a9987dfea..32e4b87c47 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -272,9 +272,10 @@ bool Ekf::initialiseFilter(void) // calculate the averaged barometer reading _baro_at_alignment = _baro_sum / (float)_baro_counter; + // set the velocity to the GPS measurement (by definition, the initial position and height is at the origin) resetVelocity(); - resetPosition(); + // initialise the state covariance matrix initialiseCovariance(); return true; diff --git a/EKF/ekf.h b/EKF/ekf.h index 74ee55932f..a630f3bc3b 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -181,6 +181,8 @@ private: void resetPosition(); + void resetHeight(); + void makeCovSymetrical(); void limitCov(); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 41e66cad86..c5c830a326 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -54,7 +54,7 @@ void Ekf::resetVelocity() // if we have a valid GPS measurement use it to initialise velocity states gpsSample gps_newest = _gps_buffer.get_newest(); - if (_time_last_imu - gps_newest.time_us < 100000) { + if (_time_last_imu - gps_newest.time_us < 400000) { _state.vel = gps_newest.vel; } else { @@ -66,19 +66,32 @@ void Ekf::resetVelocity() // gps measurement then use for position initialisation void Ekf::resetPosition() { - // if we have a valid GPS measurement use it to initialise position states + // if we have a fresh GPS measurement, use it to initialise position states and correct the position for the measurement delay gpsSample gps_newest = _gps_buffer.get_newest(); - if (_time_last_imu - gps_newest.time_us < 100000) { - _state.pos(0) = gps_newest.pos(0); - _state.pos(1) = gps_newest.pos(1); + float time_delay = 1e-6f * (float)(_time_last_imu - gps_newest.time_us); + + if (time_delay < 0.4f) { + _state.pos(0) = gps_newest.pos(0) + gps_newest.vel(0) * time_delay; + _state.pos(1) = gps_newest.pos(1) + gps_newest.vel(1) * time_delay; } else { // XXX use the value of the last known position } +} +// Reset height state using the last baro measurement +void Ekf::resetHeight() +{ + // if we have a valid height measurement, use it to initialise the vertical position state baroSample baro_newest = _baro_buffer.get_newest(); - _state.pos(2) = _baro_at_alignment - baro_newest.hgt; + + if (_time_last_imu - baro_newest.time_us < 200000) { + _state.pos(2) = _baro_at_alignment - baro_newest.hgt; + + } else { + // XXX use the value of the last known position + } } #if defined(__PX4_POSIX) && !defined(__PX4_QURT)