ekf2: make filter update period configurable

- introduces new parameter EKF2_PREDICT_US to configure the filter
update period in microseconds
 - actual filter update period will be an integer multiple of IMU
This commit is contained in:
Daniel Agar
2022-01-26 10:57:09 -05:00
parent 3f25349eb9
commit ae3070bbf1
20 changed files with 856 additions and 821 deletions
+6 -5
View File
@@ -253,6 +253,9 @@ public:
// Getter for the average imu update period in s
float get_dt_imu_avg() const { return _dt_imu_avg; }
// Getter for the average EKF update period in s
float get_dt_ekf_avg() const { return _dt_ekf_avg; }
// Getter for the imu sample on the delayed time horizon
const imuSample &get_imu_sample_delayed() const { return _imu_sample_delayed; }
@@ -264,9 +267,6 @@ public:
void print_status();
static constexpr unsigned FILTER_UPDATE_PERIOD_MS{10}; // ekf prediction period in milliseconds - this should ideally be an integer multiple of the IMU time delta
static constexpr float FILTER_UPDATE_PERIOD_S{FILTER_UPDATE_PERIOD_MS * 0.001f};
protected:
EstimatorInterface() = default;
@@ -293,7 +293,8 @@ protected:
*/
uint8_t _imu_buffer_length{0};
float _dt_imu_avg{0.0f}; // average imu update period in s
float _dt_imu_avg{0.005f}; // average imu update period in s
float _dt_ekf_avg{0.010f}; ///< average update rate of the ekf in s
imuSample _imu_sample_delayed{}; // captures the imu sample on the delayed time horizon
@@ -426,7 +427,7 @@ private:
void printBufferAllocationFailed(const char *buffer_name);
ImuDownSampler _imu_down_sampler{FILTER_UPDATE_PERIOD_S};
ImuDownSampler _imu_down_sampler{_params.filter_update_interval_us};
unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec)