From 3580940e108e1038ac1261262c6d4789ed3f864f Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 7 Apr 2016 13:15:12 -0700 Subject: [PATCH 01/10] EKF: Add sensor position offset parameters --- EKF/common.h | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/EKF/common.h b/EKF/common.h index 1e874fe792..88ee0ed28b 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -223,6 +223,12 @@ struct parameters { float req_hdrift; // maximum acceptable horizontal drift speed float req_vdrift; // maximum acceptable vertical drift speed + // XYZ offset of sensors in body axes (m) + Vector3f imu_pos_body; // xyz position of IMU in body frame (m) + Vector3f gps_pos_body; // xyz position of the GPS antenna in body frame (m) + Vector3f rng_pos_body; // xyz position of range sensor in body frame (m) + Vector3f flow_pos_body; // xyz position of range sensor focal point in body frame (m) + // Initialize parameter values. Initialization must be accomplished in the constructor to allow C99 compiler compatibility. parameters() { @@ -296,6 +302,12 @@ struct parameters { req_gdop = 2.0f; req_hdrift = 0.3f; req_vdrift = 0.5f; + + // XYZ offset of sensors in body axes (m) + imu_pos_body = {0}; + gps_pos_body = {0}; + rng_pos_body = {0}; + flow_pos_body = {0}; } }; From e89dbb9f63dab903bdfeb8df444cbc29d7bae9fb Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 9 Apr 2016 09:04:53 -0700 Subject: [PATCH 02/10] EKF: correct GPS data for antenna position offset --- EKF/ekf.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index e676a9ffe2..7cafed2bc5 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -257,6 +257,20 @@ bool Ekf::update() _fuse_pos = true; _fuse_vert_vel = true; _fuse_hor_vel = true; + + // correct velocity for offset relative to IMU + 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 = ang_rate % pos_offset_body; + Vector3f vel_offset_earth = _R_prev.transpose() * 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; + _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); + } // only use gps height observation in the main filter if specifically enabled From 48b105b748080762e2a83901e1e73118c05581a6 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 9 Apr 2016 09:05:52 -0700 Subject: [PATCH 03/10] EKF: correct range finder data for sensor position offset --- EKF/ekf.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 7cafed2bc5..5378009c6e 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -218,6 +218,11 @@ bool Ekf::update() // 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)) { + // 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); + // if we have range data we always try to estimate terrain height _fuse_hagl_data = true; From b46053415f79a96b793109a6a0757c2e8181ab59 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 9 Apr 2016 09:22:49 -0700 Subject: [PATCH 04/10] EKF: Compensate optical flow data for sensor position offset --- EKF/common.h | 4 ++-- EKF/ekf.cpp | 2 +- EKF/ekf.h | 4 ++-- EKF/estimator_interface.cpp | 7 ++++--- EKF/optflow_fusion.cpp | 29 ++++++++++++++++++----------- 5 files changed, 27 insertions(+), 19 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 88ee0ed28b..ad55d51f38 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -67,7 +67,7 @@ typedef matrix::Matrix Matrix3f; struct flow_message { uint8_t quality; // Quality of Flow data Vector2f flowdata; // Flow data received - Vector2f gyrodata; // Gyro data from flow sensor + Vector3f gyrodata; // Gyro data from flow sensor uint32_t dt; // integration time of flow samples }; @@ -120,7 +120,7 @@ struct flowSample { uint8_t quality; // quality indicator between 0 and 255 Vector2f flowRadXY; // measured delta angle of the image about the X and Y body axes (rad), RH rotaton is positive Vector2f flowRadXYcomp; // measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive - Vector2f gyroXY; // measured delta angle of the inertial frame about the X and Y body axes obtained from rate gyro measurements (rad), RH rotation is positive + Vector3f gyroXYZ; // measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive float dt; // amount of integration time (sec) uint64_t time_us; // timestamp in microseconds of the integration period mid-point }; diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 5378009c6e..37e9a017b5 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -266,7 +266,7 @@ bool Ekf::update() // correct velocity for offset relative to IMU 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 = ang_rate % pos_offset_body; + Vector3f vel_offset_body = cross_product(ang_rate,pos_offset_body); Vector3f vel_offset_earth = _R_prev.transpose() * vel_offset_body; _gps_sample_delayed.vel -= vel_offset_earth; diff --git a/EKF/ekf.h b/EKF/ekf.h index b98f9769d3..061f2c2fd0 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -173,8 +173,8 @@ private: // optical flow processing float _flow_innov[2]; // flow measurement innovation float _flow_innov_var[2]; // flow innovation variance - Vector2f _flow_gyro_bias; // bias errors in optical flow sensor rate gyro outputs - Vector2f _imu_del_ang_of; // bias corrected XY delta angle measurements accumulated across the same time frame as the optical flow rates + Vector3f _flow_gyro_bias; // bias errors in optical flow sensor rate gyro outputs + Vector3f _imu_del_ang_of; // bias corrected delta angle measurements accumulated across the same time frame as the optical flow rates float _delta_time_of; // time in sec that _imu_del_ang_of was accumulated over float _mag_declination; // magnetic declination used by reset and fusion functions (rad) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index bd4171d97b..876b3030e9 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -271,10 +271,11 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl // NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate is produced by a RH rotation of the image about the sensor axis. // copy the optical and gyro measured delta angles optflow_sample_new.flowRadXY = - flow->flowdata; - optflow_sample_new.gyroXY = - flow->gyrodata; + optflow_sample_new.gyroXYZ = - flow->gyrodata; // compensate for body motion to give a LOS rate - optflow_sample_new.flowRadXYcomp = optflow_sample_new.flowRadXY - optflow_sample_new.gyroXY; - // convert integraton interval to seconds + optflow_sample_new.flowRadXYcomp(0) = optflow_sample_new.flowRadXY(0) - optflow_sample_new.gyroXYZ(0); + optflow_sample_new.flowRadXYcomp(1) = optflow_sample_new.flowRadXY(1) - optflow_sample_new.gyroXYZ(1); + // convert integration interval to seconds optflow_sample_new.dt = 1e-6f * (float)flow->dt; _time_last_optflow = time_usec; // push to buffer diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index 4d14fa7fbc..ce62339365 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -87,8 +87,17 @@ void Ekf::fuseOptFlow() matrix::Dcm earth_to_body(_state.quat_nominal); earth_to_body = earth_to_body.transpose(); - // rotate earth velocities into body frame - Vector3f vel_body = earth_to_body * _state.vel; + // calculate the sensor position relative to the IMU + Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body; + + // calculate the velocity of the sensor reelative to the imu in body frame + 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; + + // rotate into body frame + Vector3f vel_body = earth_to_body * vel_rel_earth; // calculate range from focal point to centre of image float range = heightAboveGndEst / earth_to_body(2, 2); // absolute distance to the frame region in view @@ -498,8 +507,7 @@ void Ekf::get_flow_innov_var(float flow_innov_var[2]) void Ekf::calcOptFlowBias() { // accumulate the bias corrected delta angles from the navigation sensor and lapsed time - _imu_del_ang_of(0) += _imu_sample_delayed.delta_ang(0); - _imu_del_ang_of(1) += _imu_sample_delayed.delta_ang(1); + _imu_del_ang_of += _imu_sample_delayed.delta_ang; _delta_time_of += _imu_sample_delayed.delta_ang_dt; // reset the accumulators if the time interval is too large @@ -514,21 +522,20 @@ void Ekf::calcOptFlowBias() if ((fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.05f) && (_delta_time_of > 0.01f) && (_flow_sample_delayed.dt > 0.01f)) { // calculate a reference angular rate - Vector2f reference_body_rate; - reference_body_rate(0) = _imu_del_ang_of(0) / _delta_time_of; - reference_body_rate(1) = _imu_del_ang_of(1) / _delta_time_of; + Vector3f reference_body_rate; + reference_body_rate = _imu_del_ang_of * (1.0f / _delta_time_of); // calculate the optical flow sensor measured body rate - Vector2f of_body_rate; - of_body_rate(0) = _flow_sample_delayed.gyroXY(0) / _flow_sample_delayed.dt; - of_body_rate(1) = _flow_sample_delayed.gyroXY(1) / _flow_sample_delayed.dt; + Vector3f of_body_rate; + of_body_rate = _flow_sample_delayed.gyroXYZ * (1.0f / _flow_sample_delayed.dt); // calculate the bias estimate using a combined LPF and spike filter _flow_gyro_bias(0) = 0.99f * _flow_gyro_bias(0) + 0.01f * math::constrain((of_body_rate(0) - reference_body_rate(0)), -0.1f, 0.1f); _flow_gyro_bias(1) = 0.99f * _flow_gyro_bias(1) + 0.01f * math::constrain((of_body_rate(1) - reference_body_rate(1)), -0.1f, 0.1f); - + _flow_gyro_bias(2) = 0.99f * _flow_gyro_bias(2) + 0.01f * math::constrain((of_body_rate(2) - reference_body_rate(2)), + -0.1f, 0.1f); } // reset the accumulators From e10093854af1a34968c3b4cee6ee572f93db2a49 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 9 Apr 2016 12:46:27 -0700 Subject: [PATCH 05/10] EKF: correct outputs for IMU offset --- EKF/ekf.cpp | 4 ++-- EKF/ekf.h | 1 + EKF/ekf_helper.cpp | 38 ++++++++++++++++++++++++++++++++++++++ EKF/estimator_interface.h | 29 +++++++++++++++++++++++++---- 4 files changed, 66 insertions(+), 6 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 37e9a017b5..e514a708ec 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -583,9 +583,9 @@ void Ekf::calculateOutputStates() _output_new.quat_nominal = dq * _output_new.quat_nominal; _output_new.quat_nominal.normalize(); - matrix::Dcm R_to_earth(_output_new.quat_nominal); + _R_to_earth_now = quat_to_invrotmat(_output_new.quat_nominal); - Vector3f delta_vel_NED = R_to_earth * delta_vel + _delta_vel_corr; + Vector3f delta_vel_NED = _R_to_earth_now * delta_vel + _delta_vel_corr; delta_vel_NED(2) += 9.81f * imu_new.delta_vel_dt; Vector3f vel_last = _output_new.vel; diff --git a/EKF/ekf.h b/EKF/ekf.h index 061f2c2fd0..9c1fdcf7d8 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -330,4 +330,5 @@ private: // zero the specified range of columns in the state covariance matrix void zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last); + }; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 0f43e3c17c..929b3b2429 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -449,3 +449,41 @@ bool Ekf::global_position_is_valid() // TODO implement proper check based on published GPS accuracy, innovation consistency checks and timeout status return (_NED_origin_initialised && ((_time_last_imu - _time_last_gps) < 5e6) && _control_status.flags.gps); } + +// perform a vector cross product +Vector3f EstimatorInterface::cross_product(const Vector3f &vecIn1, const Vector3f &vecIn2) +{ + Vector3f vecOut; + vecOut(0) = vecIn1(1)*vecIn2(2) - vecIn1(2)*vecIn2(1); + vecOut(1) = vecIn1(2)*vecIn2(0) - vecIn1(0)*vecIn2(2); + vecOut(2) = vecIn1(0)*vecIn2(1) - vecIn1(1)*vecIn2(0); + return vecOut; +} + +// calculate the inverse rotation matrix from a quaternion rotation +Matrix3f EstimatorInterface::quat_to_invrotmat(const Quaternion quat) +{ + float q00 = quat(0) * quat(0); + float q11 = quat(1) * quat(1); + float q22 = quat(2) * quat(2); + float q33 = quat(3) * quat(3); + float q01 = quat(0) * quat(1); + float q02 = quat(0) * quat(2); + float q03 = quat(0) * quat(3); + float q12 = quat(1) * quat(2); + float q13 = quat(1) * quat(3); + float q23 = quat(2) * quat(3); + + Matrix3f dcm; + dcm(0,0) = q00 + q11 - q22 - q33; + dcm(1,1) = q00 - q11 + q22 - q33; + dcm(2,2) = q00 - q11 - q22 + q33; + dcm(0,1) = 2.0f * (q12 - q03); + dcm(0,2) = 2.0f * (q13 + q02); + dcm(1,0) = 2.0f * (q12 + q03); + dcm(1,2) = 2.0f * (q23 - q01); + dcm(2,0) = 2.0f * (q13 - q02); + dcm(2,1) = 2.0f * (q23 + q01); + + return dcm; +} diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 17b453fb75..827fa5e387 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -172,16 +172,30 @@ public: quat[i] = _output_new.quat_nominal(i); } } - void copy_velocity(float *vel) + // get the velocity of the body frame origin in local NED earth frame + void get_velocity(float *vel) { + // calculate the average angular rate across the last IMU update + Vector3f ang_rate = _imu_sample_new.delta_ang * (1.0f/_imu_sample_new.delta_ang_dt); + // calculate the velocity of the relative to the body origin + // Note % operator has been overloaded to performa cross product + Vector3f vel_imu_rel_body = cross_product(ang_rate , _params.imu_pos_body); + // rotate the relative velocty into earth frame and subtract from the EKF velocity + // (which is at the IMU) to get velocity of the body origin + Vector3f vel_earth = _output_new.vel - _R_to_earth_now * vel_imu_rel_body; + // copy to output for (unsigned i = 0; i < 3; i++) { - vel[i] = _output_new.vel(i); + vel[i] = vel_earth(i); } } - void copy_position(float *pos) + // get the position of the body frame origin in local NED earth frame + void get_position(float *pos) { + // rotate the position of the IMU relative to the boy origin into earth frame + Vector3f pos_offset_earth = _R_to_earth_now * _params.imu_pos_body; + // subtract from the EKF position (which is at the IMU) to get position at the body origin for (unsigned i = 0; i < 3; i++) { - pos[i] = _output_new.pos(i); + pos[i] = _output_new.pos(i) - pos_offset_earth(i); } } void copy_timestamp(uint64_t *time_us) @@ -218,6 +232,7 @@ protected: outputSample _output_sample_delayed; // filter output on the delayed time horizon outputSample _output_new; // filter output on the non-delayed time horizon imuSample _imu_sample_new; // imu sample capturing the newest imu data + Matrix3f _R_to_earth_now; // rotation matrix from body to earth frame at current time uint64_t _imu_ticks; // counter for imu updates @@ -273,4 +288,10 @@ protected: // this is the previous status of the filter control modes - used to detect mode transitions filter_control_status_u _control_status_prev; + // perform a vector cross product + Vector3f cross_product(const Vector3f &vecIn1, const Vector3f &vecIn2); + + // calculate the inverse rotation matrix from a quaternion rotation + Matrix3f quat_to_invrotmat(const Quaternion quat); + }; From 5bf02517a7a609444ae55de08d1d08c4e8be6eb5 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 12 Apr 2016 06:34:50 +1000 Subject: [PATCH 06/10] 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; - _state.quat_nominal.normalize(); - _R_prev = R_to_earth.transpose(); + // quaternions must be normalised whenever they are modified + _state.quat_nominal.normalize(); + // 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); From 2dcc6e2053d3861b9a6132c10b9403d4587f68cc Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 12 Apr 2016 06:58:10 +1000 Subject: [PATCH 07/10] EKF: Improve accuracy of state prediction Use an a common estimator value for gravity Use average orientation across update interval when rotating delta velocities --- EKF/ekf.cpp | 15 ++++++++++++--- EKF/ekf.h | 3 ++- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 4c49a30207..874f68a4cf 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -511,9 +511,18 @@ void Ekf::predictState() // 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(2) += 9.81f * _imu_sample_delayed.delta_vel_dt; + // calculate the increment in velocity using the previous orientation + Vector3f delta_vel_earth_1 = _R_to_earth * _imu_sample_delayed.delta_vel; + + // update the rotation matrix and calculate the increment in velocity using the current orientation + _R_to_earth = quat_to_invrotmat(_state.quat_nominal); + Vector3f delta_vel_earth_2 = _R_to_earth * _imu_sample_delayed.delta_vel; + + // Update the velocity assuming constant angular rate and acceleration across the same delta time interval + _state.vel += (delta_vel_earth_1 + delta_vel_earth_2) * 0.5f; + + // compensate for acceleration due to gravity + _state.vel(2) += _gravity_mss * _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; diff --git a/EKF/ekf.h b/EKF/ekf.h index c2b91eb546..30801ad6dd 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -124,7 +124,8 @@ public: private: static const uint8_t _k_num_states = 24; - static constexpr float _k_earth_rate = 0.000072921f; + const float _k_earth_rate = 0.000072921f; + const float _gravity_mss = 9.80665f; stateSample _state; // state struct of the ekf running at the delayed time horizon From 163c08a3ac2828349c018e09668c9f49c7226545 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 12 Apr 2016 07:32:24 +1000 Subject: [PATCH 08/10] 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); } From 8c55e36ca93d6a85645abc035cc0cb5fa9328542 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 12 Apr 2016 07:56:06 +1000 Subject: [PATCH 09/10] EKF: use common value for gravity --- EKF/ekf.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 06d490aa7e..1f8e8ea88c 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -628,7 +628,7 @@ void Ekf::calculateOutputStates() 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; + delta_vel_NED(2) += _gravity_mss * imu_new.delta_vel_dt; // save the previous velocity so we can use trapezidal integration Vector3f vel_last = _output_new.vel; From ea38aa130f3402051285ab971d592ec35ad000e5 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 14 Apr 2016 08:53:59 +1000 Subject: [PATCH 10/10] EKF: update default time delay parameter values --- EKF/common.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index ad55d51f38..d18b8698f7 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -241,8 +241,8 @@ struct parameters { baro_delay_ms = 0.0f; gps_delay_ms = 200.0f; airspeed_delay_ms = 200.0f; - flow_delay_ms = 60.0f; - range_delay_ms = 200.0f; + flow_delay_ms = 5.0f; + range_delay_ms = 5.0f; // input noise gyro_noise = 6.0e-2f;