From be11516dd675bc6e863006a79f0c47f0a4404492 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Thu, 19 Dec 2019 15:07:30 +0100 Subject: [PATCH] Refactor IMU downsampling into its own class --- EKF/CMakeLists.txt | 1 + EKF/common.h | 1 + EKF/ekf.cpp | 85 +------------------- EKF/ekf.h | 6 +- EKF/estimator_interface.cpp | 153 +++++++++++++++++++++--------------- EKF/estimator_interface.h | 16 ++-- EKF/imu_down_sampler.cpp | 73 +++++++++++++++++ EKF/imu_down_sampler.hpp | 66 ++++++++++++++++ 8 files changed, 247 insertions(+), 154 deletions(-) create mode 100644 EKF/imu_down_sampler.cpp create mode 100644 EKF/imu_down_sampler.hpp diff --git a/EKF/CMakeLists.txt b/EKF/CMakeLists.txt index 88d5563ff9..74f1cc2594 100644 --- a/EKF/CMakeLists.txt +++ b/EKF/CMakeLists.txt @@ -48,6 +48,7 @@ add_library(ecl_EKF vel_pos_fusion.cpp gps_yaw_fusion.cpp range_finder_checks.cpp + imu_down_sampler.cpp ) add_dependencies(ecl_EKF prebuild_targets) diff --git a/EKF/common.h b/EKF/common.h index 4478c2c962..241063990e 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -39,6 +39,7 @@ * @author Siddharth Bharat Purohit * */ +#pragma once #include diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 5fd2f28303..1c22c44882 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -47,12 +47,12 @@ bool Ekf::init(uint64_t timestamp) { bool ret = initialise_interface(timestamp); - reset(timestamp); + reset(); return ret; } -void Ekf::reset(uint64_t timestamp) +void Ekf::reset() { _state.vel.setZero(); _state.pos.setZero(); @@ -70,13 +70,6 @@ void Ekf::reset(uint64_t timestamp) initialiseCovariance(); _delta_angle_corr.setZero(); - _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.time_us = timestamp; - - _q_down_sampled.setIdentity(); _imu_updated = false; _NED_origin_initialised = false; @@ -296,76 +289,6 @@ void Ekf::predictState() _dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * input; } -bool Ekf::collect_imu(const imuSample &imu) -{ - // accumulate and downsample IMU data across a period FILTER_UPDATE_PERIOD_MS long - - // copy imu data to local variables - _imu_sample_new = imu; - - // accumulate the time deltas - _imu_down_sampled.delta_ang_dt += imu.delta_ang_dt; - _imu_down_sampled.delta_vel_dt += imu.delta_vel_dt; - - // use a quaternion to accumulate delta angle data - // this quaternion represents the rotation from the start to end of the accumulation period - Quatf delta_q; - delta_q.rotate(imu.delta_ang); - _q_down_sampled = _q_down_sampled * delta_q; - _q_down_sampled.normalize(); - - // rotate the accumulated delta velocity data forward each time so it is always in the updated rotation frame - 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.delta_vel + delta_R * imu.delta_vel) * 0.5f; - - // if the target time delta between filter prediction steps has been exceeded - // write the accumulated IMU data to the ring buffer - const float target_dt = FILTER_UPDATE_PERIOD_S; - - 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); - - imuSample imu_sample_new; - imu_sample_new.delta_ang = AxisAnglef(_q_down_sampled); - imu_sample_new.delta_vel = _imu_down_sampled.delta_vel; - imu_sample_new.delta_ang_dt = _imu_down_sampled.delta_ang_dt; - imu_sample_new.delta_vel_dt = _imu_down_sampled.delta_vel_dt; - imu_sample_new.time_us = imu.time_us; - - _imu_buffer.push(imu_sample_new); - - // 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 = (imu_sample_new.time_us - _imu_sample_delayed.time_us) / (_obs_buffer_length - 1); - - // 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; - _q_down_sampled(0) = 1.0f; - _q_down_sampled(1) = _q_down_sampled(2) = _q_down_sampled(3) = 0.0f; - - _imu_updated = true; - - } else { - _imu_updated = false; - } - - return _imu_updated; -} - /* * Implement a strapdown INS algorithm using the latest IMU data at the current time horizon. * Buffer the INS states and calculate the difference with the EKF states at the delayed fusion time horizon. @@ -379,7 +302,7 @@ bool Ekf::collect_imu(const imuSample &imu) void Ekf::calculateOutputStates() { // Use full rate IMU data at the current time horizon - const imuSample &imu = _imu_sample_new; + const imuSample &imu = _newest_high_rate_imu_sample; // correct delta angles for bias offsets const float dt_scale_correction = _dt_imu_avg / _dt_ekf_avg; @@ -598,7 +521,7 @@ Quatf Ekf::calculate_quaternion() const { // Correct delta angle data for bias errors using bias state estimates from the EKF and also apply // corrections required to track the EKF quaternion states - const Vector3f delta_angle{_imu_sample_new.delta_ang - _state.delta_ang_bias * (_dt_imu_avg / _dt_ekf_avg) + _delta_angle_corr}; + const Vector3f delta_angle{_newest_high_rate_imu_sample.delta_ang - _state.delta_ang_bias * (_dt_imu_avg / _dt_ekf_avg) + _delta_angle_corr}; // increment the quaternions using the corrected delta angle vector // the quaternions must always be normalised after modification diff --git a/EKF/ekf.h b/EKF/ekf.h index b77aa681f8..52548f9293 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -55,7 +55,7 @@ public: bool init(uint64_t timestamp) override; // set the internal states and status to their default value - void reset(uint64_t timestamp) override; + void reset() override; bool initialiseTilt(); @@ -164,8 +164,6 @@ public: // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined bool collect_gps(const gps_message &gps) override; - bool collect_imu(const imuSample &imu) override; - // get the ekf WGS-84 origin position and height and the system time it was last set // return true if the origin is valid bool get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) override; @@ -438,8 +436,6 @@ private: // output predictor states Vector3f _delta_angle_corr; ///< delta angle correction vector (rad) - imuSample _imu_down_sampled{}; ///< down sampled imu data (sensor rate -> filter update rate) - Quatf _q_down_sampled; ///< down sampled quaternion (tracking delta angles between ekf update steps) Vector3f _vel_err_integ; ///< integral of velocity tracking error (m) Vector3f _pos_err_integ; ///< integral of position tracking error (m.s) float _output_tracking_error[3] {}; ///< contains the magnitude of the angle, velocity and position track errors (rad, m/s, m) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 0e58191fbe..23070b8005 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -62,85 +62,64 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample) _dt_imu_avg = 0.8f * _dt_imu_avg + 0.2f * dt; } + _newest_high_rate_imu_sample = imu_sample; + + // Do not change order of computeVibrationMetric and checkIfVehicleAtRest + computeVibrationMetric(); + _vehicle_at_rest = checkIfVehicleAtRest(dt); + + 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(); + } +} + +void EstimatorInterface::computeVibrationMetric() +{ // calculate a metric which indicates the amount of coning vibration - Vector3f temp = imu_sample.delta_ang % _delta_ang_prev; + Vector3f temp = _newest_high_rate_imu_sample.delta_ang % _delta_ang_prev; _vibe_metrics[0] = 0.99f * _vibe_metrics[0] + 0.01f * temp.norm(); // calculate a metric which indicates the amount of high frequency gyro vibration - temp = imu_sample.delta_ang - _delta_ang_prev; - _delta_ang_prev = imu_sample.delta_ang; + temp = _newest_high_rate_imu_sample.delta_ang - _delta_ang_prev; + _delta_ang_prev = _newest_high_rate_imu_sample.delta_ang; _vibe_metrics[1] = 0.99f * _vibe_metrics[1] + 0.01f * temp.norm(); // calculate a metric which indicates the amount of high frequency accelerometer vibration - temp = imu_sample.delta_vel - _delta_vel_prev; - _delta_vel_prev = imu_sample.delta_vel; + temp = _newest_high_rate_imu_sample.delta_vel - _delta_vel_prev; + _delta_vel_prev = _newest_high_rate_imu_sample.delta_vel; _vibe_metrics[2] = 0.99f * _vibe_metrics[2] + 0.01f * temp.norm(); +} +bool EstimatorInterface::checkIfVehicleAtRest(float dt) +{ // detect if the vehicle is not moving when on ground if (!_control_status.flags.in_air) { if ((_vibe_metrics[1] * 4.0E4f > _params.is_moving_scaler) || (_vibe_metrics[2] * 2.1E2f > _params.is_moving_scaler) - || ((imu_sample.delta_ang.norm() / dt) > 0.05f * _params.is_moving_scaler)) { + || ((_newest_high_rate_imu_sample.delta_ang.norm() / dt) > 0.05f * _params.is_moving_scaler)) { - _time_last_move_detect_us = imu_sample.time_us; + _time_last_move_detect_us = _newest_high_rate_imu_sample.time_us; } - _vehicle_at_rest = ((imu_sample.time_us - _time_last_move_detect_us) > (uint64_t)1E6); + return ((_newest_high_rate_imu_sample.time_us - _time_last_move_detect_us) > (uint64_t)1E6); } else { - _time_last_move_detect_us = imu_sample.time_us; - _vehicle_at_rest = false; - } - - // accumulate and down-sample imu data and push to the buffer when new downsampled data becomes available - if (collect_imu(imu_sample)) { - - // down-sample the drag specific force data by accumulating and calculating the mean when - // sufficient samples have been collected - if ((_params.fusion_mode & MASK_USE_DRAG) && !_drag_buffer_fail) { - - // Allocate the required buffer size if not previously done - // Do not retry if allocation has failed previously - if (_drag_buffer.get_length() < _obs_buffer_length) { - _drag_buffer_fail = !_drag_buffer.allocate(_obs_buffer_length); - - if (_drag_buffer_fail) { - ECL_ERR_TIMESTAMPED("drag buffer allocation failed"); - return; - } - } - - _drag_sample_count ++; - // note acceleration is accumulated as a delta velocity - _drag_down_sampled.accelXY(0) += imu_sample.delta_vel(0); - _drag_down_sampled.accelXY(1) += imu_sample.delta_vel(1); - _drag_down_sampled.time_us += imu_sample.time_us; - _drag_sample_time_dt += imu_sample.delta_vel_dt; - - // calculate the downsample ratio for drag specific force data - uint8_t min_sample_ratio = (uint8_t) ceilf((float)_imu_buffer_length / _obs_buffer_length); - - if (min_sample_ratio < 5) { - min_sample_ratio = 5; - } - - // calculate and store means from accumulated values - if (_drag_sample_count >= min_sample_ratio) { - // note conversion from accumulated delta velocity to acceleration - _drag_down_sampled.accelXY(0) /= _drag_sample_time_dt; - _drag_down_sampled.accelXY(1) /= _drag_sample_time_dt; - _drag_down_sampled.time_us /= _drag_sample_count; - - // write to buffer - _drag_buffer.push(_drag_down_sampled); - - // reset accumulators - _drag_sample_count = 0; - _drag_down_sampled.accelXY.zero(); - _drag_down_sampled.time_us = 0; - _drag_sample_time_dt = 0.0f; - } - } + _time_last_move_detect_us = _newest_high_rate_imu_sample.time_us; + return false; } } @@ -480,6 +459,56 @@ void EstimatorInterface::setAuxVelData(uint64_t time_usec, const Vector3f &veloc } } +void EstimatorInterface::setDragData() +{ + // down-sample the drag specific force data by accumulating and calculating the mean when + // sufficient samples have been collected + if ((_params.fusion_mode & MASK_USE_DRAG) && !_drag_buffer_fail) { + + // Allocate the required buffer size if not previously done + // Do not retry if allocation has failed previously + if (_drag_buffer.get_length() < _obs_buffer_length) { + _drag_buffer_fail = !_drag_buffer.allocate(_obs_buffer_length); + + if (_drag_buffer_fail) { + printBufferAllocationFailed("drag"); + return; + } + } + + _drag_sample_count ++; + // note acceleration is accumulated as a delta velocity + _drag_down_sampled.accelXY(0) += _newest_high_rate_imu_sample.delta_vel(0); + _drag_down_sampled.accelXY(1) += _newest_high_rate_imu_sample.delta_vel(1); + _drag_down_sampled.time_us += _newest_high_rate_imu_sample.time_us; + _drag_sample_time_dt += _newest_high_rate_imu_sample.delta_vel_dt; + + // calculate the downsample ratio for drag specific force data + uint8_t min_sample_ratio = (uint8_t) ceilf((float)_imu_buffer_length / _obs_buffer_length); + + if (min_sample_ratio < 5) { + min_sample_ratio = 5; + } + + // calculate and store means from accumulated values + if (_drag_sample_count >= min_sample_ratio) { + // note conversion from accumulated delta velocity to acceleration + _drag_down_sampled.accelXY(0) /= _drag_sample_time_dt; + _drag_down_sampled.accelXY(1) /= _drag_sample_time_dt; + _drag_down_sampled.time_us /= _drag_sample_count; + + // write to buffer + _drag_buffer.push(_drag_down_sampled); + + // reset accumulators + _drag_sample_count = 0; + _drag_down_sampled.accelXY.zero(); + _drag_down_sampled.time_us = 0; + _drag_sample_time_dt = 0.0f; + } + } +} + bool EstimatorInterface::initialise_interface(uint64_t timestamp) { // find the maximum time delay the buffers are required to handle @@ -547,7 +576,7 @@ void EstimatorInterface::printBufferAllocationFailed(const char * buffer_name) { if(buffer_name) { - ECL_ERR_TIMESTAMPED("%s buffer allocation failed", buffer_name); + ECL_ERR("%s buffer allocation failed", buffer_name); } } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 0ce8d949ea..c131302068 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -45,6 +45,7 @@ #include "common.h" #include "RingBuffer.h" #include "AlphaFilter.hpp" +#include "imu_down_sampler.hpp" #include #include @@ -58,11 +59,11 @@ class EstimatorInterface public: typedef AlphaFilter AlphaFilterVector3f; - EstimatorInterface() = default; + EstimatorInterface():_imu_down_sampler(FILTER_UPDATE_PERIOD_S){}; virtual ~EstimatorInterface() = default; virtual bool init(uint64_t timestamp) = 0; - virtual void reset(uint64_t timestamp) = 0; + virtual void reset() = 0; virtual bool update() = 0; @@ -171,9 +172,6 @@ public: // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined virtual bool collect_gps(const gps_message &gps) = 0; - // accumulate and downsample IMU data to the EKF prediction rate - virtual bool collect_imu(const imuSample &imu) = 0; - void setIMUData(const imuSample &imu_sample); // legacy interface for compatibility (2018-09-14) @@ -401,6 +399,8 @@ protected: parameters _params; // filter parameters + ImuDownSampler _imu_down_sampler; + /* OBS_BUFFER_LENGTH defines how many observations (non-IMU measurements) we can buffer which sets the maximum frequency at which we can process non-IMU measurements. Measurements that @@ -453,7 +453,7 @@ protected: outputSample _output_new{}; // filter output on the non-delayed time horizon outputVert _output_vert_delayed{}; // vertical filter output on the delayed time horizon outputVert _output_vert_new{}; // vertical filter output on the non-delayed time horizon - imuSample _imu_sample_new{}; // imu sample capturing the newest imu data + imuSample _newest_high_rate_imu_sample{}; // imu sample capturing the newest imu data Matrix3f _R_to_earth_now; // rotation matrix from body to earth frame at current time Vector3f _vel_imu_rel_body_ned; // velocity of IMU relative to body origin in NED earth frame Vector3f _vel_deriv_ned; // velocity derivative at the IMU in NED earth frame (m/s/s) @@ -566,5 +566,9 @@ protected: // calculate the inverse rotation matrix from a quaternion rotation Matrix3f quat_to_invrotmat(const Quatf &quat); + void setDragData(); + + void computeVibrationMetric(); + bool checkIfVehicleAtRest(float dt); void printBufferAllocationFailed(const char * buffer_name); }; diff --git a/EKF/imu_down_sampler.cpp b/EKF/imu_down_sampler.cpp new file mode 100644 index 0000000000..4d44cbb080 --- /dev/null +++ b/EKF/imu_down_sampler.cpp @@ -0,0 +1,73 @@ +#include "imu_down_sampler.hpp" + +ImuDownSampler::ImuDownSampler(float target_dt_sec): +_target_dt{target_dt_sec}, +_imu_collection_time_adj{0.0f} +{ + reset(); + _imu_down_sampled.time_us = 0.0f; +} + +ImuDownSampler::~ImuDownSampler() +{ +} + +// integrate imu samples until target dt reached +// returns true if target dt is reached +bool ImuDownSampler::update(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; + + // 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; + } +} + +imuSample ImuDownSampler::getDownSampledImuAndTriggerReset() +{ + _do_reset = true; + return _imu_down_sampled; +} + +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; + _delta_angle_accumulated.setIdentity(); + _do_reset = false; +} diff --git a/EKF/imu_down_sampler.hpp b/EKF/imu_down_sampler.hpp new file mode 100644 index 0000000000..21583dc4a6 --- /dev/null +++ b/EKF/imu_down_sampler.hpp @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * Copyright (c) 2019 Estimation and Control Library (ECL). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name ECL nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Downsamples IMU data to a lower rate such that EKF predicition can happen less frequent + * @author Kamil Ritz + */ +#pragma once + +#include +#include + +#include "common.h" + +using namespace estimator; + +class ImuDownSampler +{ +public: + ImuDownSampler(float target_dt_sec); + ~ImuDownSampler(); + + bool update(imuSample imu_sample_new); + imuSample getDownSampledImuAndTriggerReset(); + +private: + imuSample _imu_down_sampled; + Quatf _delta_angle_accumulated; + const float _target_dt; // [sec] + float _imu_collection_time_adj; + bool _do_reset; + + void reset(); + +}; +