From 40e174b81c9de5539a75d4ba7071294a0678ee66 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 10 Feb 2016 10:02:30 +1100 Subject: [PATCH] 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];