added base class for data storage

CMakeLists.txt

@@ -42,5 +42,7 @@ px4_add_module(
 		l1/ecl_l1_pos_controller.cpp
 		validation/data_validator.cpp
 		validation/data_validator_group.cpp
+		EKF/estimator_base.cpp
+		EKF/ekf.cpp
 	)
 # vim: set noet ft=cmake fenc=utf-8 ff=unix : 

EKF/RingBuffer.h

@@ -0,0 +1,110 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file RingBuffer.h
+ * @author Roman Bapst <bapstroman@gmail.com>
+ * Template RingBuffer.
+ */
+
+template <typename data_type>
+class RingBuffer
+{
+public:
+	RingBuffer() {_buffer = NULL;};
+	~RingBuffer() {delete _buffer;}
+
+	bool allocate(unsigned size)
+	{
+		if (size == 0) {
+			return false;
+		}
+
+		_buffer = new data_type[size];
+
+		if (_buffer == NULL) {
+			return false;
+		}
+
+		_size = size;
+		return true;
+	}
+
+	inline void push(data_type sample)
+	{
+		_buffer[_head] = sample;
+		_head = (_head + 1) % _size;
+
+		// move tail if we overwrite it
+		if (_head == _tail && _size > 1) {
+			_tail = (_tail + 1) % _size;
+		}
+	}
+
+	inline data_type get_oldest()
+	{
+		return _buffer[_tail];
+	}
+
+	inline bool pop_first_older_than(uint64_t timestamp, data_type &sample)
+	{
+		// start looking from oldest data of buffer
+		for (unsigned i = 0; i < _size; i++) {
+			unsigned index = (_tail + i) % _size;
+
+			if (timestamp >= _buffer[index].time_us) {
+				sample = _buffer[_tail + i];
+				// now we can set the tail to the item
+				// which comes after the one we removed
+				_tail = (_tail + i + 1);
+				return true;
+			}
+
+			if (index == _head) {
+				// we have reached the head and haven't found anything
+				return false;
+			}
+		}
+
+		return false;
+	}
+
+	data_type &operator[](unsigned index)
+	{
+		return _buffer[index];
+	}
+
+private:
+	data_type *_buffer;
+	unsigned _head, _tail, _size;
+};

EKF/ekf.cpp

@@ -0,0 +1,98 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name ECL nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ekf.cpp
+ * Definition of ekf attitude position estimator class.
+ *
+ * @author Roman Bast <bastroman@gmail.com>
+ *
+ */
+
+#include "ekf.h"
+
+Ekf::Ekf()
+{
+
+}
+
+
+Ekf::~Ekf()
+{
+
+}
+
+void Ekf::update()
+{
+	if (!_filter_initialised) {
+		_filter_initialised = initialiseFilter();
+	}
+
+	if (!_filter_initialised) {
+		return;
+	}
+
+	// prediction
+	if (_imu_updated) {
+		predictState();
+		predictCovariance();
+		_imu_updated = false;
+	}
+
+	// measurement updates
+	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)) {
+		_fuse_height = true;
+	}
+
+	if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _gps_sample_delayed)) {
+		_fuse_pos = true;
+		_fuse_vel = true;
+	}
+
+	if (_fuse_height || _fuse_pos || _fuse_vel) {
+		fusePosVel();
+	}
+
+	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)) {
+		fuseAirspeed();
+	}
+
+}

EKF/ekf.h

@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name ECL nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ekf.h
+ * Definition of ekf attitude position estimator class.
+ *
+ * @author Roman Bast <bastroman@gmail.com>
+ *
+ */
+
+#include "estimator_base.h"
+
+class Ekf : public EstimatorBase
+{
+public:
+
+	Ekf();
+	~Ekf();
+
+	void update();
+
+private:
+
+	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
+
+	bool initialiseFilter(void);
+
+	void predictState();
+
+	void predictCovariance();
+
+	void fusePosVel();
+
+	void fuseMag();
+
+	void fuseAirspeed();
+
+	void fuseRange();
+
+
+};

EKF/estimator_base.cpp

@@ -0,0 +1,403 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name ECL nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file estimator_base.cpp
+ * Definition of base class for attitude estimators
+ *
+ * @author Roman Bast <bastroman@gmail.com>
+ *
+ */
+
+#include <math.h>
+#include "estimator_base.h"
+#include <mathlib/mathlib.h>
+
+
+EstimatorBase::EstimatorBase()
+{
+}
+
+EstimatorBase::~EstimatorBase()
+{
+
+}
+
+// read integrated imu data and store to buffer
+void EstimatorBase::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float *delta_ang,
+			       float *delta_vel)
+{
+	if (!_initialised) {
+		initialiseVariables(time_usec);
+		_initialised = true;
+		_start_predict_enabled = true;
+	}
+
+	float dt = (float)(time_usec - _time_last_imu) / 1000 / 1000;
+	dt = math::max(dt, 1.0e-4f);
+	dt = math::min(dt, 0.02f);
+
+	_time_last_imu = time_usec;
+
+	if (_imu_time_last > 0) {
+		_dt_imu_avg = 0.8f * _dt_imu_avg + 0.2f * dt;
+	}
+
+	// copy data
+	imuSample imu_sample_new = {};
+	memcpy(&imu_sample_new.delta_ang._data[0], delta_ang, sizeof(imu_sample_new.delta_ang._data));
+	memcpy(&imu_sample_new.delta_vel._data[0], delta_vel, sizeof(imu_sample_new.delta_vel._data));
+
+	imu_sample_new.delta_ang_dt = delta_ang_dt / 1e6f;
+	imu_sample_new.delta_vel_dt = delta_vel_dt / 1e6f;
+
+	imu_sample_new.time_us = time_usec;
+
+	imu_sample_new.delta_ang(0) = imu_sample_new.delta_ang(0) * _state.gyro_scale(0);
+	imu_sample_new.delta_ang(1) = imu_sample_new.delta_ang(1) * _state.gyro_scale(1);
+	imu_sample_new.delta_ang(2) = imu_sample_new.delta_ang(2) * _state.gyro_scale(2);
+
+	imu_sample_new.delta_ang -= _state.gyro_bias;
+	imu_sample_new.delta_vel(2) -= _state.accel_z_bias;
+
+	_imu_down_sampled.delta_ang_dt += imu_sample_new.delta_ang_dt;
+	_imu_down_sampled.delta_vel_dt += imu_sample_new.delta_vel_dt;
+
+
+	Quaternion delta_q;
+	delta_q.rotate(imu_sample_new.delta_ang);
+	_q_down_sampled =  _q_down_sampled * delta_q;
+	_q_down_sampled.normalize();
+
+	matrix::Dcm<float> delta_R(delta_q.inversed());
+	_imu_down_sampled.delta_vel = delta_R * _imu_down_sampled.delta_vel;
+	_imu_down_sampled.delta_vel += imu_sample_new.delta_vel;
+
+	_imu_ticks++;
+
+	if ((_dt_imu_avg * _imu_ticks >= (float)(FILTER_UPDATE_PERRIOD_MS) / 1000 && _start_predict_enabled)
+	    || (_dt_imu_avg * _imu_ticks >= 0.02f)) {
+		_imu_down_sampled.delta_ang = _q_down_sampled.to_axis_angle();
+		_imu_down_sampled.time_us = time_usec;
+
+		_imu_buffer.push(_imu_down_sampled);
+
+		_imu_down_sampled.delta_ang.setZero();
+		_imu_down_sampled.delta_vel.setZero();
+		_imu_down_sampled.delta_ang_dt = 0.0f;
+		_imu_down_sampled.delta_vel_dt = 0.0f;
+		_q_down_sampled(0) = 1.0f;
+		_q_down_sampled(1) = _q_down_sampled(2) = _q_down_sampled(3) = 0.0f;
+
+		_imu_ticks = 0;
+
+		_imu_updated = true;
+
+	} else {
+
+		_imu_updated = false;
+	}
+
+
+	_imu_sample_delayed = _imu_buffer.get_oldest();
+}
+
+
+void EstimatorBase::setMagData(uint64_t time_usec, float *data)
+{
+
+
+	if (time_usec - _time_last_mag > 70000) {
+
+		magSample mag_sample_new = {};
+		mag_sample_new.time_us = time_usec  - _params.mag_delay_ms * 1000;
+
+
+		mag_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2;
+		_time_last_mag = mag_sample_new.time_us;
+
+
+		memcpy(&mag_sample_new.mag._data[0], data, sizeof(mag_sample_new.mag._data));
+
+		_mag_buffer.push(mag_sample_new);
+	}
+}
+
+void EstimatorBase::setGpsData(uint64_t time_usec, struct gps_message *gps)
+{
+
+	if (!_gps_initialised) {
+		initialiseGPS(gps);
+		return;
+	}
+
+
+	if (time_usec - _time_last_gps > 70000 && gps_is_good(gps)) {
+
+		gpsSample gps_sample_new = {};
+		gps_sample_new.time_us = time_usec - _params.gps_delay_ms * 1000;
+
+
+		gps_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2;
+		_time_last_gps = gps_sample_new.time_us;
+
+		gps_sample_new.time_us = math::max(gps_sample_new.time_us, _imu_sample_delayed.time_us);
+
+
+		memcpy(gps_sample_new.vel._data[0], gps->vel_ned, sizeof(gps_sample_new.vel._data));
+
+		_gps_speed_valid = gps->vel_ned_valid;
+
+
+		float lpos_x = 0.0f;
+		float lpos_y = 0.0f;
+		map_projection_project(&_posRef, (gps->lat / 1.0e7), (gps->lon / 1.0e7), &lpos_x, &lpos_y);
+		gps_sample_new.pos(0) = lpos_x;
+		gps_sample_new.pos(1) = lpos_y;
+		gps_sample_new.hgt = gps->alt / 1e3f;
+
+		_gps_buffer.push(gps_sample_new);
+	}
+}
+
+void EstimatorBase::setBaroData(uint64_t time_usec, float *data)
+{
+	if (time_usec - _time_last_baro > 70000) {
+
+		baroSample baro_sample_new;
+		baro_sample_new.hgt = *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;
+
+		baro_sample_new.time_us = math::max(baro_sample_new.time_us, _imu_sample_delayed.time_us);
+
+		_baro_buffer.push(baro_sample_new);
+	}
+}
+
+void EstimatorBase::setAirspeedData(uint64_t time_usec, float *data)
+{
+	if (time_usec > _time_last_airspeed) {
+		airspeedSample airspeed_sample_new;
+		airspeed_sample_new.airspeed = *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;
+
+		_airspeed_buffer.push(airspeed_sample_new);
+	}
+}
+
+// set range data
+void EstimatorBase::setRangeData(uint64_t time_usec, float *data)
+{
+
+}
+
+// set optical flow data
+void EstimatorBase::setOpticalFlowData(uint64_t time_usec, float *data)
+{
+
+}
+
+void EstimatorBase::initialiseVariables(uint64_t time_usec)
+{
+	_imu_buffer.allocate(IMU_BUFFER_LENGTH);
+	_gps_buffer.allocate(OBS_BUFFER_LENGTH);
+	_mag_buffer.allocate(OBS_BUFFER_LENGTH);
+	_baro_buffer.allocate(OBS_BUFFER_LENGTH);
+	_range_buffer.allocate(OBS_BUFFER_LENGTH);
+	_airspeed_buffer.allocate(OBS_BUFFER_LENGTH);
+	_flow_buffer.allocate(OBS_BUFFER_LENGTH);
+	_output_buffer.allocate(IMU_BUFFER_LENGTH);
+
+	_state.ang_error.setZero();
+	_state.vel.setZero();
+	_state.pos.setZero();
+	_state.gyro_bias.setZero();
+	_state.gyro_scale(0) = 1.0f;
+	_state.gyro_scale(1) = 1.0f;
+	_state.gyro_scale(2) = 1.0f;
+	_state.accel_z_bias = 0.0f;
+	_state.mag_I.setZero();
+	_state.mag_B.setZero();
+	_state.wind_vel.setZero();
+	_state.quat_nominal.setZero();
+	_state.quat_nominal(0) = 1.0f;
+
+	_params.mag_delay_ms = 0;
+	_params.baro_delay_ms = 0;
+	_params.gps_delay_ms = 0;
+	_params.airspeed_delay_ms = 0;
+	_params.requiredEph = 10;
+	_params.requiredEpv = 10;
+
+	_dt_imu_avg = 0.0f;
+	_imu_time_last = time_usec;
+
+	_imu_sample_delayed.delta_ang.setZero();
+	_imu_sample_delayed.delta_vel.setZero();
+	_imu_sample_delayed.delta_ang_dt = 0.0f;
+	_imu_sample_delayed.delta_vel_dt = 0.0f;
+	_imu_sample_delayed.time_us = time_usec;
+
+	_imu_down_sampled.delta_ang.setZero();
+	_imu_down_sampled.delta_vel.setZero();
+	_imu_down_sampled.delta_ang_dt = 0.0f;
+	_imu_down_sampled.delta_vel_dt = 0.0f;
+	_imu_down_sampled.time_us = time_usec;
+
+	_q_down_sampled(0) = 1.0f;
+	_q_down_sampled(1) = 0.0f;
+	_q_down_sampled(2) = 0.0f;
+	_q_down_sampled(3) = 0.0f;
+
+	_imu_ticks = 0;
+
+	_imu_updated = false;
+	_start_predict_enabled = false;
+	_initialised = false;
+	_gps_initialised = false;
+	_gps_speed_valid = false;
+
+	_time_last_imu = 0;
+	_time_last_gps = 0;
+	_time_last_mag = 0;
+	_time_last_baro = 0;
+	_time_last_range = 0;
+	_time_last_airspeed = 0;
+
+}
+
+void EstimatorBase::initialiseGPS(struct gps_message *gps)
+{
+	//Check if the GPS fix is good enough for us to use
+	if (gps_is_good(gps)) {
+		printf("gps is good\n");
+		// Initialise projection
+		double lat = gps->lat / 1.0e7;
+		double lon = gps->lon / 1.0e7;
+		map_projection_init(&_posRef, lat, lon);
+		_gps_alt_ref = gps->alt / 1e3f;
+		_gps_initialised = true;
+	}
+}
+
+bool EstimatorBase::gps_is_good(struct gps_message *gps)
+{
+	// go through apm implementation of calcGpsGoodToAlign for fancier checks
+	if ((gps->fix_type >= 3) && (gps->eph < _params.requiredEph) && (gps->epv < _params.requiredEpv)) {
+		return true;
+
+	} else {
+		return false;
+	}
+}
+
+void EstimatorBase::printStoredIMU()
+{
+	printf("---------Printing IMU data buffer------------\n");
+
+	for (int i = 0; i < IMU_BUFFER_LENGTH; i++) {
+		printIMU(&_imu_buffer[i]);
+	}
+}
+
+void EstimatorBase::printIMU(struct imuSample *data)
+{
+	printf("time %i\n", data->time_us);
+	printf("delta_ang_dt %.5f\n", (double)data->delta_ang_dt);
+	printf("delta_vel_dt %.5f\n", (double)data->delta_vel_dt);
+	printf("dA: %.5f %.5f %.5f \n", (double)data->delta_ang(0), (double)data->delta_ang(1), (double)data->delta_ang(2));
+	printf("dV: %.5f %.5f %.5f \n\n", (double)data->delta_vel(0), (double)data->delta_vel(1), (double)data->delta_vel(2));
+}
+
+void EstimatorBase::printQuaternion(Quaternion &q)
+{
+	printf("q1 %.5f q2 %.5f q3 %.5f q4 %.5f\n", (double)q(0), (double)q(1), (double)q(2), (double)q(3));
+}
+
+void EstimatorBase::print_imu_avg_time()
+{
+	printf("dt_avg: %.5f\n", (double)_dt_imu_avg);
+}
+
+void EstimatorBase::printStoredMag()
+{
+	printf("---------Printing mag data buffer------------\n");
+
+	for (int i = 0; i < OBS_BUFFER_LENGTH; i++) {
+		printMag(&_mag_buffer[i]);
+	}
+}
+
+void EstimatorBase::printMag(struct magSample *data)
+{
+	printf("time %i\n", data->time_us);
+	printf("mag: %.5f %.5f %.5f \n\n", (double)data->mag(0), (double)data->mag(1), (double)data->mag(2));
+
+}
+
+void EstimatorBase::printBaro(struct baroSample *data)
+{
+	printf("time %i\n", data->time_us);
+	printf("baro: %.5f\n\n", (double)data->hgt);
+}
+
+void EstimatorBase::printStoredBaro()
+{
+	printf("---------Printing baro data buffer------------\n");
+
+	for (int i = 0; i < OBS_BUFFER_LENGTH; i++) {
+		printBaro(&_baro_buffer[i]);
+	}
+}
+
+void EstimatorBase::printGps(struct gpsSample *data)
+{
+	printf("time %i\n", data->time_us);
+	printf("gps pos: %.5f %.5f %.5f\n", (double)data->pos(0), (double)data->pos(1), (double)data->hgt);
+	printf("gps vel %.5f %.5f %.5f\n\n", (double)data->vel(0), (double)data->vel(1), (double)data->vel(2));
+}
+
+void EstimatorBase::printStoredGps()
+{
+	printf("---------Printing GPS data buffer------------\n");
+
+	for (int i = 0; i < OBS_BUFFER_LENGTH; i++) {
+		printGps(&_gps_buffer[i]);
+	}
+}

EKF/estimator_base.h

@@ -0,0 +1,231 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name ECL nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file estimator_base.h
+ * Definition of base class for attitude estimators
+ *
+ * @author Roman Bast <bastroman@gmail.com>
+ *
+ */
+
+#include <stdint.h>
+#include <matrix/matrix/math.hpp>
+#include <lib/geo/geo.h>
+#include "RingBuffer.h"
+
+struct gps_message {
+	uint64_t time_usec;
+	int32_t lat;			// Latitude in 1E-7 degrees
+	int32_t lon;			// Longitude in 1E-7 degrees
+	int32_t alt;			// Altitude in 1E-3 meters (millimeters) above MSL
+	uint8_t fix_type;		// 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time
+	float eph;			// GPS HDOP horizontal dilution of position in m
+	float epv;			// GPS VDOP horizontal dilution of position in m
+	uint64_t time_usec_vel;	// Timestamp for velocity informations
+	float vel_m_s;			// GPS ground speed (m/s)
+	float vel_ned[3];		// GPS ground speed NED
+	bool vel_ned_valid;		// GPS ground speed is valid
+};
+
+class EstimatorBase
+{
+public:
+	EstimatorBase();
+	~EstimatorBase();
+
+	// 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);
+
+	// set magnetometer data
+	void setMagData(uint64_t time_usec, float *data);
+
+	// set gps data
+	void setGpsData(uint64_t time_usec, struct gps_message *gps);
+
+	// set baro data
+	void setBaroData(uint64_t time_usec, float *data);
+
+	// set airspeed data
+	void setAirspeedData(uint64_t time_usec, float *data);
+
+	// set range data
+	void setRangeData(uint64_t time_usec, float *data);
+
+	// set optical flow data
+	void setOpticalFlowData(uint64_t time_usec, float *data);
+
+protected:
+
+	typedef matrix::Vector<float, 2> Vector2f;
+	typedef matrix::Vector<float, 3> Vector3f;
+	typedef matrix::Quaternion<float> Quaternion;
+	typedef matrix::Matrix<float, 3, 3> Matrix3f;
+
+	struct stateSample {
+		Vector3f    ang_error;
+		Vector3f    vel;
+		Vector3f    pos;
+		Vector3f    gyro_bias;
+		Vector3f    gyro_scale;
+		float       accel_z_bias;
+		Vector3f    mag_I;
+		Vector3f    mag_B;
+		Vector2f    wind_vel;
+		Quaternion  quat_nominal;
+	} _state;
+
+	struct outputSample {
+		Quaternion  quat_nominal;
+		Vector3f    vel;
+		Vector3f    pos;
+	};
+
+	struct imuSample {
+		Vector3f    delta_ang;
+		Vector3f    delta_vel;
+		float       delta_ang_dt;
+		float       delta_vel_dt;
+		uint32_t    time_us;
+	};
+
+	struct gpsSample {
+		Vector2f    pos;
+		float       hgt;
+		Vector3f    vel;
+		uint32_t    time_us;
+	};
+
+	struct magSample {
+		Vector3f    mag;
+		uint32_t    time_us;
+	};
+
+	struct baroSample {
+		float       hgt;
+		uint32_t    time_us;
+	};
+
+	struct rangeSample {
+		float       rng;
+		uint32_t    time_us;
+	};
+
+	struct airspeedSample {
+		float       airspeed;
+		uint32_t    time_us;
+	};
+
+	struct flowSample {
+		Vector2f    flowRadXY;
+		Vector2f    flowRadXYcomp;
+		uint32_t    time_us;
+	};
+
+	struct {
+		uint32_t mag_delay_ms;
+		uint32_t baro_delay_ms;
+		uint32_t gps_delay_ms;
+		uint32_t airspeed_delay_ms;
+		float 	requiredEph;
+		float 	requiredEpv;
+	} _params;
+
+
+	static const uint8_t OBS_BUFFER_LENGTH = 5;
+	static const uint8_t IMU_BUFFER_LENGTH = 25;
+	static const unsigned FILTER_UPDATE_PERRIOD_MS = 10;
+
+	float _dt_imu_avg;
+	uint32_t _imu_time_last;
+
+	imuSample _imu_sample_delayed;
+	imuSample _imu_down_sampled;
+	Quaternion
+	_q_down_sampled;
+
+	magSample _mag_sample_delayed;
+	baroSample _baro_sample_delayed;
+	gpsSample _gps_sample_delayed;
+	rangeSample _range_sample_delayed;
+	airspeedSample _airspeed_sample_delayed;
+	flowSample _flow_sample_delayed;
+
+	struct map_projection_reference_s _posRef;
+	float _gps_alt_ref;
+
+
+	uint32_t _imu_ticks;
+
+	bool _imu_updated;
+	bool _start_predict_enabled;
+	bool _initialised;
+	bool _gps_initialised;
+	bool _gps_speed_valid;
+
+	RingBuffer<imuSample> _imu_buffer;
+	RingBuffer<gpsSample> _gps_buffer;
+	RingBuffer<magSample> _mag_buffer;
+	RingBuffer<baroSample> _baro_buffer;
+	RingBuffer<rangeSample> _range_buffer;
+	RingBuffer<airspeedSample> _airspeed_buffer;
+	RingBuffer<flowSample> 	_flow_buffer;
+	RingBuffer<outputSample> _output_buffer;
+
+	uint64_t _time_last_imu;
+	uint64_t _time_last_gps;
+	uint64_t _time_last_mag;
+	uint64_t _time_last_baro;
+	uint64_t _time_last_range;
+	uint64_t _time_last_airspeed;
+
+
+	void initialiseVariables(uint64_t timestamp);
+
+	void initialiseGPS(struct gps_message *gps);
+
+	bool gps_is_good(struct gps_message *gps);
+
+public:
+	void printIMU(struct imuSample *data);
+	void printStoredIMU();
+	void printQuaternion(Quaternion &q);
+	void print_imu_avg_time();
+	void printMag(struct magSample *data);
+	void printStoredMag();
+	void printBaro(struct baroSample *data);
+	void printStoredBaro();
+	void printGps(struct gpsSample *data);
+	void printStoredGps();
+};
+

EKF/tests/base/CMakeLists.txt

@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2015 ECL Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name ECL nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE lib__ecl__EKF__tests__base
+	MAIN base
+	STACK 4096
+	COMPILE_FLAGS
+		-Os
+	SRCS
+		base.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

EKF/tests/base/base.cpp

@@ -0,0 +1,150 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file base.cpp
+ *
+ * Tests for the estimator base class
+ */
+
+#include <cstdio>
+#include <random>
+#include "../../estimator_base.h"
+
+extern "C" __EXPORT int base_main(int argc, char *argv[]);
+
+int base_main(int argc, char *argv[])
+{
+	EstimatorBase *base = new EstimatorBase();
+
+	// Test1: feed in fake imu data and check if delta angles are summed correclty
+	float delta_vel[3] = { 0.002f, 0.002f, 0.002f};
+	float delta_ang[3] = { -0.1f, 0.2f, 0.3f};
+	uint32_t time_usec = 1000;
+
+
+	// simulate 400 Hz imu rate, filter should downsample to 100Hz
+	// feed in 2 seconds of data
+	for (int i = 0; i < 800; i++) {
+		base->setIMUData(time_usec, 2500, 2500, delta_ang, delta_vel);
+		time_usec += 2500;
+	}
+
+	//base->printStoredIMU();
+
+
+	// Test2: feed fake imu data and check average imu delta t
+	// simulate 400 Hz imu rate, filter should downsample to 100Hz
+	// feed in 2 seconds of data
+	for (int i = 0; i < 800; i++) {
+		base->setIMUData(time_usec, 2500, 2500, delta_ang, delta_vel);
+		//base->print_imu_avg_time();
+		time_usec += 2500;
+	}
+
+	// Test3: feed in slow imu data, filter should now take every sample
+	for (int i = 0; i < 800; i++) {
+		base->setIMUData(time_usec, 2500, 2500, delta_ang, delta_vel);
+		time_usec += 30000;
+	}
+
+	//base->printStoredIMU();
+
+	// Test4: Feed in mag data at 50 Hz (too fast), check if filter samples correctly
+	float mag[3] = {0.2f, 0.0f, 0.4f};
+
+	for (int i = 0; i < 100; i++) {
+		base->setMagData(time_usec, mag);
+		time_usec += 20000;
+	}
+
+	//base->printStoredMag();
+
+	// Test5: Feed in baro data at 50 Hz (too fast), check if filter samples correctly
+	float baro = 120.22f;;
+	time_usec = 100000;
+
+	for (int i = 0; i < 100; i++) {
+		base->setBaroData(time_usec, &baro);
+		baro += 10.0f;
+		time_usec += 20000;
+	}
+
+	//base->printStoredBaro();
+
+	// Test 5: Run everything rogether in one run
+	std::default_random_engine generator;
+	std::uniform_int_distribution<int> distribution(-200, 200);
+
+	int imu_sample_period = 2500;
+	uint64_t timer = 2000;		// simulation start time
+	uint64_t timer_last = timer;
+
+	float airspeed = 0.0f;
+
+	struct gps_message gps = {};
+	gps.lat = 40 * 1e7;			// Latitude in 1E-7 degrees
+	gps.lon = 5 * 1e7;			// Longitude in 1E-7 degrees
+	gps.alt = 200 * 1e3;			// Altitude in 1E-3 meters (millimeters) above MSL
+	gps.fix_type = 4;		// 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time
+	gps.eph = 5.0f;			// GPS HDOP horizontal dilution of position in m
+	gps.epv = 5.0f;			// GPS VDOP horizontal dilution of position in m
+	gps.vel_ned_valid = true;	// GPS ground speed is valid
+
+	// simulate two seconds
+	for (int i = 0; i < 800; i++) {
+		timer += (imu_sample_period + distribution(generator));
+
+		if ((timer - timer_last) > 70000) {
+			base->setAirspeedData(timer, &airspeed);
+		}
+
+		gps.time_usec = timer;
+		gps.time_usec_vel = timer;
+		base->setIMUData(timer, timer - timer_last, timer - timer_last, delta_ang, delta_vel);
+		base->setMagData(timer, mag);
+		base->setBaroData(timer, &baro);
+		base->setGpsData(timer, &gps);
+		base->print_imu_avg_time();
+
+		timer_last = timer;
+
+	}
+
+	base->printStoredIMU();
+	base->printStoredBaro();
+	base->printStoredMag();
+	base->printStoredGps();
+
+	return 0;
+}