diff --git a/CMakeLists.txt b/CMakeLists.txt index fb17243f14..8cce5e3dc8 100644 --- a/CMakeLists.txt +++ b/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 : diff --git a/EKF/RingBuffer.h b/EKF/RingBuffer.h new file mode 100644 index 0000000000..6fd7f3415f --- /dev/null +++ b/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 + * Template RingBuffer. + */ + +template +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; +}; \ No newline at end of file diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp new file mode 100644 index 0000000000..82fc95f67b --- /dev/null +++ b/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 + * + */ + +#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(); + } + +} \ No newline at end of file diff --git a/EKF/ekf.h b/EKF/ekf.h new file mode 100644 index 0000000000..b3aaffd2e6 --- /dev/null +++ b/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 + * + */ + +#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(); + + +}; \ No newline at end of file diff --git a/EKF/estimator_base.cpp b/EKF/estimator_base.cpp new file mode 100644 index 0000000000..88f4883339 --- /dev/null +++ b/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 + * + */ + +#include +#include "estimator_base.h" +#include + + +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 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]); + } +} \ No newline at end of file diff --git a/EKF/estimator_base.h b/EKF/estimator_base.h new file mode 100644 index 0000000000..ea3c61aebd --- /dev/null +++ b/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 + * + */ + +#include +#include +#include +#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 Vector2f; + typedef matrix::Vector Vector3f; + typedef matrix::Quaternion Quaternion; + typedef matrix::Matrix 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 _imu_buffer; + RingBuffer _gps_buffer; + RingBuffer _mag_buffer; + RingBuffer _baro_buffer; + RingBuffer _range_buffer; + RingBuffer _airspeed_buffer; + RingBuffer _flow_buffer; + RingBuffer _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(); +}; + diff --git a/EKF/tests/base/CMakeLists.txt b/EKF/tests/base/CMakeLists.txt new file mode 100644 index 0000000000..777d60ed9b --- /dev/null +++ b/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 : diff --git a/EKF/tests/base/base.cpp b/EKF/tests/base/base.cpp new file mode 100644 index 0000000000..73314d0927 --- /dev/null +++ b/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 +#include +#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 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; +} \ No newline at end of file