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(); }