From edfb7aefcceccb0bc588059930f80251c77ea264 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 3 Jan 2016 00:03:03 +1100 Subject: [PATCH] EKF: Fix initial alignment errors Use the gravity vector to estimate the initial roll and pitch angle Use the projection of the magnetic field measurement onto the earth axis horizontal plane to calculate the initial heading --- EKF/ekf.cpp | 28 ++++++++++++++++++---------- 1 file changed, 18 insertions(+), 10 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 5e3c9f258e..cca1996cb8 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -133,7 +133,7 @@ bool Ekf::initialiseFilter(void) _state.mag_B.setZero(); _state.wind_vel.setZero(); - // get initial attitude estimate from accel vector, assuming vehicle is static + // 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; float pitch = 0.0f; @@ -146,22 +146,30 @@ bool Ekf::initialiseFilter(void) roll = -asinf(accel_init(1) / cosf(pitch)); } - magSample mag_init = _mag_buffer.get_newest(); + 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 + magSample mag_init = _mag_buffer.get_newest(); if (mag_init.time_us == 0) { return false; } - float yaw_init = atan2f(mag_init.mag(1), mag_init.mag(0)); + // 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)); - matrix::Euler euler_init(roll, pitch, yaw_init); - _state.quat_nominal = Quaternion(euler_init); + // calculate initial quaternion states + _state.quat_nominal = Quaternion(euler_init); + // calculate initial earth magnetic field states + matrix::Dcm R_to_earth(euler_init); + _state.mag_I = R_to_earth * mag_init.mag; - matrix::Dcm R_to_earth(euler_init); - _state.mag_I = R_to_earth * mag_init.mag; - - - resetVelocity(); + resetVelocity(); resetPosition(); initialiseCovariance();