EKF: Rationalise use of rotation matrices and improve efficiency

EKF/ekf.cpp

@@ -89,7 +89,7 @@ Ekf::Ekf():
 	_state = {};
 	_last_known_posNE.setZero();
 	_earth_rate_NED.setZero();
-	_R_prev = matrix::Dcm<float>();
+	_R_to_earth = matrix::Dcm<float>();
 	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<float> 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();
 }
 

EKF/ekf.h

@@ -149,7 +149,7 @@ private:
 
 	Vector3f _earth_rate_NED;	// earth rotation vector (NED) in rad/s
 
-	matrix::Dcm<float> _R_prev;	// transformation matrix from earth frame to body frame of previous ekf step
+	matrix::Dcm<float> _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

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
 

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;

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);

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);