diff options
author | Mark Whitehorn <kd0aij@gmail.com> | 2015-01-13 08:53:11 -0700 |
---|---|---|
committer | Mark Whitehorn <kd0aij@gmail.com> | 2015-01-13 09:10:44 -0700 |
commit | b795705640c6a14ba359e381c935351f9377aea9 (patch) | |
tree | 96df03c47b9ad1ba2780aaecf75cdd5974bab3d5 /src | |
parent | dba0a5a90fd9fb87ba5a0083b0a009e8ac74b0ea (diff) | |
download | px4-firmware-b795705640c6a14ba359e381c935351f9377aea9.tar.gz px4-firmware-b795705640c6a14ba359e381c935351f9377aea9.tar.bz2 px4-firmware-b795705640c6a14ba359e381c935351f9377aea9.zip |
improve efficiency of non-uniform rate LPFs
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 11 |
1 files 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 8acf09977..4ee2b70e6 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)); |