Remove filtering on external vision alignment

EKF/control.cpp

@@ -205,14 +205,6 @@ void Ekf::controlExternalVisionFusion()
 					resetVelocity();
 					ECL_INFO_TIMESTAMPED("commencing external vision velocity fusion");
 				}
-
-				if ((_params.fusion_mode & MASK_ROTATE_EV) && !(_params.fusion_mode & MASK_USE_EVYAW)
-					&& !_R_ev_to_ekf_initialised)  {
-					// Reset transformation between EV and EKF navigation frames when starting fusion
-					resetExtVisRotMat();
-					_R_ev_to_ekf_initialised = true;
-					ECL_INFO_TIMESTAMPED("external vision aligned");
-				}
 			}
 		}
 

EKF/ekf.h

@@ -319,10 +319,7 @@ private:
 	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
-	AxisAnglef _ev_rot_vec_filt;		///< filtered rotation vector defining the rotation EV to EKF reference, initiliazied to zero rotation  (rad)
 	Dcmf _R_ev_to_ekf;			///< transformation matrix that rotates observations from the EV to the EKF navigation frame, initialized with Identity
-	uint64_t _ev_rot_last_time_us{0};	///< previous time that the calculation of the EV to EKF rotation matrix was updated (uSec)
-	bool _R_ev_to_ekf_initialised{0};	///< _R_ev_to_ekf should only be initialised once in the beginning through the reset function
 
 	// 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
@@ -615,15 +612,9 @@ 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
+	// 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
 	// force symmetry when the argument is true
 	void fixCovarianceErrors(bool force_symmetry);

EKF/ekf_helper.cpp

@@ -696,11 +696,6 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
 	// record the state change
 	_state_reset_status.quat_change = q_error;
 
-	// reset the rotation from the EV to EKF frame of reference if it is being used
-	if ((_params.fusion_mode & MASK_ROTATE_EV) && !_control_status.flags.ev_yaw) {
-		resetExtVisRotMat();
-	}
-
 	if (increase_yaw_var) {
 		// update the yaw angle variance using the variance of the measurement
 		if (_control_status.flags.ev_yaw) {
@@ -1657,64 +1652,18 @@ void Ekf::startMag3DFusion()
 	}
 }
 
-// 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 navigation frame
+// update the rotation matrix which rotates EV measurements into the EKF's navigation frame
 void Ekf::calcExtVisRotMat()
 {
 	// Calculate the quaternion delta that rotates from the EV to the EKF reference frame at the EKF fusion time horizon.
 	const Quatf q_error((_state.quat_nominal * _ev_sample_delayed.quat.inversed()).normalized());
-
-	// convert to a delta angle and apply a spike and low pass filter
-	AxisAnglef rot_vec(q_error);
-
-	if (rot_vec.norm() > 1e-6f) {
-
-		// apply an input limiter to protect from spikes
-		const Vector3f _input_delta_vec = rot_vec - _ev_rot_vec_filt;
-		const float input_delta_len = _input_delta_vec.norm();
-
-		if (input_delta_len > 0.1f) {
-			rot_vec = _ev_rot_vec_filt + _input_delta_vec * (0.1f / input_delta_len);
-		}
-
-		// 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;
-
-	}
-
-	_R_ev_to_ekf = Dcmf(_ev_rot_vec_filt);
-
-}
-
-// 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 that rotates from the EV to the EKF reference frame at the EKF fusion time horizon.
-	Quatf q_error = _state.quat_nominal * _ev_sample_delayed.quat.inversed();
-	q_error.normalize();
-
-	AxisAnglef rot_vec(q_error);
-
-	float rot_vec_norm = rot_vec.norm();
-
-	if (rot_vec_norm > 1e-9f) {
-		_ev_rot_vec_filt = rot_vec;
-
-	} else {
-		_ev_rot_vec_filt.zero();
-	}
-
 	_R_ev_to_ekf = Dcmf(q_error);
 }
 
 // return the quaternions for the rotation from External Vision system reference frame to the EKF reference frame
 void Ekf::get_ev2ekf_quaternion(float *quat)
 {
-	const Quatf quat_ev2ekf(_ev_rot_vec_filt);
+	const Quatf quat_ev2ekf(_R_ev_to_ekf);
 
 	for (unsigned i = 0; i < 4; i++) {
 		quat[i] = quat_ev2ekf(i);

EKF/gps_yaw_fusion.cpp

@@ -393,11 +393,6 @@ bool Ekf::resetGpsAntYaw()
 			// update transformation matrix from body to world frame using the current estimate
 			_R_to_earth = Dcmf(_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)) {
-				resetExtVisRotMat();
-			}
-
 			// update the yaw angle variance using the variance of the measurement
 			increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)));