EKF: Enable use of rotated external nav estimates

EKF/common.h

@@ -174,6 +174,7 @@ struct dragSample {
 #define MASK_USE_EVPOS	(1<<3)		///< set to true to use external vision NED position data
 #define MASK_USE_EVYAW  (1<<4)		///< set to true to use exernal vision quaternion data for yaw
 #define MASK_USE_DRAG  (1<<5)		///< set to true to use the multi-rotor drag model to estimate wind
+#define MASK_ROTATE_EV  (1<<6)		///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used
 
 // Integer definitions for mag_fusion_type
 #define MAG_FUSE_TYPE_AUTO      0	///< The selection of either heading or 3D magnetometer fusion will be automatic

EKF/control.cpp

@@ -133,6 +133,7 @@ void Ekf::controlFusionModes()
 
 	// report dead reckoning if we are no longer fusing measurements that directly constrain velocity drift
 	_is_dead_reckoning = (_time_last_imu - _time_last_pos_fuse > _params.no_aid_timeout_max)
+			&& (_time_last_imu - _time_last_delpos_fuse > _params.no_aid_timeout_max)
 			&& (_time_last_imu - _time_last_vel_fuse > _params.no_aid_timeout_max)
 			&& (_time_last_imu - _time_last_of_fuse > _params.no_aid_timeout_max);
 
@@ -143,6 +144,13 @@ void Ekf::controlExternalVisionFusion()
 	// Check for new exernal vision data
 	if (_ev_data_ready) {
 
+		// if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames
+		// needs to be calculated and the observations rotated into the EKF frame of reference
+		if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !(_params.fusion_mode & MASK_USE_EVYAW)) {
+			// rotate EV measurements into the EKF Navigation frame
+			calcExtVisRotMat();
+		}
+
 		// external vision position aiding selection logic
 		if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align && _control_status.flags.yaw_align) {
 			// check for a exernal vision measurement that has fallen behind the fusion time horizon
@@ -154,12 +162,17 @@ void Ekf::controlExternalVisionFusion()
 				// reset the position if we are not already aiding using GPS, else use a relative position
 				// method for fusing the position data
 				if (_control_status.flags.gps) {
-					_hpos_odometry = true;
+					_fuse_hpos_as_odom = true;
 
 				} else {
 					resetPosition();
 					resetVelocity();
-					_hpos_odometry = false;
+					// we cannot use an absolue position from a rotating reference frame
+					if (_params.fusion_mode & MASK_ROTATE_EV) {
+						_fuse_hpos_as_odom = true;
+					} else {
+						_fuse_hpos_as_odom = false;
+					}
 
 				}
 			}
@@ -243,16 +256,57 @@ void Ekf::controlExternalVisionFusion()
 			_ev_sample_delayed.posNED(1) -= pos_offset_earth(1);
 			_ev_sample_delayed.posNED(2) -= pos_offset_earth(2);
 
-			// if GPS data is being used, then use an incremental position fusion method for EV data
-			if (_control_status.flags.gps) {
-				_hpos_odometry = true;
+			// Use an incremental position fusion method for EV data if using GPS or if the observations are not in NED
+			if (_control_status.flags.gps || (_params.fusion_mode & MASK_ROTATE_EV)) {
+				_fuse_hpos_as_odom = true;
+			} else {
+				_fuse_hpos_as_odom = false;
+			}
+
+			if(_fuse_hpos_as_odom) {
+				if(!_hpos_prev_available) {
+					// no previous observation available to calculate position change
+					_fuse_pos = false;
+					_hpos_prev_available = true;
+
+				} else {
+					// calculate the change in position since the last measurement
+					Vector3f ev_delta_pos = _ev_sample_delayed.posNED - _pos_meas_prev;
+
+					// rotate measurement into body frame if required
+					if (_params.fusion_mode & MASK_ROTATE_EV) {
+						ev_delta_pos = _ev_rot_mat * ev_delta_pos;
+					}
+
+					// use the change in position since the last measurement
+					_vel_pos_innov[3] = _state.pos(0) - _hpos_pred_prev(0) - ev_delta_pos(0);
+					_vel_pos_innov[4] = _state.pos(1) - _hpos_pred_prev(1) - ev_delta_pos(1);
+
+				}
+
+				// record observation and estimate for use next time
+				_pos_meas_prev = _ev_sample_delayed.posNED;
+				_hpos_pred_prev(0) = _state.pos(0);
+				_hpos_pred_prev(1) = _state.pos(1);
+
+			} else {
+				// use the absolute position
+				_vel_pos_innov[3] = _state.pos(0) - _ev_sample_delayed.posNED(0);
+				_vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1);
 			}
+
+			// observation 1-STD error
+			_posObsNoiseNE = fmaxf(_ev_sample_delayed.posErr, 0.01f);
+
+			// innovation gate size
+			_posInnovGateNE = fmaxf(_params.ev_innov_gate, 1.0f);
 		}
 
 		// Fuse available NED position data into the main filter
 		if (_fuse_height || _fuse_pos) {
 			fuseVelPosHeight();
 			_fuse_pos = _fuse_height = false;
+			_fuse_hpos_as_odom = false;
 
 		}
 
@@ -423,12 +477,16 @@ void Ekf::controlGpsFusion()
 		}
 
 		// handle the case when we now have GPS, but have not been using it for an extended period
-		if (_control_status.flags.gps && !_control_status.flags.opt_flow) {
-			// We are relying on GPS aiding to constrain attitude drift so after 7 seconds without aiding we need to do something
-			bool do_reset = (_time_last_imu - _time_last_pos_fuse > _params.no_gps_timeout_max) && (_time_last_imu - _time_last_vel_fuse > _params.no_gps_timeout_max);
+		if (_control_status.flags.gps) {
+			// We are relying on aiding to constrain drift so after a specified time
+			// with no aiding we need to do something
+			bool do_reset = (_time_last_imu - _time_last_pos_fuse > _params.no_gps_timeout_max)
+					&& (_time_last_imu - _time_last_delpos_fuse > _params.no_gps_timeout_max)
+					&& (_time_last_imu - _time_last_vel_fuse > _params.no_gps_timeout_max)
+					&& (_time_last_imu - _time_last_of_fuse > _params.no_gps_timeout_max);
 
-			// Our position measurments have been rejected for more than 14 seconds
-			do_reset |= _time_last_imu - _time_last_pos_fuse > 2 * _params.no_gps_timeout_max;
+			// We haven't had an absolute position fix for a longer time so need to do something
+			do_reset = do_reset || (_time_last_imu - _time_last_pos_fuse > 2 * _params.no_gps_timeout_max);
 
 			if (do_reset) {
 				// Reset states to the last GPS measurement
@@ -469,6 +527,19 @@ void Ekf::controlGpsFusion()
 			_gps_sample_delayed.pos(1) -= pos_offset_earth(1);
 			_gps_sample_delayed.hgt += pos_offset_earth(2);
 
+			// calculate observation process noise
+			float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
+			float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
+			_posObsNoiseNE = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit);
+
+			// calculate innovations
+			_vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0);
+			_vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1);
+
+			// set innovation gate size
+			_posInnovGateNE = fmaxf(_params.posNE_innov_gate, 1.0f);
+
+
 		}
 
 	} else if (_control_status.flags.gps && (_time_last_imu - _time_last_gps > 10e6)) {
@@ -1251,6 +1322,7 @@ void Ekf::controlVelPosFusion()
 			_last_known_posNE(0) = _state.pos(0);
 			_last_known_posNE(1) = _state.pos(1);
 			_state.vel.setZero();
+			_fuse_hpos_as_odom = false;
 			ECL_WARN("EKF stopping navigation");
 
 		}
@@ -1258,6 +1330,17 @@ void Ekf::controlVelPosFusion()
 		_fuse_pos = true;
 		_time_last_fake_gps = _time_last_imu;
 
+		if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
+			_posObsNoiseNE = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise);
+		} else {
+			_posObsNoiseNE = 0.5f;
+		}
+		_vel_pos_innov[3] = _state.pos(0) - _last_known_posNE(0);
+		_vel_pos_innov[4] = _state.pos(1) - _last_known_posNE(1);
+
+		// glitch protection is not required so set gate to a large value
+		_posInnovGateNE = 100.0f;
+
 	}
 
 	// Fuse available NED velocity and position data into the main filter

EKF/ekf.cpp

@@ -273,6 +273,11 @@ bool Ekf::initialiseFilter()
 		// calculate the initial magnetic field and yaw alignment
 		_control_status.flags.yaw_align = resetMagHeading(mag_init);
 
+		// initialise the rotation from EV to EKF navigation frame if required
+		if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !(_params.fusion_mode & MASK_USE_EVYAW)) {
+			resetExtVisRotMat();
+		}
+
 		if (_control_status.flags.rng_hgt) {
 			// if we are using the range finder as the primary source, then calculate the baro height at origin so  we can use baro as a backup
 			// so it can be used as a backup ad set the initial height using the range finder
@@ -297,6 +302,7 @@ bool Ekf::initialiseFilter()
 		// reset the essential fusion timeout counters
 		_time_last_hgt_fuse = _time_last_imu;
 		_time_last_pos_fuse = _time_last_imu;
+		_time_last_delpos_fuse = _time_last_imu;
 		_time_last_vel_fuse = _time_last_imu;
 		_time_last_hagl_fuse = _time_last_imu;
 		_time_last_of_fuse = _time_last_imu;

EKF/ekf.h

@@ -226,6 +226,9 @@ public:
 	// return a bitmask integer that describes which state estimates can be used for flight control
 	void get_ekf_soln_status(uint16_t *status);
 
+	// return the quaternion defining the rotation from the EKF to the External Vision reference frame
+	void get_ekf2ev_quaternion(float *quat);
+
 private:
 
 	static constexpr uint8_t _k_num_states{24};		///< number of EKF states
@@ -258,11 +261,17 @@ private:
 	bool _fuse_hor_vel{false};	///< true when gps horizontal velocity measurement should be fused
 	bool _fuse_vert_vel{false};	///< true when gps vertical velocity measurement should be fused
 
+	float _posObsNoiseNE;		///< 1-STD observtion noise used for the fusion of NE position data (m)
+	float _posInnovGateNE;		///< Number of standard deviations used for the NE position fusion innovation consistency check
+
 	// variables used when position data is being fused using a relative position odometry model
-	bool _hpos_odometry{false};		///< true when the NE position data is being fused using an odometry assumption
-	Vector2f _hpos_meas_prev;		///< previous value of NE position measurement fused using odometry assumption (m)
+	bool _fuse_hpos_as_odom{false};		///< true when the NE position data is being fused using an odometry assumption
+	Vector3f _pos_meas_prev;		///< previous value of NED position measurement fused using odometry assumption (m)
 	Vector2f _hpos_pred_prev;		///< previous value of NE position state used by odometry fusion (m)
 	bool _hpos_prev_available{false};	///< true when previous values of the estimate and measurement are available for use
+	Vector3f _ev_rot_vec_filt;		///< filtered rotation vector defining the rotation from EKF to EV reference (rad)
+	Dcmf _ev_rot_mat;			///< transformation matrix that rotates observations from the EV to the EKF navigation frame
+	uint64_t _ev_rot_last_time_us{0};	///< previous time that the calculation of the ekf to ev rotation matrix was updated (uSec)
 
 	// booleans true when fresh sensor data is available at the fusion time horizon
 	bool _gps_data_ready{false};	///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
@@ -276,6 +285,7 @@ private:
 	uint64_t _time_last_fake_gps{0};	///< last time we faked GPS position measurements to constrain tilt errors during operation without external aiding (uSec)
 
 	uint64_t _time_last_pos_fuse{0};	///< time the last fusion of horizontal position measurements was performed (uSec)
+	uint64_t _time_last_delpos_fuse{0};	///< time the last fusion of incremental horizontal position measurements was performed (uSec)
 	uint64_t _time_last_vel_fuse{0};	///< time the last fusion of velocity measurements was performed (uSec)
 	uint64_t _time_last_hgt_fuse{0};	///< time the last fusion of height measurements was performed (uSec)
 	uint64_t _time_last_of_fuse{0};		///< time the last fusion of optical flow measurements were performed (uSec)
@@ -487,6 +497,15 @@ private:
 	// modify output filter to match the the EKF state at the fusion time horizon
 	void alignOutputFilter();
 
+	// update the estimated angular misalignment vector between the EV naigration frame and the EKF navigation frame
+	// and update the rotation matrix which transforms EV navigation frame measurements into NED
+	void calcExtVisRotMat();
+
+
+	// reset the estimated angular misalignment vector between the EV naigration frame and the EKF navigation frame
+	// and reset the rotation matrix which transforms EV navigation frame measurements into NED
+	void resetExtVisRotMat();
+
 	// limit the diagonal of the covariance matrix
 	void fixCovarianceErrors();
 

EKF/ekf_helper.cpp

@@ -607,6 +607,11 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
 	// update transformation matrix from body to world frame using the current estimate
 	_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
 
+	// reset the rotation from the EV to EKF frame of reference if it is being used
+	if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !(_params.fusion_mode & MASK_USE_EVYAW)) {
+		resetExtVisRotMat();
+	}
+
 	// update the yaw angle variance using the variance of the measurement
 	if (_params.fusion_mode & MASK_USE_EVYAW) {
 		// using error estimate from external vision data
@@ -1182,7 +1187,9 @@ bool Ekf::global_position_is_valid()
 bool Ekf::inertial_dead_reckoning()
 {
 	bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos)
-			    && ((_time_last_imu - _time_last_pos_fuse <= _params.no_aid_timeout_max) || (_time_last_imu - _time_last_vel_fuse <= _params.no_aid_timeout_max));
+			    && ((_time_last_imu - _time_last_pos_fuse <= _params.no_aid_timeout_max)
+				|| (_time_last_imu - _time_last_vel_fuse <= _params.no_aid_timeout_max)
+				|| (_time_last_imu - _time_last_delpos_fuse <= _params.no_aid_timeout_max));
 	bool optFlowAiding = _control_status.flags.opt_flow && (_time_last_imu - _time_last_of_fuse <= _params.no_aid_timeout_max);
 	bool airDataAiding = _control_status.flags.wind && (_time_last_imu - _time_last_arsp_fuse <= _params.no_aid_timeout_max) && (_time_last_imu - _time_last_beta_fuse <= _params.no_aid_timeout_max);
 
@@ -1422,3 +1429,111 @@ void Ekf::setControlEVHeight()
 	_control_status.flags.gps_hgt = false;
 	_control_status.flags.rng_hgt = false;
 }
+
+// update the estimated misalignment between the EV navigation frame and the EKF navigation frame
+// and calculate a rotation matrix which rotates EV measurements into the EKF's navigatin frame
+void Ekf::calcExtVisRotMat()
+{
+	// calculate the quaternion delta between the EKF and EV reference frames at the EKF fusion time horizon
+	Quatf quat_inv = _ev_sample_delayed.quat.inversed();
+	Quatf q_error =   quat_inv * _state.quat_nominal;
+	q_error.normalize();
+
+	// convert to a delta angle and apply a spike and low pass filter
+	Vector3f rot_vec;
+	float delta;
+	float scaler;
+	if (q_error(0) >= 0.0f) {
+	    delta = 2 * acosf(q_error(0));
+	    if (delta > 1e-6f) {
+		    scaler = 1.0f /  sinf(delta/2);
+	    } else {
+		    // The rotation is too small to calculate a vector accurately
+		    // Make the rotation vector zero
+		    scaler = 0.0f;
+	    }
+	} else {
+	    delta = 2 * acosf(-q_error(0));
+	    if (delta > 1e-6f) {
+		   scaler = -1.0f /  sinf(delta/2);
+	    } else {
+		    // The rotation is too small to calculate a vector accurately
+		    // Make the rotation vector zero
+		    scaler = 0.0f;
+	    }
+	}
+	rot_vec(0) = q_error(2) * scaler;
+	rot_vec(1) = q_error(3) * scaler;
+	rot_vec(2) = q_error(4) * scaler;
+
+	float rot_vec_norm = rot_vec.norm();
+	if (rot_vec_norm > 1e-6f) {
+		// ensure magnitude of rotation matches the quaternion
+		rot_vec = rot_vec * (delta / rot_vec_norm);
+
+		// apply an input limiter to protect from spikes
+		Vector3f _input_delta_vec = rot_vec - _ev_rot_vec_filt;
+		float input_delta_mag = _input_delta_vec.norm();
+		if (input_delta_mag > 0.1f) {
+			rot_vec = _ev_rot_vec_filt + rot_vec * (0.1f / input_delta_mag);
+		}
+
+		// Apply a first order IIR low pass filter
+		const float omega_lpf_us = 0.2e-6f; // cutoff frequency in rad/uSec
+		float alpha = math::constrain(omega_lpf_us * (float)(_time_last_imu - _ev_rot_last_time_us) , 0.0f , 1.0f);
+		_ev_rot_last_time_us = _time_last_imu;
+		_ev_rot_vec_filt = _ev_rot_vec_filt * (1.0f - alpha) + rot_vec * alpha;
+
+	}
+
+	// convert filtered vector to a quaternion and then to a rotation matrix
+	q_error.from_axis_angle(_ev_rot_vec_filt);
+	_ev_rot_mat = quat_to_invrotmat(q_error);
+
+}
+
+// reset the estimated misalignment between the EV navigation frame and the EKF navigation frame
+// and update the rotation matrix which rotates EV measurements into the EKF's navigation frame
+void Ekf::resetExtVisRotMat()
+{
+	// calculate the quaternion delta between the EKF and EV reference frames at the EKF fusion time horizon
+	Quatf quat_inv = _ev_sample_delayed.quat.inversed();
+	Quatf q_error =   quat_inv * _state.quat_nominal;
+	q_error.normalize();
+
+	// convert to a delta angle and reset
+	Vector3f rot_vec;
+	float delta;
+	if (q_error(0) >= 0.0f) {
+	    delta = 2 * acosf(q_error(0));
+	    rot_vec(0) = q_error(1) / sinf(delta/2);
+	    rot_vec(1) = q_error(2) / sinf(delta/2);
+	    rot_vec(2) = q_error(3) / sinf(delta/2);
+	} else {
+	    delta = 2 * acosf(-q_error(1));
+	    rot_vec(0) = -q_error(2) / sinf(delta/2);
+	    rot_vec(1) = -q_error(3) / sinf(delta/2);
+	    rot_vec(2) = -q_error(4) / sinf(delta/2);
+	}
+
+	float rot_vec_norm = rot_vec.norm();
+	if (rot_vec_norm > 1e-9f) {
+		_ev_rot_vec_filt = rot_vec * delta / rot_vec_norm;
+	} else {
+		_ev_rot_vec_filt.zero();
+	}
+
+	// reset the rotation matrix
+	_ev_rot_mat = quat_to_invrotmat(q_error);
+}
+
+// return the quaternions for the rotation from the EKF to the External Vision system frame of reference
+void Ekf::get_ekf2ev_quaternion(float *quat)
+{
+	Quatf quat_ekf2ev;
+	quat_ekf2ev.from_axis_angle(_ev_rot_vec_filt);
+	for (unsigned i = 0; i < 4; i++) {
+		quat[i] = quat_ekf2ev(i);
+	}
+}
+

EKF/estimator_interface.h

@@ -236,6 +236,9 @@ public:
 		}
 	}
 
+	// return the quaternion defining the rotation from the EKF to the External Vision reference frame
+	virtual void get_ekf2ev_quaternion(float *quat) = 0;
+
 	// get the velocity of the body frame origin in local NED earth frame
 	void get_velocity(float *vel)
 	{

EKF/vel_pos_fusion.cpp

@@ -82,76 +82,12 @@ void Ekf::fuseVelPosHeight()
 	}
 
 	if (_fuse_pos) {
+		// enable fusion for the NE position axes
 		fuse_map[3] = fuse_map[4] = true;
 
-		// Calculate innovations and observation variance depending on type of observations
-		// being used
-		if (_control_status.flags.gps) {
-			// we are using GPS measurements
-			float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
-			float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
-			R[3] = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit);
-			_vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0);
-			_vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1);
-
-			// innovation gate size
-			gate_size[3] = fmaxf(_params.posNE_innov_gate, 1.0f);
-
-
-		} else if (_control_status.flags.ev_pos) {
-			// calculate innovations
-			if(_hpos_odometry) {
-				if(!_hpos_prev_available) {
-					// no previous observation available to calculate position change
-					fuse_map[3] = fuse_map[4] = false;
-					_hpos_prev_available = true;
-
-				} else {
-					// use the change in position since the last measurement
-					_vel_pos_innov[3] = _state.pos(0) - _hpos_pred_prev(0) - _ev_sample_delayed.posNED(0) + _hpos_meas_prev(0);
-					_vel_pos_innov[4] = _state.pos(1) - _hpos_pred_prev(1) - _ev_sample_delayed.posNED(1) + _hpos_meas_prev(1);
-
-				}
-
-				// record observation and estimate for use next time
-				_hpos_meas_prev(0) = _ev_sample_delayed.posNED(0);
-				_hpos_meas_prev(1) = _ev_sample_delayed.posNED(1);
-				_hpos_pred_prev(0) = _state.pos(0);
-				_hpos_pred_prev(1) = _state.pos(1);
-
-			} else {
-				// use the absolute position
-				_vel_pos_innov[3] = _state.pos(0) - _ev_sample_delayed.posNED(0);
-				_vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1);
-			}
-
-			// observation 1-STD error
-			R[3] = fmaxf(_ev_sample_delayed.posErr, 0.01f);
-
-			// innovation gate size
-			gate_size[3] = fmaxf(_params.ev_innov_gate, 1.0f);
-
-		} else {
-			// No observations - use a static position to constrain drift
-			if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
-				R[3] = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise);
-			} else {
-				R[3] = 0.5f;
-			}
-			_vel_pos_innov[3] = _state.pos(0) - _last_known_posNE(0);
-			_vel_pos_innov[4] = _state.pos(1) - _last_known_posNE(1);
-
-			// glitch protection is not required so set gate to a large value
-			gate_size[3] = 100.0f;
-
-		}
-
-		// convert North position noise to variance
-		R[3] = R[3] * R[3];
-
-		// copy North axis values to East axis
-		R[4] = R[3];
-		gate_size[4] = gate_size[3];
+		// Set observation noise variance and innovation consistency check gate size for the NE position observations
+		R[4] = R[3] = sq(_posObsNoiseNE);
+		gate_size[4] = gate_size[3] = _posInnovGateNE;
 
 	}
 
@@ -234,7 +170,11 @@ void Ekf::fuseVelPosHeight()
 
 	// record the successful position fusion event
 	if (pos_check_pass && _fuse_pos) {
-		_time_last_pos_fuse = _time_last_imu;
+		if (!_fuse_hpos_as_odom) {
+			_time_last_pos_fuse = _time_last_imu;
+		} else {
+			_time_last_delpos_fuse = _time_last_imu;
+		}
 		_innov_check_fail_status.flags.reject_pos_NE = false;
 	} else if (!pos_check_pass) {
 		_innov_check_fail_status.flags.reject_pos_NE = true;