Refactor IMU downsampling into its own class

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)

EKF/common.h

@@ -39,6 +39,7 @@
  * @author Siddharth Bharat Purohit <siddharthbharatpurohit@gmail.com>
  *
  */
+#pragma once
 
 #include <matrix/math.hpp>
 

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

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)

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

EKF/estimator_interface.h

@@ -45,6 +45,7 @@
 #include "common.h"
 #include "RingBuffer.h"
 #include "AlphaFilter.hpp"
+#include "imu_down_sampler.hpp"
 
 #include <geo/geo.h>
 #include <matrix/math.hpp>
@@ -58,11 +59,11 @@ class EstimatorInterface
 public:
 	typedef AlphaFilter<Vector3f> 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);
 };

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;
+}

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 <ka.ritz@hotmail.com>
+ */
+#pragma once
+
+#include <matrix/math.hpp>
+#include <mathlib/mathlib.h>
+
+#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();
+
+};
+