本帖最后由 wilsonleong 于 2020-8-10 14:22 编辑
IMU下采样
estimator_interface.h
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};
EstimatorInterface():_imu_down_sampler(FILTER_UPDATE_PERIOD_S){};
estimator_interface.cpp
const bool new_downsampled_imu_sample_ready = _imu_down_sampler.update(_newest_high_rate_imu_sample);
_imu_updated = new_downsampled_imu_sample_ready;
// accumulate and down-sample imu data and push to the buffer when new downsampled data becomes available
if (new_downsampled_imu_sample_ready) {
_imu_buffer.push(_imu_down_sampler.getDownSampledImuAndTriggerReset());
// get the oldest data from the buffer
_imu_sample_delayed = _imu_buffer.get_oldest();
// calculate the minimum interval between observations required to guarantee no loss of data
// this will occur if data is overwritten before its time stamp falls behind the fusion time horizon
_min_obs_interval_us = (_newest_high_rate_imu_sample.time_us - _imu_sample_delayed.time_us) / (_obs_buffer_length - 1);
setDragData();
}
imu_down_sampler.cpp
bool ImuDownSampler::update(const imuSample &imu_sample_new)
{
if (_do_reset) {
reset();
}
// accumulate time deltas
_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];
// use a quaternion to accumulate delta angle data
// this quaternion represents the rotation from the start to end of the accumulation period
const Quatf delta_q(AxisAnglef(imu_sample_new.delta_ang));
_delta_angle_accumulated = _delta_angle_accumulated * delta_q;
_delta_angle_accumulated.normalize();
// rotate the accumulated delta velocity data forward each time so it is always in the updated rotation frame
const Dcmf delta_R(delta_q.inversed());
_imu_down_sampled.delta_vel = delta_R * _imu_down_sampled.delta_vel;
// accumulate the most recent delta velocity data at the updated rotation frame
// 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);
_imu_down_sampled.delta_ang = _delta_angle_accumulated.to_axis_angle();
return true;
} else {
return false;
}
}
| 
发表于 2020-8-4 18:34:12
10973 浏览 15 回复
