Merge pull request #94 from priseborough/pr-sensorPosComp

Add compensation for EKF sensor position offsets

EKF/common.h

@@ -67,7 +67,7 @@ typedef matrix::Matrix<float, 3, 3> 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
 };
@@ -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()
 	{
@@ -235,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;
+		flow_delay_ms = 5.0f;
-		range_delay_ms = 200.0f;
+		range_delay_ms = 5.0f;
 
 		// input noise
 		gyro_noise = 6.0e-2f;
@@ -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};
 	}
 };
 

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,7 +217,12 @@ 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_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;
 
@@ -257,6 +262,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 = cross_product(ang_rate,pos_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_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);
+
 			}
 
 			// only use gps height observation in the main filter if specifically enabled
@@ -269,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;
 		}
 
@@ -431,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;
 
@@ -472,26 +494,42 @@ void Ekf::predictState()
 		}
 	}
 
-	// attitude error state prediction
+	// correct delta angles for earth rotation rate
-	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_to_earth.transpose() * _earth_rate_NED *
-	Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _R_prev * _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
+	// calculate the increment in velocity using the previous orientation
-	_state.vel += R_to_earth * _imu_sample_delayed.delta_vel;
+	Vector3f delta_vel_earth_1 = _R_to_earth * _imu_sample_delayed.delta_vel;
-	_state.vel(2) += 9.81f * _imu_sample_delayed.delta_vel_dt;
+
+	// 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;
 
+	// update transformation matrix from body to world frame
+	_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
+
 	constrainStates();
 }
 
@@ -543,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();
 
-	matrix::Dcm<float> R_to_earth(_output_new.quat_nominal);
+	// calculate the rotation matrix from body to earth frame
+	_R_to_earth_now = quat_to_invrotmat(_output_new.quat_nominal);
 
-	Vector3f delta_vel_NED = R_to_earth * delta_vel + _delta_vel_corr;
+	// rotate the delta velocity to earth frame
-	delta_vel_NED(2) += 9.81f * imu_new.delta_vel_dt;
+	// 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) += _gravity_mss * 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);
 }

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
 
@@ -149,7 +150,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
@@ -173,8 +174,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
+	Vector3f _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 _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)
@@ -330,4 +331,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);
+
 };

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;
+}

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;
+			optflow_sample_new.flowRadXYcomp(0) = optflow_sample_new.flowRadXY(0) - optflow_sample_new.gyroXYZ(0);
-			// convert integraton interval to seconds
+			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

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

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(0) = atan2f(-_R_to_earth(0, 1) , _R_to_earth(1, 1)); // first rotation (yaw)
-		euler312(1) = asinf(_R_prev(1, 2)); // second rotation (roll)
+		euler312(1) = asinf(_R_to_earth(2, 1)); // second rotation (roll)
-		euler312(2) = atan2f(-_R_prev(0, 2) , _R_prev(2, 2)); // third rotation (pitch)
+		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

@@ -87,8 +87,17 @@ void Ekf::fuseOptFlow()
 	matrix::Dcm<float> earth_to_body(_state.quat_nominal);
 	earth_to_body = earth_to_body.transpose();
 
-	// rotate earth velocities into body frame
+	// calculate the sensor position relative to the IMU
-	Vector3f vel_body = earth_to_body * _state.vel;
+	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_to_earth * 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 += _imu_sample_delayed.delta_ang;
-	_imu_del_ang_of(1) += _imu_sample_delayed.delta_ang(1);
 	_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;
+			Vector3f reference_body_rate;
-			reference_body_rate(0) = _imu_del_ang_of(0) / _delta_time_of;
+			reference_body_rate = _imu_del_ang_of * (1.0f / _delta_time_of);
-			reference_body_rate(1) = _imu_del_ang_of(1) / _delta_time_of;
 
 			// calculate the optical flow sensor measured body rate
-			Vector2f of_body_rate;
+			Vector3f of_body_rate;
-			of_body_rate(0) = _flow_sample_delayed.gyroXY(0) / _flow_sample_delayed.dt;
+			of_body_rate = _flow_sample_delayed.gyroXYZ * (1.0f / _flow_sample_delayed.dt);
-			of_body_rate(1) = _flow_sample_delayed.gyroXY(1) / _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

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