Merge pull request #139 from PX4/pr-ekf2Alignment

EKF: Improve angular alignment consistency

EKF/control.cpp

@@ -54,14 +54,15 @@ void Ekf::controlFusionModes()
 		// whilst we are aligning the tilt, monitor the variances
 		Vector3f angle_err_var_vec = calcRotVecVariances();
 
-		// Once the tilt variances have reduced sufficiently, initialise the yaw and magnetic field states
-		if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < 0.002f) {
+		// Once the tilt variances have reduced to equivalent of 3deg uncertainty, re-set the yaw and magnetic field states
+		// and declare the tilt alignment complete
+		if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < sq(0.05235f)) {
 			_control_status.flags.tilt_align = true;
 			_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
 		}
 	}
 
-	// control use of various external souces for positon and velocity aiding
+	// control use of various external souces for position and velocity aiding
 	controlExternalVisionAiding();
 	controlOpticalFlowAiding();
 	controlGpsAiding();
@@ -480,7 +481,7 @@ void Ekf::controlMagAiding()
 	// or the more accurate 3-axis fusion
 	if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) {
 
-	if (_control_status.flags.in_air) {
+	if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
 		// if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states
 		if (!_control_status.flags.mag_3D) {
 			_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);

EKF/ekf.cpp

@@ -92,6 +92,7 @@ Ekf::Ekf():
 	_hgt_counter(0),
 	_rng_filt_state(0.0f),
 	_mag_counter(0),
+	_ev_counter(0),
 	_time_last_mag(0),
 	_hgt_sensor_offset(0.0f),
 	_terrain_vpos(0.0f),
@@ -387,7 +388,7 @@ bool Ekf::update()
 
 bool Ekf::initialiseFilter(void)
 {
-	// Keep accumulating measurements until we have a minimum of 10 samples for the baro and magnetoemter
+	// Keep accumulating measurements until we have a minimum of 10 samples for the required sensors
 
 	// Sum the IMU delta angle measurements
 	imuSample imu_init = _imu_buffer.get_newest();
@@ -406,6 +407,17 @@ bool Ekf::initialiseFilter(void)
 		}
 	}
 
+	// Count the number of external vision measurements received
+	if (_ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed)) {
+		if (_ev_counter == 0 && _ev_sample_delayed.time_us !=0) {
+			// initialise the counter
+			_ev_counter = 1;
+		} else if (_ev_counter != 0) {
+			// increment the sample count
+			_ev_counter ++;
+		}
+	}
+
 	// set the default height source from the adjustable parameter
 	if (_hgt_counter == 0) {
 		if (_params.fusion_mode & MASK_USE_EVPOS) {
@@ -454,7 +466,10 @@ bool Ekf::initialiseFilter(void)
 	}
 
 	// check to see if we have enough measurements and return false if not
-	if (_hgt_counter <= 10 || _mag_counter <= 10) {
+	bool hgt_count_fail = _hgt_counter <= 10;
+	bool mag_count_fail = _mag_counter <= 10;
+	bool ev_count_fail = ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVYAW)) && (_ev_counter <= 10);
+	if (hgt_count_fail || mag_count_fail || ev_count_fail) {
 		return false;
 
 	} else {
@@ -496,7 +511,7 @@ bool Ekf::initialiseFilter(void)
 		Vector3f mag_init = _mag_filt_state;
 
 		// calculate the initial magnetic field and yaw alignment
-		resetMagHeading(mag_init);
+		_control_status.flags.yaw_align = resetMagHeading(mag_init);
 
 		// 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

EKF/ekf.h

@@ -209,9 +209,10 @@ private:
 	float _gps_alt_ref;		// WGS-84 height (m)
 
 	// Variables used to initialise the filter states
-	uint32_t _hgt_counter;		// number of height samples taken
+	uint32_t _hgt_counter;		// number of height samples read during initialisation
 	float _rng_filt_state;		// filtered height measurement
-	uint32_t _mag_counter;		// number of magnetometer samples taken
+	uint32_t _mag_counter;		// number of magnetometer samples read during initialisation
+	uint32_t _ev_counter;		// number of exgernal vision samples read during initialisation
 	uint64_t _time_last_mag;	// measurement time of last magnetomter sample
 	Vector3f _mag_filt_state;	// filtered magnetometer measurement
 	Vector3f _delVel_sum;		// summed delta velocity

EKF/ekf_helper.cpp

@@ -407,8 +407,10 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
 	// reset the quaternion variances because the yaw angle could have changed by a significant amount
 	// by setting them to zero we avoid 'kicks' in angle when 3-D fusion starts and the imu process noise
 	// will grow them again.
-	zeroRows(P, 0, 3);
-	zeroCols(P, 0, 3);
+	if (_control_status.flags.tilt_align) {
+		zeroRows(P, 0, 3);
+		zeroCols(P, 0, 3);
+	}
 
 	// update transformation matrix from body to world frame using the current estimate
 	_R_to_earth = quat_to_invrotmat(_state.quat_nominal);

EKF/vel_pos_fusion.cpp

@@ -88,11 +88,10 @@ void Ekf::fuseVelPosHeight()
 		// being used
 		if (!_control_status.flags.gps && !_control_status.flags.ev_pos) {
 			// No observations - use a static position to constrain drift
-			if (_control_status.flags.in_air) {
+			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] = _params.gps_pos_noise;
+				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);