From 163c08a3ac2828349c018e09668c9f49c7226545 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 12 Apr 2016 07:32:24 +1000 Subject: [PATCH] EKF: Improve output observer documentation --- EKF/ekf.cpp | 44 ++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 40 insertions(+), 4 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 874f68a4cf..06d490aa7e 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -581,66 +581,102 @@ bool Ekf::collect_imu(imuSample &imu) return false; } +/* + * Implement a strapdown INS algorithm using the latest IMU data at the current time horizon. + * Buffer the INS states and calculate the difference with the EKF states at the delayed fusion time horizon. + * Calculate delta angle, delta velocity and velocity corrections from the differences and apply them at the + * current time horizon so that the INS states track the EKF states at the delayed fusion time horizon. + * The inspiration for using a complementary filter to correct for time delays in the EKF + * is based on the work by A Khosravian: + * “Recursive Attitude Estimation in the Presence of Multi-rate and Multi-delay Vector Measurements” + * A Khosravian, J Trumpf, R Mahony, T Hamel, Australian National University +*/ void Ekf::calculateOutputStates() { + // use latest IMU data imuSample imu_new = _imu_sample_new; - Vector3f delta_angle; + // Get the delta angle and velocity from the latest IMU data // Note: We do no not need to consider any bias or scale correction here // since the base class has already corrected the imu sample + Vector3f delta_angle; delta_angle(0) = imu_new.delta_ang(0); delta_angle(1) = imu_new.delta_ang(1); delta_angle(2) = imu_new.delta_ang(2); Vector3f delta_vel = imu_new.delta_vel; + // Apply corrections to the delta angle required to track the quaternion states at the EKF fusion time horizon delta_angle += _delta_angle_corr; + + // convert the delta angle to an equivalent delta quaternions Quaternion dq; dq.from_axis_angle(delta_angle); + // rotate the previous INS quaternion by the delta quaternions _output_new.time_us = imu_new.time_us; _output_new.quat_nominal = dq * _output_new.quat_nominal; + + // the quaternions must always be normalised afer modification _output_new.quat_nominal.normalize(); + // calculate the rotation matrix from body to earth frame _R_to_earth_now = quat_to_invrotmat(_output_new.quat_nominal); + // rotate the delta velocity to earth frame + // apply a delta velocity correction required to track the velocity states at the EKF fusion time horizon Vector3f delta_vel_NED = _R_to_earth_now * delta_vel + _delta_vel_corr; + + // corrrect for measured accceleration due to gravity delta_vel_NED(2) += 9.81f * imu_new.delta_vel_dt; + // save the previous velocity so we can use trapezidal integration Vector3f vel_last = _output_new.vel; + // increment the INS velocity states by the measurement plus corrections _output_new.vel += delta_vel_NED; + // use trapezoidal integration to calculate the INS position states + // apply a velocity correction required to track the position states at the EKF fusion time horizon _output_new.pos += (_output_new.vel + vel_last) * (imu_new.delta_vel_dt * 0.5f) + _vel_corr * imu_new.delta_vel_dt; + // store INS states in a ring buffer that with the same length and time coordinates as the IMU data buffer if (_imu_updated) { _output_buffer.push(_output_new); _imu_updated = false; } + // get the oldest INS state data from the ring buffer + // this data will be at the EKF fusion time horizon _output_sample_delayed = _output_buffer.get_oldest(); + // calculate the quaternion delta between the INS and EKF quaternions at the EKF fusion time horizon Quaternion quat_inv = _state.quat_nominal.inversed(); Quaternion q_error = _output_sample_delayed.quat_nominal * quat_inv; q_error.normalize(); - Vector3f delta_ang_error; + // convert the quaternion delta to a delta angle + Vector3f delta_ang_error; float scalar; - if (q_error(0) >= 0.0f) { scalar = -2.0f; } else { scalar = 2.0f; } - delta_ang_error(0) = scalar * q_error(1); delta_ang_error(1) = scalar * q_error(2); delta_ang_error(2) = scalar * q_error(3); + // calculate a corrrection to the delta angle + // that will cause the INS to track the EKF quaternions with a 1 sec time constant _delta_angle_corr = delta_ang_error * imu_new.delta_ang_dt; + // calculate a correction to the delta velocity + // that will cause the INS to track the EKF velocity with a 1 sec time constant _delta_vel_corr = (_state.vel - _output_sample_delayed.vel) * imu_new.delta_vel_dt; + // calculate a correction to the INS velocity + // that will cause the INS to track the EKF position with a 1 sec time constant _vel_corr = (_state.pos - _output_sample_delayed.pos); }