From 3c98c7119ebe956d1dc9c10379e9158567d16642 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Mon, 22 Dec 2014 13:47:20 -0700 Subject: [PATCH 1/4] use non-uniform 1st order IIR lowpass filters for baro_gps_offset estimation --- src/drivers/ms5611/ms5611.cpp | 2 +- .../mathlib/math/filter/LowPassFilter2p.cpp | 5 +- .../ekf_att_pos_estimator_main.cpp | 59 ++++++++++++++++--- 3 files changed, 57 insertions(+), 9 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 0a793cbc97..6eda6933d9 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -90,7 +90,7 @@ static const int ERROR = -1; /* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */ #define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ -#define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ +#define MS5611_MEASUREMENT_RATIO 100 /* pressure measurements per temperature measurement */ #define MS5611_BARO_DEVICE_PATH_EXT "/dev/ms5611_ext" #define MS5611_BARO_DEVICE_PATH_INT "/dev/ms5611_int" diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp index 6f640c9f9a..c709a28348 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp @@ -66,6 +66,7 @@ float LowPassFilter2p::apply(float sample) // no filtering return sample; } + // do the filtering float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2; if (isnan(delay_element_0) || isinf(delay_element_0)) { @@ -82,7 +83,9 @@ float LowPassFilter2p::apply(float sample) } float LowPassFilter2p::reset(float sample) { - _delay_element_1 = _delay_element_2 = sample; + float dval = sample / (_b0 + _b1 + _b2); + _delay_element_1 = dval; + _delay_element_2 = dval; return apply(sample); } diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index c41777968e..1770a75e53 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -82,12 +82,11 @@ #include #include #include +#include #include #include "estimator_23states.h" - - /** * estimator app start / stop handling function * @@ -101,7 +100,7 @@ __EXPORT uint64_t getMicros(); static uint64_t IMUmsec = 0; static uint64_t IMUusec = 0; -static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; +static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; // units: microseconds uint32_t millis() { @@ -222,8 +221,10 @@ private: float _baro_ref; /**< barometer reference altitude */ float _baro_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */ float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */ + hrt_abstime _last_debug_print = 0; perf_counter_t _loop_perf; ///< loop performance counter + perf_counter_t _loop_intvl; ///< loop rate counter perf_counter_t _perf_gyro; ///gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; _ekf->gpsHgt = _gps.alt / 1e3f; + // update LPF + _gps_alt_filt += (1 - expf(-gps_elapsed * rc)) * (_gps_last - _gps_alt_filt); + _gps_last = _ekf->gpsHgt; + + //warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)gps_elapsed); + // if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) { // _ekf->vneSigma = sqrtf(_gps.s_variance_m_s); // } else { @@ -1070,7 +1085,6 @@ FixedwingEstimator::task_main() // warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); newDataGps = true; - last_gps = _gps.timestamp_position; } @@ -1082,15 +1096,16 @@ FixedwingEstimator::task_main() if (baro_updated) { - hrt_abstime baro_last = _baro.timestamp; - orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro); float baro_elapsed = (_baro.timestamp - baro_last) / 1e6f; + baro_last = _baro.timestamp; _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f)); _ekf->baroHgt = _baro.altitude; + _baro_alt_filt += (1 - expf(-baro_elapsed * rc)) * (_baro_last - _baro_alt_filt); + _baro_last = _baro.altitude; if (!_baro_init) { _baro_ref = _baro.altitude; @@ -1180,6 +1195,24 @@ FixedwingEstimator::task_main() float initVelNED[3]; + // maintain filtered baro and gps altitudes to calculate weather offset + // baro sample rate is ~70Hz and measurement bandwidth is high + // gps sample rate is 5Hz and altitude is assumed accurate when averaged over 30 seconds + // maintain heavily filtered values for both baro and gps altitude + // Assume the filtered output should be identical for both sensors + _baro_gps_offset = _baro_alt_filt - _gps_alt_filt; + if (hrt_elapsed_time(&_last_debug_print) >= 5e6) { + _last_debug_print = hrt_absolute_time(); + perf_print_counter(_perf_baro); + perf_reset(_perf_baro); + warnx("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f", + (double)_baro_gps_offset, + (double)_baro_alt_filt, + (double)_gps_alt_filt, + (double)_global_pos.alt, + (double)_local_pos.z); + } + /* Initialize the filter first */ if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) { @@ -1195,7 +1228,13 @@ FixedwingEstimator::task_main() // Set up height correctly orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro); _baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame - _baro_gps_offset = _baro.altitude - gps_alt; + + // init filtered gps and baro altitudes + _gps_alt_filt = gps_alt; + _gps_last = gps_alt; + _baro_alt_filt = _baro.altitude; + _baro_last = _baro.altitude; + _ekf->baroHgt = _baro.altitude; _ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref)); @@ -1502,6 +1541,12 @@ FixedwingEstimator::task_main() _global_pos.timestamp = _local_pos.timestamp; + // FIXME: usurp terrain alt field for baro_gps_offset + _global_pos.terrain_alt = _baro_gps_offset; + _global_pos.terrain_alt_valid = true; + _global_pos.eph = _baro_alt_filt; + _global_pos.epv = _gps_alt_filt; + /* lazily publish the global position only once available */ if (_global_pos_pub > 0) { /* publish the global position */ From ea57dec24b14d94ec82f04b67bc31775b1f4a9cf Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Mon, 12 Jan 2015 17:15:38 -0700 Subject: [PATCH 2/4] revert to original ms5611 temperature sample rate --- src/drivers/ms5611/ms5611.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 6eda6933d9..0a793cbc97 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -90,7 +90,7 @@ static const int ERROR = -1; /* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */ #define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ -#define MS5611_MEASUREMENT_RATIO 100 /* pressure measurements per temperature measurement */ +#define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ #define MS5611_BARO_DEVICE_PATH_EXT "/dev/ms5611_ext" #define MS5611_BARO_DEVICE_PATH_INT "/dev/ms5611_int" From dba0a5a90fd9fb87ba5a0083b0a009e8ac74b0ea Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Mon, 12 Jan 2015 17:23:30 -0700 Subject: [PATCH 3/4] comment out debug warnx --- .../ekf_att_pos_estimator_main.cpp | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 1770a75e53..8acf09977e 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1201,17 +1201,17 @@ FixedwingEstimator::task_main() // maintain heavily filtered values for both baro and gps altitude // Assume the filtered output should be identical for both sensors _baro_gps_offset = _baro_alt_filt - _gps_alt_filt; - if (hrt_elapsed_time(&_last_debug_print) >= 5e6) { - _last_debug_print = hrt_absolute_time(); - perf_print_counter(_perf_baro); - perf_reset(_perf_baro); - warnx("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f", - (double)_baro_gps_offset, - (double)_baro_alt_filt, - (double)_gps_alt_filt, - (double)_global_pos.alt, - (double)_local_pos.z); - } +// if (hrt_elapsed_time(&_last_debug_print) >= 5e6) { +// _last_debug_print = hrt_absolute_time(); +// perf_print_counter(_perf_baro); +// perf_reset(_perf_baro); +// warnx("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f", +// (double)_baro_gps_offset, +// (double)_baro_alt_filt, +// (double)_gps_alt_filt, +// (double)_global_pos.alt, +// (double)_local_pos.z); +// } /* Initialize the filter first */ if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) { From b795705640c6a14ba359e381c935351f9377aea9 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Tue, 13 Jan 2015 08:53:11 -0700 Subject: [PATCH 4/4] improve efficiency of non-uniform rate LPFs --- .../ekf_att_pos_estimator_main.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 8acf09977e..4ee2b70e65 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -786,8 +786,7 @@ FixedwingEstimator::task_main() // init lowpass filters for baro and gps altitude float _gps_alt_filt = 0, _baro_alt_filt = 0; - float _gps_last = 0, _baro_last = 0; - float rc = (1.0f/10); // actually 1/RC time constant of 1st order LPF + float rc = 10.0f; // RC time constant of 1st order LPF in seconds hrt_abstime baro_last = 0; _task_running = true; @@ -1065,8 +1064,7 @@ FixedwingEstimator::task_main() _ekf->gpsHgt = _gps.alt / 1e3f; // update LPF - _gps_alt_filt += (1 - expf(-gps_elapsed * rc)) * (_gps_last - _gps_alt_filt); - _gps_last = _ekf->gpsHgt; + _gps_alt_filt += (gps_elapsed / (rc + gps_elapsed)) * (_ekf->gpsHgt - _gps_alt_filt); //warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)gps_elapsed); @@ -1104,8 +1102,7 @@ FixedwingEstimator::task_main() _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f)); _ekf->baroHgt = _baro.altitude; - _baro_alt_filt += (1 - expf(-baro_elapsed * rc)) * (_baro_last - _baro_alt_filt); - _baro_last = _baro.altitude; + _baro_alt_filt += (baro_elapsed/(rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt); if (!_baro_init) { _baro_ref = _baro.altitude; @@ -1231,9 +1228,7 @@ FixedwingEstimator::task_main() // init filtered gps and baro altitudes _gps_alt_filt = gps_alt; - _gps_last = gps_alt; _baro_alt_filt = _baro.altitude; - _baro_last = _baro.altitude; _ekf->baroHgt = _baro.altitude; _ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref));