EKF: Use last known position when resuming OF aiding

EKF/control.cpp

@@ -279,6 +279,13 @@ void Ekf::controlOpticalFlowFusion()
 						_state.pos(0) = 0.0f;
 						_state.pos(1) = 0.0f;
 
+					} else {
+						// set to the last known position
+						_state.pos(0) = _last_known_posNE(0);
+						_state.pos(1) = _last_known_posNE(1);
+
+					}
+
 					// reset the corresponding covariances
 					// we are by definition at the origin at commencement so variances are also zeroed
 					zeroRows(P,7,8);
@@ -289,7 +296,6 @@ void Ekf::controlOpticalFlowFusion()
 
 				}
 			}
-			}
 
 		} else if (!(_params.fusion_mode & MASK_USE_OF)) {
 			_control_status.flags.opt_flow = false;