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();
+		//fuseHeading();
-		//fuseMag(_mag_fuse_index);
+		fuseMag(_mag_fuse_index);
-		//_mag_fuse_index = (_mag_fuse_index + 1) % 3;
+		_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)) {
@@ -151,6 +158,7 @@ bool Ekf::initialiseFilter(void)
 	// 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;
 	}
@@ -164,6 +172,7 @@ bool Ekf::initialiseFilter(void)
 
 	// 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);
@@ -172,6 +181,16 @@ bool Ekf::initialiseFilter(void)
 	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_height;	// baro height data should be fused
-	bool _fuse_pos;		// true if there is new position data from gps
+	bool _fuse_pos;		// gps position data should be fused
-	bool _fuse_vel;		// true if there is new velocity data from gps
+	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,20 +41,25 @@
 
 #include "ekf.h"
 
- void Ekf::fuseVelPosHeight()
+void Ekf::fuseVelPosHeight()
- {
+{
 	bool fuse_map[6] = {};
 	float innovations[6] = {};
 	float R[6] = {};
 	float Kfusion[24] = {};
+
 	// calculate innovations
- 	if (_fuse_vel) {
+	if (_fuse_hor_vel) {
- 		fuse_map[0] = fuse_map[1] = fuse_map[2] = true;
+		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);
- 		innovations[2] = _state.vel(2) - _gps_sample_delayed.vel(2);
 		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;
 	}
 
@@ -122,10 +127,10 @@
 		limitCov();
 	}
 
- }
+}
 
- void Ekf::fuse(float *K, float 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;
 	}
@@ -159,4 +164,4 @@
 	for (unsigned i = 0; i < 2; i++) {
 		_state.wind_vel(i) = _state.wind_vel(i) - K[i + 22] * innovation;
 	}
- }
+}