EKF: Compensate optical flow data for sensor position offset

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

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;
 

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)

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

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