EKF: Prevent badly conditioned covariance calculation when starting or resetting to optical flow

EKF/control.cpp

@@ -73,18 +73,16 @@ void Ekf::controlFusionModes()
 			_control_status.flags.opt_flow = true;
 			_time_last_of_fuse = _time_last_imu;
 
-			// if we are not using GPS and are in air, then we need to reset the velocity to be consistent with the optical flow reading
+			// if we are not using GPS then the velocity and position states and covariances need to be set
 			if (!_control_status.flags.gps) {
-				// calculate the rotation matrix from body to earth frame
-				matrix::Dcm<float> body_to_earth(_state.quat_nominal);
-
 				// constrain height above ground to be above minimum possible
 				float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance);
 
 				// calculate absolute distance from focal point to centre of frame assuming a flat earth
-				float range = heightAboveGndEst / body_to_earth(2, 2);
+				float range = heightAboveGndEst / _R_to_earth(2, 2);
 
-				if (_in_air && (range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) {
+				if ((range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) {
+					// we should ahve reliable OF measurements so
 					// calculate X and Y body relative velocities from OF measurements
 					Vector3f vel_optflow_body;
 					vel_optflow_body(0) = - range * _flow_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt;
@@ -93,14 +91,30 @@ void Ekf::controlFusionModes()
 
 					// rotate from body to earth frame
 					Vector3f vel_optflow_earth;
-					vel_optflow_earth = body_to_earth * vel_optflow_body;
+					vel_optflow_earth = _R_to_earth * vel_optflow_body;
 
 					// take x and Y components
 					_state.vel(0) = vel_optflow_earth(0);
 					_state.vel(1) = vel_optflow_earth(1);
 
 				} else {
-					_state.vel.setZero();
+					_state.vel(0) = 0.0f;
+					_state.vel(1) = 0.0f;
+				}
+
+				// reset the velocity covariance terms
+				zeroRows(P,4,5);
+				zeroCols(P,4,5);
+
+				// reset the horizontal velocity variance using the optical flow noise
+				P[5][5] = P[4][4] = sq(range * _params.flow_noise_qual_min);
+
+				if (!_in_air) {
+					// we are likely starting OF for the first time so reset the position and states
+					_state.pos(0) = 0.0f;
+					_state.pos(1) = 0.0f;
+					// align the output observer to the EKF states
+					alignOutputFilter();
 				}
 			}
 		}

EKF/ekf.h

@@ -307,6 +307,9 @@ private:
 	// reset height state of the ekf
 	void resetHeight();
 
+	// modify output filter to match the the EKF state at the fusion time horizon
+	void alignOutputFilter();
+
 	// limit the diagonal of the covariance matrix
 	void fixCovarianceErrors();
 

EKF/ekf_helper.cpp

@@ -225,6 +225,31 @@ void Ekf::resetHeight()
 
 }
 
+// align output filter states to match EKF states at the fusion time horizon
+void Ekf::alignOutputFilter()
+{
+	// calculate the quaternion delta between the output and EKF quaternions at the EKF fusion time horizon
+	Quaternion quat_inv = _state.quat_nominal.inversed();
+	Quaternion q_delta =  _output_sample_delayed.quat_nominal * quat_inv;
+	q_delta.normalize();
+
+	// calculate the velocity and posiiton deltas between the output and EKF at the EKF fusion time horizon
+	Vector3f vel_delta = _state.vel - _output_sample_delayed.vel;
+	Vector3f pos_delta = _state.pos - _output_sample_delayed.pos;
+
+	// loop through the output filter state history and add the deltas
+	outputSample output_states;
+	unsigned output_length = _output_buffer.get_length();
+	for (unsigned i=0; i < output_length; i++) {
+		output_states = _output_buffer.get_from_index(i);
+		output_states.quat_nominal *= q_delta;
+		output_states.quat_nominal.normalize();
+		output_states.vel += vel_delta;
+		output_states.pos += pos_delta;
+		_output_buffer.push_to_index(i,output_states);
+	}
+}
+
 // Reset heading and magnetic field states
 bool Ekf::resetMagHeading(Vector3f &mag_init)
 {