bool Ekf::collect_imu(imuSample &imu)
{
imu.delta_ang(0) = imu.delta_ang(0) * _state.gyro_scale(0);
imu.delta_ang(1) = imu.delta_ang(1) * _state.gyro_scale(1);
imu.delta_ang(2) = imu.delta_ang(2) * _state.gyro_scale(2);
imu.delta_ang -= _state.gyro_bias * imu.delta_ang_dt / (_dt_imu_avg > 0 ? _dt_imu_avg : 0.01f);
imu.delta_vel(2) -= _state.accel_z_bias * imu.delta_vel_dt / (_dt_imu_avg > 0 ? _dt_imu_avg : 0.01f);;
_imu_sample_new.delta_ang = imu.delta_ang;
_imu_sample_new.delta_vel = imu.delta_vel;
_imu_sample_new.delta_ang_dt = imu.delta_ang_dt;
_imu_sample_new.delta_vel_dt = imu.delta_vel_dt;
_imu_sample_new.time_us = imu.time_us;
_imu_down_sampled.delta_ang_dt += imu.delta_ang_dt;
_imu_down_sampled.delta_vel_dt += imu.delta_vel_dt;
Quaternion delta_q;
delta_q.rotate(imu.delta_ang);
_q_down_sampled = _q_down_sampled * delta_q;
_q_down_sampled.normalize();
matrix::Dcm<float> delta_R(delta_q.inversed());
_imu_down_sampled.delta_vel = delta_R * _imu_down_sampled.delta_vel;
_imu_down_sampled.delta_vel += imu.delta_vel;
if ((_dt_imu_avg * _imu_ticks >= (float)(FILTER_UPDATE_PERRIOD_MS) / 1000) ||
_dt_imu_avg * _imu_ticks >= 0.02f) {
imu.delta_ang = _q_down_sampled.to_axis_angle();
imu.delta_vel = _imu_down_sampled.delta_vel;
imu.delta_ang_dt = _imu_down_sampled.delta_ang_dt;
imu.delta_vel_dt = _imu_down_sampled.delta_vel_dt;
imu.time_us = imu.time_us;
_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;
_q_down_sampled(0) = 1.0f;
_q_down_sampled(1) = _q_down_sampled(2) = _q_down_sampled(3) = 0.0f;
return true;
}
return false;
}
Angle PseudoJet::DeltaRTo(PseudoJet const& jet) const
{
auto delta_r = delta_R(jet);
return delta_r == fastjet::pseudojet_invalid_rap || delta_r > 100 ? 0_rad : delta_r * rad;
}
// Accumulate imu data and store to buffer at desired rate
void EstimatorBase::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float *delta_ang,
float *delta_vel)
{
if (!_initialised) {
initialiseVariables(time_usec);
_initialised = true;
_start_predict_enabled = true;
}
float dt = (float)(time_usec - _time_last_imu) / 1000 / 1000;
dt = math::max(dt, 1.0e-4f);
dt = math::min(dt, 0.02f);
_time_last_imu = time_usec;
if (_time_last_imu > 0) {
_dt_imu_avg = 0.8f * _dt_imu_avg + 0.2f * dt;
}
// copy data
imuSample imu_sample_new = {};
memcpy(&imu_sample_new.delta_ang._data[0], delta_ang, sizeof(imu_sample_new.delta_ang._data));
memcpy(&imu_sample_new.delta_vel._data[0], delta_vel, sizeof(imu_sample_new.delta_vel._data));
imu_sample_new.delta_ang_dt = delta_ang_dt / 1e6f;
imu_sample_new.delta_vel_dt = delta_vel_dt / 1e6f;
imu_sample_new.time_us = time_usec;
imu_sample_new.delta_ang(0) = imu_sample_new.delta_ang(0) * _state.gyro_scale(0);
imu_sample_new.delta_ang(1) = imu_sample_new.delta_ang(1) * _state.gyro_scale(1);
imu_sample_new.delta_ang(2) = imu_sample_new.delta_ang(2) * _state.gyro_scale(2);
imu_sample_new.delta_ang -= _state.gyro_bias * imu_sample_new.delta_ang_dt / (_dt_imu_avg > 0 ? _dt_imu_avg : 0.01f);
imu_sample_new.delta_vel(2) -= _state.accel_z_bias * imu_sample_new.delta_vel_dt / (_dt_imu_avg > 0 ? _dt_imu_avg : 0.01f);;
// store the new sample for the complementary filter prediciton
_imu_sample_new = imu_sample_new;
_imu_down_sampled.delta_ang_dt += imu_sample_new.delta_ang_dt;
_imu_down_sampled.delta_vel_dt += imu_sample_new.delta_vel_dt;
Quaternion delta_q;
delta_q.rotate(imu_sample_new.delta_ang);
_q_down_sampled = _q_down_sampled * delta_q;
_q_down_sampled.normalize();
matrix::Dcm<float> delta_R(delta_q.inversed());
_imu_down_sampled.delta_vel = delta_R * _imu_down_sampled.delta_vel;
_imu_down_sampled.delta_vel += imu_sample_new.delta_vel;
_imu_ticks++;
if ((_dt_imu_avg * _imu_ticks >= (float)(FILTER_UPDATE_PERRIOD_MS) / 1000 && _start_predict_enabled)
|| (_dt_imu_avg * _imu_ticks >= 0.02f)) {
_imu_down_sampled.delta_ang = _q_down_sampled.to_axis_angle();
_imu_down_sampled.time_us = time_usec;
_imu_buffer.push(_imu_down_sampled);
_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;
_q_down_sampled(0) = 1.0f;
_q_down_sampled(1) = _q_down_sampled(2) = _q_down_sampled(3) = 0.0f;
_imu_ticks = 0;
_imu_updated = true;
} else {
_imu_updated = false;
}
_imu_sample_delayed = _imu_buffer.get_oldest();
}
