EKF: update initial angle alignment check

EKF/control.cpp

@@ -51,13 +51,9 @@ void Ekf::controlFusionModes()
 	// Get the magnetic declination
 	calcMagDeclination();
 
-	// Check for tilt convergence during initial alignment
-	// filter the tilt error vector using a 1 sec time constant LPF
-	float filt_coef = 1.0f * _imu_sample_delayed.delta_ang_dt;
-	_tilt_err_length_filt = filt_coef * _tilt_err_vec.norm() + (1.0f - filt_coef) * _tilt_err_length_filt;
-
-	// Once the tilt error has reduced sufficiently, initialise the yaw and magnetic field states
-	if (_tilt_err_length_filt < 0.005f && !_control_status.flags.tilt_align) {
+	// Once the angular uncertainty has reduced sufficiently, initialise the yaw and magnetic field states
+	float total_angle_variance = P[0][0] + P[1][1] + P[2][2] + P[3][3];
+	if (total_angle_variance < 0.001f && !_control_status.flags.tilt_align) {
 		_control_status.flags.tilt_align = true;
 		_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
 	}

EKF/ekf.cpp

@@ -470,9 +470,6 @@ bool Ekf::initialiseFilter(void)
 		// update transformation matrix from body to world frame
 		_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
 
-		// initialise the filtered alignment error estimate to a larger starting value
-		_tilt_err_length_filt = 1.0f;
-
 		// calculate the averaged magnetometer reading
 		Vector3f mag_init = _mag_filt_state;
 

EKF/ekf.h

@@ -180,9 +180,6 @@ private:
 	float _heading_innov;		// heading measurement innovation
 	float _heading_innov_var;	// heading measurement innovation variance
 
-	Vector3f _tilt_err_vec;         // Vector of the most recent attitude error correction from velocity and position fusion
-	float _tilt_err_length_filt;    // filtered length of _tilt_err_vec
-
 	// optical flow processing
 	float _flow_innov[2];		// flow measurement innovation
 	float _flow_innov_var[2];	// flow innovation variance

EKF/vel_pos_fusion.cpp

@@ -176,13 +176,11 @@ void Ekf::fuseVelPosHeight()
 	// record the successful velocity fusion time
 	if (vel_check_pass && _fuse_hor_vel) {
 		_time_last_vel_fuse = _time_last_imu;
-		_tilt_err_vec.setZero();
 	}
 
 	// record the successful position fusion time
 	if (pos_check_pass && _fuse_pos) {
 		_time_last_pos_fuse = _time_last_imu;
-		_tilt_err_vec.setZero();
 	}
 
 	// record the successful height fusion time