From 5e5d6f432a1cf8ac3be16a057683bb6ae16e1e0a Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 22 Dec 2015 11:22:17 +0100 Subject: [PATCH 1/9] added static gps mode and init mag state correctly --- EKF/ekf.cpp | 15 +++++++++++---- EKF/ekf.h | 2 ++ EKF/estimator_base.cpp | 2 +- 3 files changed, 14 insertions(+), 5 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index db605e6494..ffe380dc68 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -48,7 +48,8 @@ Ekf::Ekf(): _fuse_height(false), _fuse_pos(false), _fuse_vel(false), - _mag_fuse_index(0) + _mag_fuse_index(0), + _time_last_fake_gps(0) { _earth_rate_NED.setZero(); _R_prev = matrix::Dcm(); @@ -96,6 +97,9 @@ bool Ekf::update() if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed)) { _fuse_pos = true; _fuse_vel = true; + } else if (_time_last_imu - _time_last_gps > 2000000 && _time_last_imu - _time_last_fake_gps > 70000) { + _fuse_vel = true; + _gps_sample_delayed.vel.setZero(); } @@ -142,14 +146,17 @@ bool Ekf::initialiseFilter(void) roll = -asinf(accel_init(1) / cosf(pitch)); } - matrix::Euler euler_init(roll, pitch, 0.0f); - _state.quat_nominal = Quaternion(euler_init); - magSample mag_init = _mag_buffer.get_newest(); if (mag_init.time_us == 0) { return false; } + float yaw_init = atan2f(mag_init.mag(1), mag_init.mag(0)); + + matrix::Euler euler_init(roll, pitch, yaw_init); + _state.quat_nominal = Quaternion(euler_init); + + matrix::Dcm R_to_earth(euler_init); _state.mag_I = R_to_earth * mag_init.mag; diff --git a/EKF/ekf.h b/EKF/ekf.h index 561010de57..519b83dc9d 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -66,6 +66,8 @@ private: uint8_t _mag_fuse_index; // counter for sequential mag axis fusion + uint64_t _time_last_fake_gps; + Vector3f _earth_rate_NED; matrix::Dcm _R_prev; diff --git a/EKF/estimator_base.cpp b/EKF/estimator_base.cpp index 5f9665831b..3624d2d6b3 100644 --- a/EKF/estimator_base.cpp +++ b/EKF/estimator_base.cpp @@ -69,7 +69,7 @@ void EstimatorBase::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64 _time_last_imu = time_usec; - if (_imu_time_last > 0) { + if (_time_last_imu > 0) { _dt_imu_avg = 0.8f * _dt_imu_avg + 0.2f * dt; } From faf48242ede4c1ca8bfc3fa137947dd344a3cb57 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Dec 2015 11:26:50 +0100 Subject: [PATCH 2/9] EKF: Fix ringbuffer warnings --- EKF/RingBuffer.h | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/EKF/RingBuffer.h b/EKF/RingBuffer.h index 04a7908846..c444075f1b 100644 --- a/EKF/RingBuffer.h +++ b/EKF/RingBuffer.h @@ -76,7 +76,7 @@ public: inline void push(data_type sample, bool debug = false) { if (debug) { - printf("elapsed %i\n", sample.time_us - _time_last); + printf("elapsed %llu\n", sample.time_us - _time_last); _time_last = sample.time_us; } @@ -120,11 +120,12 @@ public: if (timestamp >= _buffer[index].time_us && timestamp - _buffer[index].time_us < 100000) { - memcpy(sample, &_buffer[index], sizeof(*sample)); + // TODO Re-evaluate the static cast and usage patterns + memcpy(static_cast(sample), static_cast(&_buffer[index]), sizeof(*sample)); // Now we can set the tail to the item which comes after the one we removed // since we don't want to have any older data in the buffer - if (index == _head) { + if (index == static_cast(_head)) { _tail = _head; _first_write = true; @@ -137,7 +138,7 @@ public: return true; } - if (index == _tail) { + if (index == static_cast(_tail)) { // we have reached the tail and haven't got a match return false; } From 889478675249a198ea5add23dbb13c977739ae75 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Dec 2015 11:27:11 +0100 Subject: [PATCH 3/9] Estimator base: Fix printf formatting --- EKF/estimator_base.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/EKF/estimator_base.cpp b/EKF/estimator_base.cpp index 3624d2d6b3..20b293bdea 100644 --- a/EKF/estimator_base.cpp +++ b/EKF/estimator_base.cpp @@ -360,7 +360,7 @@ void EstimatorBase::printStoredIMU() void EstimatorBase::printIMU(struct imuSample *data) { - printf("time %i\n", data->time_us); + printf("time %llu\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)); @@ -388,14 +388,14 @@ void EstimatorBase::printStoredMag() void EstimatorBase::printMag(struct magSample *data) { - printf("time %i\n", data->time_us); + printf("time %llu\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("time %llu\n", data->time_us); printf("baro: %.5f\n\n", (double)data->hgt); } @@ -410,7 +410,7 @@ void EstimatorBase::printStoredBaro() void EstimatorBase::printGps(struct gpsSample *data) { - printf("time %i\n", data->time_us); + printf("time %llu\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)); } From 6b49e2495cd6bad27dafe749d141a2f34f6f17b3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Dec 2015 18:56:15 +0100 Subject: [PATCH 4/9] Fix field initializers --- EKF/estimator_base.h | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/EKF/estimator_base.h b/EKF/estimator_base.h index 4b3a88979e..98a9e8a1fa 100644 --- a/EKF/estimator_base.h +++ b/EKF/estimator_base.h @@ -212,15 +212,15 @@ protected: uint64_t _imu_ticks; - bool _imu_updated; - bool _start_predict_enabled; - bool _initialised; - bool _gps_initialised; - bool _gps_speed_valid; + bool _imu_updated = false; + bool _start_predict_enabled = false; + bool _initialised = false; + bool _gps_initialised = false; + bool _gps_speed_valid = false; - bool _mag_healthy; // computed by mag innovation test + bool _mag_healthy = false; // computed by mag innovation test - bool _in_air; // indicates if the vehicle is in the air + bool _in_air = true; // indicates if the vehicle is in the air RingBuffer _imu_buffer; RingBuffer _gps_buffer; From 457a57c6ff224a3bfd5fc304e75995db02e1952a Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 22 Dec 2015 18:00:59 +0100 Subject: [PATCH 5/9] use simple heading fusion --- EKF/ekf.cpp | 6 +++--- EKF/estimator_base.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index ffe380dc68..c9d8888879 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -85,9 +85,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)) { diff --git a/EKF/estimator_base.cpp b/EKF/estimator_base.cpp index 20b293bdea..ed88c1f8a8 100644 --- a/EKF/estimator_base.cpp +++ b/EKF/estimator_base.cpp @@ -311,7 +311,7 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec) _gps_speed_valid = false; _mag_healthy = false; - _in_air = true; // XXX get this flag from the application + _in_air = false; // XXX get this flag from the application _time_last_imu = 0; _time_last_gps = 0; From 101bcdc4e1768dd019e8283973989db91cd141a1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Dec 2015 11:29:57 +0100 Subject: [PATCH 6/9] Ignore system and temp files --- .gitignore | 2 ++ 1 file changed, 2 insertions(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000000..99a241d6b8 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +*.DS_Store +*~ From ec0189bb3ca19ee82cebf08caf702eb6580a258b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 24 Dec 2015 10:25:31 +0100 Subject: [PATCH 7/9] Update README --- README.md | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 19a5462b46..45d31df98b 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,7 @@ -# libgnc -Guidance, navigation and and control library (WIP) +# ECL + +Very lightweight Estimation & Control Library. + +This library solves the estimation & control problems of a number of robots and drones. It accepts GPS, vision and inertial sensor inputs. It is extremely lightweight and efficient and yet has the rugged field-proven performance. + +The library is currently BSD licensed, but might move to Apache 2.0. From eda69e727fab22c81c421b635cb34303b90b7968 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 24 Dec 2015 13:48:22 +0100 Subject: [PATCH 8/9] Do not print states by default --- EKF/ekf.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index c9d8888879..5e3c9f258e 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -73,7 +73,7 @@ bool Ekf::update() return false; } } - printStates(); + //printStates(); //printStatesFast(); // prediction if (_imu_updated) { From 0cb210d045d005d0775bc7b4c1631addb128c8e3 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 1 Jan 2016 11:56:38 +1100 Subject: [PATCH 9/9] EKF: Changes required to enter POSCTL mode --- EKF/estimator_base.cpp | 30 +++++++++++++++++++++++++----- EKF/estimator_base.h | 11 ++++++++--- 2 files changed, 33 insertions(+), 8 deletions(-) diff --git a/EKF/estimator_base.cpp b/EKF/estimator_base.cpp index ed88c1f8a8..fe2dd95ee4 100644 --- a/EKF/estimator_base.cpp +++ b/EKF/estimator_base.cpp @@ -170,6 +170,8 @@ 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; + _last_valid_gps_time_us = hrt_absolute_time(); + gps_sample_new.time_us = math::max(gps_sample_new.time_us, _imu_sample_delayed.time_us); @@ -261,8 +263,8 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec) _params.baro_delay_ms = 0; _params.gps_delay_ms = 200; _params.airspeed_delay_ms = 0; - _params.requiredEph = 200; - _params.requiredEpv = 200; + _params.requiredEph = 500; + _params.requiredEpv = 800; _params.gyro_noise = 1e-3f; _params.accel_noise = 1e-1f; _params.gyro_bias_p_noise = 1e-5f; @@ -335,20 +337,38 @@ void EstimatorBase::initialiseGPS(struct gps_message *gps) map_projection_init(&_posRef, lat, lon); _gps_alt_ref = gps->alt / 1e3f; _gps_initialised = true; + _last_gps_origin_time_us = hrt_absolute_time(); } } 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; + // Use a stricter check for initialisation than during flight to avoid complete loss of GPS + if (_gps_initialised) { + if ((gps->fix_type >= 3) && (gps->eph < _params.requiredEph * 2) && (gps->epv < _params.requiredEpv * 2)) { + return true; + } else { + return false; + } } else { - return false; + if ((gps->fix_type >= 3) && (gps->eph < _params.requiredEph) && (gps->epv < _params.requiredEpv)) { + return true; + + } else { + return false; + } } } +bool EstimatorBase::position_is_valid() +{ + // return true if the position estimate is valid + // TOTO implement proper check based on published GPS accuracy, innovaton consistency checks and timeout status + return _gps_initialised && (hrt_absolute_time() - _last_valid_gps_time_us) < 5e6; +} + void EstimatorBase::printStoredIMU() { printf("---------Printing IMU data buffer------------\n"); diff --git a/EKF/estimator_base.h b/EKF/estimator_base.h index 98a9e8a1fa..ee606c1143 100644 --- a/EKF/estimator_base.h +++ b/EKF/estimator_base.h @@ -190,6 +190,8 @@ protected: float _dt_imu_avg; uint64_t _imu_time_last; + uint64_t _last_valid_gps_time_us; + imuSample _imu_sample_delayed; imuSample _imu_down_sampled; Quaternion @@ -206,9 +208,6 @@ protected: outputSample _output_new; imuSample _imu_sample_new; - struct map_projection_reference_s _posRef; - float _gps_alt_ref; - uint64_t _imu_ticks; @@ -266,6 +265,8 @@ public: void printGps(struct gpsSample *data); void printStoredGps(); + bool position_is_valid(); + void copy_quaternion(float *quat) { for (unsigned i = 0; i < 4; i++) { quat[i] = _output_new.quat_nominal(i); @@ -284,4 +285,8 @@ public: void copy_timestamp(uint64_t *time_us) { *time_us = _imu_time_last; } + + uint64_t _last_gps_origin_time_us; + struct map_projection_reference_s _posRef; + float _gps_alt_ref; };