From 5bf02517a7a609444ae55de08d1d08c4e8be6eb5 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 12 Apr 2016 06:34:50 +1000 Subject: [PATCH] EKF: Rationalise use of rotation matrices and improve efficiency --- EKF/ekf.cpp | 38 ++++++++++++++++++++++++-------------- EKF/ekf.h | 2 +- EKF/mag_fusion.cpp | 8 ++++---- EKF/optflow_fusion.cpp | 2 +- EKF/terrain_estimator.cpp | 6 +++--- EKF/vel_pos_fusion.cpp | 4 ++-- 6 files changed, 35 insertions(+), 25 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index e514a708ec..4c49a30207 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -89,7 +89,7 @@ Ekf::Ekf(): _state = {}; _last_known_posNE.setZero(); _earth_rate_NED.setZero(); - _R_prev = matrix::Dcm(); + _R_to_earth = matrix::Dcm(); memset(_vel_pos_innov, 0, sizeof(_vel_pos_innov)); memset(_mag_innov, 0, sizeof(_mag_innov)); memset(_flow_innov, 0, sizeof(_flow_innov)); @@ -217,11 +217,11 @@ bool Ekf::update() // determine if range finder data has fallen behind the fusin time horizon fuse it if we are // not tilted too much to use it if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed) - && (_R_prev(2, 2) > 0.7071f)) { + && (_R_to_earth(2, 2) > 0.7071f)) { // correct the range data for position offset relative to the IMU Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body; - Vector3f pos_offset_earth = _R_prev.transpose() * pos_offset_body; - _range_sample_delayed.rng += pos_offset_earth(2) / _R_prev(2, 2); + Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; + _range_sample_delayed.rng += pos_offset_earth(2) / _R_to_earth(2, 2); // if we have range data we always try to estimate terrain height _fuse_hagl_data = true; @@ -267,11 +267,11 @@ bool Ekf::update() Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f/_imu_sample_delayed.delta_ang_dt); Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body; Vector3f vel_offset_body = cross_product(ang_rate,pos_offset_body); - Vector3f vel_offset_earth = _R_prev.transpose() * vel_offset_body; + Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; _gps_sample_delayed.vel -= vel_offset_earth; // correct position and height for offset relative to IMU - Vector3f pos_offset_earth = _R_prev.transpose() * pos_offset_body; + Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; _gps_sample_delayed.pos(0) -= pos_offset_earth(0); _gps_sample_delayed.pos(1) -= pos_offset_earth(1); _gps_sample_delayed.hgt += pos_offset_earth(2); @@ -288,7 +288,7 @@ bool Ekf::update() // If we are using optical flow aiding and data has fallen behind the fusion time horizon, then fuse it if (_flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed) && _control_status.flags.opt_flow && (_time_last_imu - _time_last_optflow) < 2e5 - && (_R_prev(2, 2) > 0.7071f)) { + && (_R_to_earth(2, 2) > 0.7071f)) { _fuse_flow = true; } @@ -450,6 +450,9 @@ bool Ekf::initialiseFilter(void) _state.quat_nominal = Quaternion(euler_init); _output_new.quat_nominal = _state.quat_nominal; + // update transformation matrix from body to world frame + _R_to_earth = quat_to_invrotmat(_state.quat_nominal); + // initialise the filtered alignment error estimate to a larger starting value _tilt_err_length_filt = 1.0f; @@ -491,26 +494,33 @@ void Ekf::predictState() } } - // attitude error state prediction - matrix::Dcm R_to_earth(_state.quat_nominal); // transformation matrix from body to world frame - Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _R_prev * _earth_rate_NED * + // correct delta angles for earth rotation rate + Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _R_to_earth.transpose() * _earth_rate_NED * _imu_sample_delayed.delta_ang_dt; - Quaternion dq; // delta quaternion since last update + + // convert the delta angle to a delta quaternion + Quaternion dq; dq.from_axis_angle(corrected_delta_ang); + + // rotate the previous quaternion by the delta quaternion using a quaternion multiplication _state.quat_nominal = dq * _state.quat_nominal; + + // quaternions must be normalised whenever they are modified _state.quat_nominal.normalize(); - _R_prev = R_to_earth.transpose(); - + // save the previous value of velocity so we can use trapzoidal integration Vector3f vel_last = _state.vel; // predict velocity states - _state.vel += R_to_earth * _imu_sample_delayed.delta_vel; + _state.vel += _R_to_earth * _imu_sample_delayed.delta_vel; _state.vel(2) += 9.81f * _imu_sample_delayed.delta_vel_dt; // predict position states via trapezoidal integration of velocity _state.pos += (vel_last + _state.vel) * _imu_sample_delayed.delta_vel_dt * 0.5f; + // update transformation matrix from body to world frame + _R_to_earth = quat_to_invrotmat(_state.quat_nominal); + constrainStates(); } diff --git a/EKF/ekf.h b/EKF/ekf.h index 9c1fdcf7d8..c2b91eb546 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -149,7 +149,7 @@ private: Vector3f _earth_rate_NED; // earth rotation vector (NED) in rad/s - matrix::Dcm _R_prev; // transformation matrix from earth frame to body frame of previous ekf step + matrix::Dcm _R_to_earth; // transformation matrix from body frame to earth frame from last EKF predition float P[_k_num_states][_k_num_states]; // state covariance matrix float KH[_k_num_states][_k_num_states]; // intermediate variable for the covariance update diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 689bdf23ca..22625289bd 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -496,7 +496,7 @@ void Ekf::fuseHeading() matrix::Vector3f mag_earth_pred; // determine if a 321 or 312 Euler sequence is best - if (fabsf(_R_prev(0, 2)) < fabsf(_R_prev(1, 2))) { + if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) { // calculate observation jacobian when we are observing the first rotation in a 321 sequence float t2 = q0 * q0; float t3 = q1 * q1; @@ -584,9 +584,9 @@ void Ekf::fuseHeading() // Calculate the 312 sequence euler angles that rotate from earth to body frame // See http://www.atacolorado.com/eulersequences.doc Vector3f euler312; - euler312(0) = atan2f(-_R_prev(1, 0) , _R_prev(1, 1)); // first rotation (yaw) - euler312(1) = asinf(_R_prev(1, 2)); // second rotation (roll) - euler312(2) = atan2f(-_R_prev(0, 2) , _R_prev(2, 2)); // third rotation (pitch) + euler312(0) = atan2f(-_R_to_earth(0, 1) , _R_to_earth(1, 1)); // first rotation (yaw) + euler312(1) = asinf(_R_to_earth(2, 1)); // second rotation (roll) + euler312(2) = atan2f(-_R_to_earth(2, 0) , _R_to_earth(2, 2)); // third rotation (pitch) predicted_hdg = euler312(0); // we will need the predicted heading to calculate the innovation diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index ce62339365..09f325b8f4 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -94,7 +94,7 @@ void Ekf::fuseOptFlow() Vector3f vel_rel_imu_body = cross_product(_flow_sample_delayed.gyroXYZ , pos_offset_body); // calculate the velocity of the sensor in the earth frame - Vector3f vel_rel_earth = _state.vel + _R_prev.transpose() * vel_rel_imu_body; + Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body; // rotate into body frame Vector3f vel_body = earth_to_body * vel_rel_earth; diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index 9879bbecb4..6c3fa82f5b 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -88,9 +88,9 @@ void Ekf::predictHagl() void Ekf::fuseHagl() { // If the vehicle is excessively tilted, do not try to fuse range finder observations - if (_R_prev(2, 2) > 0.7071f) { + if (_R_to_earth(2, 2) > 0.7071f) { // get a height above ground measurement from the range finder assuming a flat earth - float meas_hagl = _range_sample_delayed.rng * _R_prev(2, 2); + float meas_hagl = _range_sample_delayed.rng * _R_to_earth(2, 2); // predict the hagl from the vehicle position and terrain height float pred_hagl = _terrain_vpos - _state.pos(2); @@ -99,7 +99,7 @@ void Ekf::fuseHagl() _hagl_innov = pred_hagl - meas_hagl; // calculate the observation variance adding the variance of the vehicles own height uncertainty and factoring in the effect of tilt on measurement error - float obs_variance = fmaxf(P[8][8], 0.0f) + sq(_params.range_noise / _R_prev(2, 2)); + float obs_variance = fmaxf(P[8][8], 0.0f) + sq(_params.range_noise / _R_to_earth(2, 2)); // calculate the innovation variance - limiting it to prevent a badly conditioned fusion _hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance); diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index f01f7be6ae..acc430c176 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -135,10 +135,10 @@ void Ekf::fuseVelPosHeight() // innovation gate size gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f); - } else if (_control_status.flags.rng_hgt && (_R_prev(2, 2) > 0.7071f)) { + } else if (_control_status.flags.rng_hgt && (_R_to_earth(2, 2) > 0.7071f)) { fuse_map[5] = true; // use range finder with tilt correction - _vel_pos_innov[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_prev(2, 2), + _vel_pos_innov[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_to_earth(2, 2), _params.rng_gnd_clearance)); // observation variance - user parameter defined R[5] = fmaxf(_params.range_noise, 0.01f);