fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 11:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: make filter update period configurable

Browse Source 
- 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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/imu_down_sampler.cpp
+32 -28
View File
@@ -1,23 +1,26 @@
#include "imu_down_sampler.hpp"
ImuDownSampler::ImuDownSampler(float target_dt_sec) : _target_dt{target_dt_sec} { reset(); }
#include <lib/mathlib/mathlib.h>
ImuDownSampler::ImuDownSampler(int32_t &target_dt_us) : _target_dt_us(target_dt_us)
{
reset();
}
// integrate imu samples until target dt reached
// assumes that dt of the gyroscope is close to the dt of the accelerometer
// returns true if target dt is reached
bool ImuDownSampler::update(const imuSample &imu_sample_new)
{
if (_do_reset) {
reset();
}
_delta_ang_dt_avg = 0.9f * _delta_ang_dt_avg + 0.1f * imu_sample_new.delta_ang_dt;
// accumulate time deltas
_imu_down_sampled.time_us = imu_sample_new.time_us;
_imu_down_sampled.delta_ang_dt += imu_sample_new.delta_ang_dt;
_imu_down_sampled.delta_vel_dt += imu_sample_new.delta_vel_dt;
_imu_down_sampled.time_us = imu_sample_new.time_us;
_imu_down_sampled.delta_vel_clipping[0] += imu_sample_new.delta_vel_clipping[0];
_imu_down_sampled.delta_vel_clipping[1] += imu_sample_new.delta_vel_clipping[1];
_imu_down_sampled.delta_vel_clipping[2] += imu_sample_new.delta_vel_clipping[2];
_imu_down_sampled.delta_vel_clipping[0] |= imu_sample_new.delta_vel_clipping[0];
_imu_down_sampled.delta_vel_clipping[1] |= imu_sample_new.delta_vel_clipping[1];
_imu_down_sampled.delta_vel_clipping[2] |= imu_sample_new.delta_vel_clipping[2];
// use a quaternion to accumulate delta angle data
// this quaternion represents the rotation from the start to end of the accumulation period
@@ -33,33 +36,34 @@ bool ImuDownSampler::update(const imuSample &imu_sample_new)
// assume effective sample time is halfway between the previous and current rotation frame
_imu_down_sampled.delta_vel += (imu_sample_new.delta_vel + delta_R * imu_sample_new.delta_vel) * 0.5f;
// check if the target time delta between filter prediction steps has been exceeded
if (_imu_down_sampled.delta_ang_dt >= _target_dt - _imu_collection_time_adj) {
// accumulate the amount of time to advance the IMU collection time so that we meet the
// average EKF update rate requirement
_imu_collection_time_adj += 0.01f * (_imu_down_sampled.delta_ang_dt - _target_dt);
_imu_collection_time_adj = math::constrain(_imu_collection_time_adj, -0.5f * _target_dt,
0.5f * _target_dt);
_accumulated_samples++;
// required number of samples accumulated and the total time is at least half of the target
// OR total time already exceeds the target
if ((_accumulated_samples >= _required_samples && _imu_down_sampled.delta_ang_dt > _min_dt_s)
|| (_imu_down_sampled.delta_ang_dt > _target_dt_s)) {
_imu_down_sampled.delta_ang = AxisAnglef(_delta_angle_accumulated);
return true;
} else {
return false;
}
return false;
}
void ImuDownSampler::reset()
{
_imu_down_sampled.delta_ang.setZero();
_imu_down_sampled.delta_vel.setZero();
_imu_down_sampled.delta_ang_dt = 0.0f;
_imu_down_sampled.delta_vel_dt = 0.0f;
_imu_down_sampled.delta_vel_clipping[0] = false;
_imu_down_sampled.delta_vel_clipping[1] = false;
_imu_down_sampled.delta_vel_clipping[2] = false;
_imu_down_sampled = {};
_delta_angle_accumulated.setIdentity();
_do_reset = false;
_accumulated_samples = 0;
// target dt in seconds safely constrained
float target_dt_s = math::constrain(_target_dt_us, (int32_t)1000, (int32_t)100000) * 1e-6f;
_required_samples = math::max((int)roundf(target_dt_s / _delta_ang_dt_avg), 1);
_target_dt_s = _required_samples * _delta_ang_dt_avg;
// minimum delta angle dt (in addition to number of samples)
_min_dt_s = math::max(_delta_ang_dt_avg * (_required_samples - 1.f), _delta_ang_dt_avg * 0.5f);
}