forked from Archive/PX4-Autopilot
use non-uniform 1st order IIR lowpass filters for baro_gps_offset estimation
This commit is contained in:
Mark Whitehorn
parent
78b603bb35
commit
3c98c7119e
|
|
@ -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"
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -82,12 +82,11 @@
|
|||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <mathlib/math/filter/LowPassFilter2p.hpp>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#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; ///<local performance counter for gyro updates
|
||||
perf_counter_t _perf_mag; ///<local performance counter for mag updates
|
||||
perf_counter_t _perf_gps; ///<local performance counter for gps updates
|
||||
|
|
@ -399,6 +400,7 @@ FixedwingEstimator::FixedwingEstimator() :
|
|||
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")),
|
||||
_loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")),
|
||||
_perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")),
|
||||
_perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")),
|
||||
_perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")),
|
||||
|
|
@ -782,6 +784,12 @@ FixedwingEstimator::task_main()
|
|||
_gps.vel_e_m_s = 0.0f;
|
||||
_gps.vel_d_m_s = 0.0f;
|
||||
|
||||
// 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
|
||||
hrt_abstime baro_last = 0;
|
||||
|
||||
_task_running = true;
|
||||
|
||||
while (!_task_should_exit) {
|
||||
|
|
@ -800,6 +808,7 @@ FixedwingEstimator::task_main()
|
|||
}
|
||||
|
||||
perf_begin(_loop_perf);
|
||||
perf_count(_loop_intvl);
|
||||
|
||||
/* only update parameters if they changed */
|
||||
if (fds[0].revents & POLLIN) {
|
||||
|
|
@ -1055,6 +1064,12 @@ FixedwingEstimator::task_main()
|
|||
_ekf->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 */
|
||||
|
|
|
|||
Loading…
Reference in New Issue