Merge pull request #7 from PX4/ekf2_fixes

Ekf2 fixes

EKF/ekf.cpp

@@ -47,7 +47,8 @@ Ekf::Ekf():
 	_earth_rate_initialised(false),
 	_fuse_height(false),
 	_fuse_pos(false),
-	_fuse_vel(false),
+	_fuse_hor_vel(false),
+	_fuse_vert_vel(false),
 	_mag_fuse_index(0),
 	_time_last_fake_gps(0)
 {
@@ -67,12 +68,15 @@ Ekf::~Ekf()
 bool Ekf::update()
 {
 	bool ret = false;	// indicates if there has been an update
+
 	if (!_filter_initialised) {
 		_filter_initialised = initialiseFilter();
+
 		if (!_filter_initialised) {
 			return false;
 		}
 	}
+
 	//printStates();
 	//printStatesFast();
 	// prediction
@@ -85,9 +89,9 @@ bool Ekf::update()
 	// measurement updates
 
 	if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
-		fuseHeading();
-		//fuseMag(_mag_fuse_index);
-		//_mag_fuse_index = (_mag_fuse_index + 1) % 3;
+		//fuseHeading();
+		fuseMag(_mag_fuse_index);
+		_mag_fuse_index = (_mag_fuse_index + 1) % 3;
 	}
 
 	if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
@@ -96,16 +100,19 @@ bool Ekf::update()
 
 	if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed)) {
 		_fuse_pos = true;
-		_fuse_vel = true;
+		_fuse_vert_vel = true;
+		_fuse_hor_vel = true;
+
 	} else if (_time_last_imu - _time_last_gps > 2000000 && _time_last_imu - _time_last_fake_gps > 70000) {
-		_fuse_vel = true;
+		// if gps does not update then fake position and horizontal veloctiy measurements
+		_fuse_hor_vel = true;	// we only fake horizontal velocity because we still have baro
 		_gps_sample_delayed.vel.setZero();
 	}
 
 
-	if (_fuse_height || _fuse_pos || _fuse_vel) {
+	if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) {
 		fuseVelPosHeight();
-		_fuse_vel = _fuse_pos = _fuse_height = false;
+		_fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false;
 	}
 
 	if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) {
@@ -133,7 +140,7 @@ bool Ekf::initialiseFilter(void)
 	_state.mag_B.setZero();
 	_state.wind_vel.setZero();
 
-    // get initial roll and pitch estimate from accel vector, assuming vehicle is static
+	// get initial roll and pitch estimate from accel vector, assuming vehicle is static
 	Vector3f accel_init = _imu_down_sampled.delta_vel / _imu_down_sampled.delta_vel_dt;
 
 	float pitch = 0.0f;
@@ -146,32 +153,44 @@ bool Ekf::initialiseFilter(void)
 		roll = -asinf(accel_init(1) / cosf(pitch));
 	}
 
-    matrix::Euler<float> euler_init(roll, pitch, 0.0f);
+	matrix::Euler<float> euler_init(roll, pitch, 0.0f);
+
+	// Get the latest magnetic field measurement.
+	// If we don't have a measurement then we cannot initialise the filter
+	magSample mag_init = _mag_buffer.get_newest();
 
-    // Get the latest magnetic field measurement.
-    // If we don't have a measurement then we cannot initialise the filter
-    magSample mag_init = _mag_buffer.get_newest();
 	if (mag_init.time_us == 0) {
 		return false;
 	}
 
-    // rotate magnetic field into earth frame assuming zero yaw and estimate yaw angle assuming zero declination
-    // TODO use declination if available
-    matrix::Dcm<float> R_to_earth_zeroyaw(euler_init);
-    Vector3f mag_ef_zeroyaw = R_to_earth_zeroyaw * mag_init.mag;
-    float declination = 0.0f;
-    euler_init(2) = declination - atan2f(mag_ef_zeroyaw(1), mag_ef_zeroyaw(0));
+	// rotate magnetic field into earth frame assuming zero yaw and estimate yaw angle assuming zero declination
+	// TODO use declination if available
+	matrix::Dcm<float> R_to_earth_zeroyaw(euler_init);
+	Vector3f mag_ef_zeroyaw = R_to_earth_zeroyaw * mag_init.mag;
+	float declination = 0.0f;
+	euler_init(2) = declination - atan2f(mag_ef_zeroyaw(1), mag_ef_zeroyaw(0));
 
-    // calculate initial quaternion states
-    _state.quat_nominal = Quaternion(euler_init);
+	// calculate initial quaternion states
+	_state.quat_nominal = Quaternion(euler_init);
+	_output_new.quat_nominal = _state.quat_nominal;
 
-    // calculate initial earth magnetic field states
-    matrix::Dcm<float> R_to_earth(euler_init);
-    _state.mag_I = R_to_earth * mag_init.mag;
+	// calculate initial earth magnetic field states
+	matrix::Dcm<float> R_to_earth(euler_init);
+	_state.mag_I = R_to_earth * mag_init.mag;
 
-    resetVelocity();
+	resetVelocity();
 	resetPosition();
 
+	// initialize vertical position with newest baro measurement
+	baroSample baro_init = _baro_buffer.get_newest();
+
+	if (baro_init.time_us == 0) {
+		return false;
+	}
+
+	_state.pos(2) = -baro_init.hgt;
+	_output_new.pos(2) = -baro_init.hgt;
+
 	initialiseCovariance();
 
 	return true;
@@ -181,14 +200,15 @@ void Ekf::predictState()
 {
 	if (!_earth_rate_initialised) {
 		if (_gps_initialised) {
-			calcEarthRateNED(_earth_rate_NED, _posRef.lat_rad );
+			calcEarthRateNED(_earth_rate_NED, _posRef.lat_rad);
 			_earth_rate_initialised = true;
 		}
 	}
 
 	// attitude error state prediciton
 	matrix::Dcm<float> R_to_earth(_state.quat_nominal);	// transformation matrix from body to world frame
-	Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _R_prev * _earth_rate_NED * _imu_sample_delayed.delta_ang_dt;
+	Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _R_prev * _earth_rate_NED *
+				       _imu_sample_delayed.delta_ang_dt;
 	Quaternion dq;	// delta quaternion since last update
 	dq.from_axis_angle(corrected_delta_ang);
 	_state.quat_nominal = dq * _state.quat_nominal;
@@ -245,8 +265,7 @@ void Ekf::calculateOutputStates()
 		_imu_updated = false;
 	}
 
-	if (!_output_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_output_sample_delayed))
-	{
+	if (!_output_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_output_sample_delayed)) {
 		return;
 	}
 
@@ -259,6 +278,7 @@ void Ekf::calculateOutputStates()
 
 	if (q_error(0) >= 0.0f) {
 		scalar = -2.0f;
+
 	} else {
 		scalar = 2.0f;
 	}
@@ -292,7 +312,8 @@ void Ekf::printStates()
 
 	if (counter % 50 == 0) {
 		printf("quaternion\n");
-		for(int i = 0; i < 4; i++) {
+
+		for (int i = 0; i < 4; i++) {
 			printf("quat %i %.5f\n", i, (double)_state.quat_nominal(i));
 		}
 
@@ -300,31 +321,38 @@ void Ekf::printStates()
 		printf("yaw pitch roll %.5f %.5f %.5f\n", (double)euler(2), (double)euler(1), (double)euler(0));
 
 		printf("vel\n");
-		for(int i = 0; i < 3; i++) {
+
+		for (int i = 0; i < 3; i++) {
 			printf("v %i %.5f\n", i, (double)_state.vel(i));
 		}
 
 		printf("pos\n");
-		for(int i = 0; i < 3; i++) {
+
+		for (int i = 0; i < 3; i++) {
 			printf("p %i %.5f\n", i, (double)_state.pos(i));
 		}
 
 		printf("gyro_scale\n");
-		for(int i = 0; i < 3; i++) {
+
+		for (int i = 0; i < 3; i++) {
 			printf("gs %i %.5f\n", i, (double)_state.gyro_scale(i));
 		}
 
 		printf("mag earth\n");
-		for(int i = 0; i < 3; i++) {
+
+		for (int i = 0; i < 3; i++) {
 			printf("mI %i %.5f\n", i, (double)_state.mag_I(i));
 		}
 
 		printf("mag bias\n");
-		for(int i = 0; i < 3; i++) {
+
+		for (int i = 0; i < 3; i++) {
 			printf("mB %i %.5f\n", i, (double)_state.mag_B(i));
 		}
+
 		counter = 0;
 	}
+
 	counter++;
 
 }
@@ -335,21 +363,25 @@ void Ekf::printStatesFast()
 
 	if (counter_fast % 50 == 0) {
 		printf("quaternion\n");
-		for(int i = 0; i < 4; i++) {
+
+		for (int i = 0; i < 4; i++) {
 			printf("quat %i %.5f\n", i, (double)_output_new.quat_nominal(i));
 		}
 
 		printf("vel\n");
-		for(int i = 0; i < 3; i++) {
+
+		for (int i = 0; i < 3; i++) {
 			printf("v %i %.5f\n", i, (double)_output_new.vel(i));
 		}
 
 		printf("pos\n");
-		for(int i = 0; i < 3; i++) {
+
+		for (int i = 0; i < 3; i++) {
 			printf("p %i %.5f\n", i, (double)_output_new.pos(i));
 		}
 
 		counter_fast = 0;
 	}
+
 	counter_fast++;
 }

EKF/ekf.h

@@ -60,9 +60,10 @@ private:
 	bool _filter_initialised;
 	bool _earth_rate_initialised;
 
-	bool _fuse_height;	// true if there is new baro data
-	bool _fuse_pos;		// true if there is new position data from gps
-	bool _fuse_vel;		// true if there is new velocity data from gps
+	bool _fuse_height;	// baro height data should be fused
+	bool _fuse_pos;		// gps position data should be fused
+	bool _fuse_hor_vel;		// gps horizontal velocity measurement should be fused
+	bool _fuse_vert_vel;	// gps vertical velocity measurement should be fused
 
 	uint8_t _mag_fuse_index;	// counter for sequential mag axis fusion
 

EKF/ekf_helper.cpp

@@ -78,7 +78,7 @@ void Ekf::resetPosition()
 	}
 
 	baroSample baro_newest = _baro_buffer.get_newest();
-	_state.pos(2) = baro_newest.hgt;
+	_state.pos(2) = -baro_newest.hgt;
 }
 
 #if defined(__PX4_POSIX) && !defined(__PX4_QURT)

EKF/estimator_base.cpp

@@ -145,7 +145,7 @@ void EstimatorBase::setMagData(uint64_t time_usec, float *data)
 
 
 		mag_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2;
-		_time_last_mag = mag_sample_new.time_us;
+		_time_last_mag = time_usec;
 
 
 		memcpy(&mag_sample_new.mag._data[0], data, sizeof(mag_sample_new.mag._data));
@@ -168,7 +168,7 @@ void EstimatorBase::setGpsData(uint64_t time_usec, struct gps_message *gps)
 
 
 		gps_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2;
-		_time_last_gps = gps_sample_new.time_us;
+		_time_last_gps = time_usec;
 
 		_last_valid_gps_time_us = hrt_absolute_time();
 
@@ -200,7 +200,7 @@ void EstimatorBase::setBaroData(uint64_t time_usec, float *data)
 		baro_sample_new.time_us = time_usec - _params.baro_delay_ms * 1000;
 
 		baro_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2;
-		_time_last_baro = baro_sample_new.time_us;
+		_time_last_baro = time_usec;
 
 		baro_sample_new.time_us = math::max(baro_sample_new.time_us, _imu_sample_delayed.time_us);
 
@@ -216,7 +216,7 @@ void EstimatorBase::setAirspeedData(uint64_t time_usec, float *data)
 		airspeed_sample_new.time_us -= _params.airspeed_delay_ms * 1000;
 
 		airspeed_sample_new.time_us = time_usec -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2;
-		_time_last_airspeed = airspeed_sample_new.time_us;
+		_time_last_airspeed = time_usec;
 
 		_airspeed_buffer.push(airspeed_sample_new);
 	}

EKF/vel_pos_fusion.cpp

@@ -41,64 +41,69 @@
 
 #include "ekf.h"
 
- void Ekf::fuseVelPosHeight()
- {
- 	bool fuse_map[6] = {};
- 	float innovations[6] = {};
- 	float R[6] = {};
- 	float Kfusion[24] = {};
- 	// calculate innovations
- 	if (_fuse_vel) {
- 		fuse_map[0] = fuse_map[1] = fuse_map[2] = true;
- 		innovations[0] = _state.vel(0) - _gps_sample_delayed.vel(0);
- 		innovations[1] = _state.vel(1) - _gps_sample_delayed.vel(1);
- 		innovations[2] = _state.vel(2) - _gps_sample_delayed.vel(2);
- 		R[0] = _params.gps_vel_noise;
- 		R[1] = _params.gps_vel_noise;
- 		R[2] = _params.gps_vel_noise;
- 	}
-
- 	if (_fuse_pos) {
- 		fuse_map[3] = fuse_map[4] = true;
- 		innovations[3] = _state.pos(0) - _gps_sample_delayed.pos(0);
- 		innovations[4] = _state.pos(1) - _gps_sample_delayed.pos(1);
- 		R[3] = _params.gps_pos_noise;
- 		R[4] = _params.gps_pos_noise;
-
- 	}
-
- 	if (_fuse_height) {
- 		fuse_map[5] = true;
- 		innovations[5] = _state.pos(2) - (-_baro_sample_delayed.hgt);		// baro measurement has inversed z axis
- 		R[5] = _params.baro_noise;
- 	}
-
- 	// XXX Do checks here
-
- 	for (unsigned obs_index = 0; obs_index < 6; obs_index++) {
- 		if (!fuse_map[obs_index]) {
- 			continue;
- 		}
-
- 		unsigned state_index = obs_index + 3;	// we start with vx and this is the 4. state
-
- 		// compute the innovation variance SK = HPH + R
- 		float S = P[state_index][state_index] + R[obs_index];
- 		S = 1.0f / S;
-
- 		// calculate kalman gain K = PHS
- 		for (int row = 0; row < 24; row++) {
- 			Kfusion[row] = P[row][state_index] * S;
- 		}
- 
- 		// by definition the angle error state is zero at the fusion time
- 		_state.ang_error.setZero();
-
- 		// fuse the observation
- 		fuse(Kfusion, innovations[obs_index]);
-
- 		// correct the nominal quaternion
- 		Quaternion dq;
+void Ekf::fuseVelPosHeight()
+{
+	bool fuse_map[6] = {};
+	float innovations[6] = {};
+	float R[6] = {};
+	float Kfusion[24] = {};
+
+	// calculate innovations
+	if (_fuse_hor_vel) {
+		fuse_map[0] = fuse_map[1] = true;
+		innovations[0] = _state.vel(0) - _gps_sample_delayed.vel(0);
+		innovations[1] = _state.vel(1) - _gps_sample_delayed.vel(1);
+		R[0] = _params.gps_vel_noise;
+		R[1] = _params.gps_vel_noise;
+	}
+
+	if (_fuse_vert_vel) {
+		fuse_map[2] = true;
+		innovations[2] = _state.vel(2) - _gps_sample_delayed.vel(2);
+		R[2] = _params.gps_vel_noise;
+	}
+
+	if (_fuse_pos) {
+		fuse_map[3] = fuse_map[4] = true;
+		innovations[3] = _state.pos(0) - _gps_sample_delayed.pos(0);
+		innovations[4] = _state.pos(1) - _gps_sample_delayed.pos(1);
+		R[3] = _params.gps_pos_noise;
+		R[4] = _params.gps_pos_noise;
+
+	}
+
+	if (_fuse_height) {
+		fuse_map[5] = true;
+		innovations[5] = _state.pos(2) - (-_baro_sample_delayed.hgt);		// baro measurement has inversed z axis
+		R[5] = _params.baro_noise;
+	}
+
+	// XXX Do checks here
+
+	for (unsigned obs_index = 0; obs_index < 6; obs_index++) {
+		if (!fuse_map[obs_index]) {
+			continue;
+		}
+
+		unsigned state_index = obs_index + 3;	// we start with vx and this is the 4. state
+
+		// compute the innovation variance SK = HPH + R
+		float S = P[state_index][state_index] + R[obs_index];
+		S = 1.0f / S;
+
+		// calculate kalman gain K = PHS
+		for (int row = 0; row < 24; row++) {
+			Kfusion[row] = P[row][state_index] * S;
+		}
+
+		// by definition the angle error state is zero at the fusion time
+		_state.ang_error.setZero();
+
+		// fuse the observation
+		fuse(Kfusion, innovations[obs_index]);
+
+		// correct the nominal quaternion
+		Quaternion dq;
 		dq.from_axis_angle(_state.ang_error);
 		_state.quat_nominal = dq * _state.quat_nominal;
 		_state.quat_nominal.normalize();
@@ -120,43 +125,43 @@
 
 		makeSymmetrical();
 		limitCov();
- 	}
- 	
- }
-
- void Ekf::fuse(float *K, float innovation)
- {
- 		for (unsigned i = 0; i < 3; i++) {
- 			_state.ang_error(i) = _state.ang_error(i) - K[i] * innovation;
- 		}
-
- 		for (unsigned i = 0; i < 3; i++) {
- 			_state.vel(i) = _state.vel(i) - K[i + 3] * innovation;
- 		}
-
- 		for (unsigned i = 0; i < 3; i++) {
- 			_state.pos(i) = _state.pos(i) - K[i + 6] * innovation;
- 		}
-
- 		for (unsigned i = 0; i < 3; i++) {
- 			_state.gyro_bias(i) = _state.gyro_bias(i) - K[i + 9] * innovation;
- 		}
-
- 		for (unsigned i = 0; i < 3; i++) {
- 			_state.gyro_scale(i) = _state.gyro_scale(i) - K[i + 12] * innovation;
- 		}
-
- 		_state.accel_z_bias -= K[15] * innovation;
-
- 		for (unsigned i = 0; i < 3; i++) {
- 			_state.mag_I(i) = _state.mag_I(i) - K[i + 16] * innovation;
- 		}
-
- 		for (unsigned i = 0; i < 3; i++) {
- 			_state.mag_B(i) = _state.mag_B(i) - K[i + 19] * innovation;
- 		}
-
- 		for (unsigned i = 0; i < 2; i++) {
- 			_state.wind_vel(i) = _state.wind_vel(i) - K[i + 22] * innovation;
- 		}
- }
+	}
+
+}
+
+void Ekf::fuse(float *K, float innovation)
+{
+	for (unsigned i = 0; i < 3; i++) {
+		_state.ang_error(i) = _state.ang_error(i) - K[i] * innovation;
+	}
+
+	for (unsigned i = 0; i < 3; i++) {
+		_state.vel(i) = _state.vel(i) - K[i + 3] * innovation;
+	}
+
+	for (unsigned i = 0; i < 3; i++) {
+		_state.pos(i) = _state.pos(i) - K[i + 6] * innovation;
+	}
+
+	for (unsigned i = 0; i < 3; i++) {
+		_state.gyro_bias(i) = _state.gyro_bias(i) - K[i + 9] * innovation;
+	}
+
+	for (unsigned i = 0; i < 3; i++) {
+		_state.gyro_scale(i) = _state.gyro_scale(i) - K[i + 12] * innovation;
+	}
+
+	_state.accel_z_bias -= K[15] * innovation;
+
+	for (unsigned i = 0; i < 3; i++) {
+		_state.mag_I(i) = _state.mag_I(i) - K[i + 16] * innovation;
+	}
+
+	for (unsigned i = 0; i < 3; i++) {
+		_state.mag_B(i) = _state.mag_B(i) - K[i + 19] * innovation;
+	}
+
+	for (unsigned i = 0; i < 2; i++) {
+		_state.wind_vel(i) = _state.wind_vel(i) - K[i + 22] * innovation;
+	}
+}