implemented prediction of states and covariance matrix

EKF/ekf.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
+ *   Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,13 +33,14 @@
 
 /**
  * @file ekf.cpp
- * Definition of ekf attitude position estimator class.
+ * Core functions for ekf attitude and position estimator.
  *
- * @author Roman Bast <bastroman@gmail.com>
+ * @author Roman Bast <bapstroman@gmail.com>
  *
  */
 
 #include "ekf.h"
+#include <drivers/drv_hrt.h>
 
 Ekf::Ekf()
 {
@@ -58,10 +59,6 @@ void Ekf::update()
 		_filter_initialised = initialiseFilter();
 	}
 
-	if (!_filter_initialised) {
-		return;
-	}
-
 	// prediction
 	if (_imu_updated) {
 		predictState();
@@ -70,15 +67,15 @@ void Ekf::update()
 	}
 
 	// measurement updates
-	if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _mag_sample_delayed)) {
+	if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
 		fuseMag();
 	}
 
-	if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _baro_sample_delayed)) {
+	if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
 		_fuse_height = true;
 	}
 
-	if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _gps_sample_delayed)) {
+	if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed)) {
 		_fuse_pos = true;
 		_fuse_vel = true;
 	}
@@ -87,12 +84,94 @@ void Ekf::update()
 		fusePosVel();
 	}
 
-	if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _range_sample_delayed)) {
+	if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) {
 		fuseRange();
 	}
 
-	if (_airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _airspeed_sample_delayed)) {
+	if (_airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed)) {
 		fuseAirspeed();
 	}
 
-}
+}
+
+bool Ekf::initialiseFilter(void)
+{
+
+	_state.ang_error.setZero();
+	_state.vel.setZero();
+	_state.pos.setZero();
+	_state.gyro_bias.setZero();
+	_state.gyro_scale(0) = _state.gyro_scale(1) = _state.gyro_scale(2) = 1.0f;
+	_state.accel_z_bias = 0.0f;
+	_state.mag_I.setZero();
+	_state.mag_B.setZero();
+	_state.wind_vel.setZero();
+
+	// get initial attitude 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;
+	float roll = 0.0f;
+
+	if (accel_init.norm() > 0.001f) {
+		accel_init.normalize();
+
+		pitch = asinf(accel_init(0));
+		roll = -asinf(accel_init(1) / cosf(pitch));
+	}
+
+	matrix::Euler<float> euler_init(0, pitch, roll);
+	_state.quat_nominal = Quaternion(euler_init);
+
+	resetVelocity();
+	resetPosition();
+
+	initialiseCovariance();
+
+	return true;
+}
+
+void Ekf::predictState()
+{
+	// compute transformation matrix from body to world frame
+	matrix::Dcm<float> R(_state.quat_nominal);
+	R.transpose();
+
+	// attitude error state prediciton
+	Quaternion dq;
+	dq.from_axis_angle(_imu_sample_delayed.delta_ang);
+	_state.quat_nominal = dq * _state.quat_nominal;
+	_state.quat_nominal.normalize();
+
+	Vector3f vel_last = _state.vel;
+
+	// predict velocity states
+	_state.vel += R * _imu_sample_delayed.delta_vel;
+	_state.vel(2) += 9.81f * _imu_sample_delayed.delta_vel_dt;
+
+	// predict position states via trapezoidal integration of velocity
+	_state.pos += (vel_last + _state.vel) * _imu_sample_delayed.delta_vel_dt * 0.5f;
+
+	//matrix::Euler<float> euler(_state.quat_nominal);
+	//printf("roll pitch yaw %.5f %.5f %.5f\n", (double)euler(2), (double)euler(1), (double)euler(0));
+}
+
+
+void Ekf::fusePosVel()
+{
+
+}
+
+void Ekf::fuseMag()
+{
+}
+
+void Ekf::fuseAirspeed()
+{
+
+}
+
+void Ekf::fuseRange()
+{
+
+}

EKF/ekf.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
+ *   Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,9 +33,9 @@
 
 /**
  * @file ekf.h
- * Definition of ekf attitude position estimator class.
+ * Class for core functions for ekf attitude and position estimator.
  *
- * @author Roman Bast <bastroman@gmail.com>
+ * @author Roman Bast <bapstroman@gmail.com>
  *
  */
 
@@ -52,14 +52,19 @@ public:
 
 private:
 
+	static const uint8_t _kNumStates = 24;
 	bool _filter_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
 
+	float P[_kNumStates][_kNumStates];	// state covariance matrix
+
 	bool initialiseFilter(void);
 
+	void initialiseCovariance();
+
 	void predictState();
 
 	void predictCovariance();
@@ -72,5 +77,9 @@ private:
 
 	void fuseRange();
 
+	void resetVelocity();
+
+	void resetPosition();
+
 
-};
+};

EKF/estimator_base.cpp

@@ -35,7 +35,7 @@
  * @file estimator_base.cpp
  * Definition of base class for attitude estimators
  *
- * @author Roman Bast <bastroman@gmail.com>
+ * @author Roman Bast <bapstroman@gmail.com>
  *
  */
 
@@ -264,6 +264,21 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec)
 	_params.airspeed_delay_ms = 0;
 	_params.requiredEph = 10;
 	_params.requiredEpv = 10;
+	_params.dax_noise = 0.0f;
+	_params.day_noise = 0.0f;
+	_params.daz_noise = 0.0f;
+	_params.dvx_noise = 0.0f;
+	_params.dvy_noise = 0.0f;
+	_params.dvz_noise = 0.0f;
+	_params.delta_ang_sig = 0.0f;
+	_params.delta_vel_sig = 0.0f;
+	_params.delta_pos_sig = 0.0f;
+	_params.delta_gyro_bias_sig = 0.0f;
+	_params.delta_gyro_scale_sig = 0.0f;
+	_params.delta_vel_bias_z_sig = 0.0f;
+	_params.delta_mag_body_sig = 0.0f;
+	_params.delta_mag_earth_sig = 0.0f;
+	_params.delta_wind_sig = 0.0f;
 
 	_dt_imu_avg = 0.0f;
 	_imu_time_last = time_usec;
@@ -400,4 +415,4 @@ void EstimatorBase::printStoredGps()
 	for (int i = 0; i < OBS_BUFFER_LENGTH; i++) {
 		printGps(&_gps_buffer[i]);
 	}
-}
+}

EKF/estimator_base.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
+ *   Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,7 +35,7 @@
  * @file estimator_base.h
  * Definition of base class for attitude estimators
  *
- * @author Roman Bast <bastroman@gmail.com>
+ * @author Roman Bast <bapstroman@gmail.com>
  *
  */
 
@@ -64,6 +64,8 @@ public:
 	EstimatorBase();
 	~EstimatorBase();
 
+	virtual void update() = 0;
+
 	// set delta angle imu data
 	void setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float *delta_ang, float *delta_vel);
 
@@ -159,8 +161,25 @@ protected:
 		uint32_t airspeed_delay_ms;
 		float 	requiredEph;
 		float 	requiredEpv;
-	} _params;
 
+		float dax_noise;
+		float day_noise;
+		float daz_noise;
+		float dvx_noise;
+		float dvy_noise;
+		float dvz_noise;
+
+		float delta_ang_sig;
+		float delta_vel_sig;
+		float delta_pos_sig;
+		float delta_gyro_bias_sig;
+		float delta_gyro_scale_sig;
+		float delta_vel_bias_z_sig;
+		float delta_mag_body_sig;
+		float delta_mag_earth_sig;
+		float delta_wind_sig;
+
+	} _params;
 
 	static const uint8_t OBS_BUFFER_LENGTH = 5;
 	static const uint8_t IMU_BUFFER_LENGTH = 25;
@@ -228,4 +247,3 @@ public:
 	void printGps(struct gpsSample *data);
 	void printStoredGps();
 };
-