ekf2: ImuDownSampler simple accumulation

src/modules/ekf2/EKF/imu_down_sampler/imu_down_sampler.cpp

@@ -7,44 +7,38 @@ ImuDownSampler::ImuDownSampler(int32_t &target_dt_us) : _target_dt_us(target_dt_
 	reset();
 }
 
-// integrate imu samples until target dt reached
-// assumes that dt of the gyroscope is close to the dt of the accelerometer
-// returns true if target dt is reached
-bool ImuDownSampler::update(const imuSample &imu_sample_new)
+bool ImuDownSampler::update(const imuSample &imu)
 {
-	_delta_ang_dt_avg = 0.9f * _delta_ang_dt_avg + 0.1f * imu_sample_new.delta_ang_dt;
+	_delta_ang_dt_avg = 0.9f * _delta_ang_dt_avg + 0.1f * imu.delta_ang_dt;
 
 	// accumulate time deltas
-	_imu_down_sampled.time_us = imu_sample_new.time_us;
-	_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.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;
+	_imu_down_sampled.time_us = imu.time_us;
+	_imu_down_sampled.delta_ang += imu.delta_ang;
+	_imu_down_sampled.delta_vel += imu.delta_vel;
+	_imu_down_sampled.delta_ang_dt += imu.delta_ang_dt;
+	_imu_down_sampled.delta_vel_dt += imu.delta_vel_dt;
+	_imu_down_sampled.delta_vel_clipping[0] |= imu.delta_vel_clipping[0];
+	_imu_down_sampled.delta_vel_clipping[1] |= imu.delta_vel_clipping[1];
+	_imu_down_sampled.delta_vel_clipping[2] |= imu.delta_vel_clipping[2];
 
 	_accumulated_samples++;
 
-
 	// required number of samples accumulated and the total time is at least half of the target
 	//  OR total time already exceeds the target
-	if ((_accumulated_samples >= _required_samples && _imu_down_sampled.delta_ang_dt > _min_dt_s)
-	    || (_imu_down_sampled.delta_ang_dt > _target_dt_s)) {
 
-		_imu_down_sampled.delta_ang = AxisAnglef(_delta_angle_accumulated);
+	// target dt in seconds safely constrained
+	const float constrained_target_dt_s = math::constrain(_target_dt_us, (int32_t)1000, (int32_t)100000) * 1e-6f;
+	const int required_samples = math::max((int)roundf(constrained_target_dt_s / _delta_ang_dt_avg), 1);
+
+	float target_dt_s = required_samples * _delta_ang_dt_avg;
+
+	// minimum delta angle dt (in addition to number of samples)
+	float min_dt_s = math::max(_delta_ang_dt_avg * (required_samples - 1.f), _delta_ang_dt_avg * 0.5f);
+
+	if ((_accumulated_samples >= required_samples && _imu_down_sampled.delta_ang_dt > min_dt_s)
+	    || (_imu_down_sampled.delta_ang_dt > target_dt_s)
+	) {
+
 		return true;
 	}
 
@@ -54,16 +48,5 @@ bool ImuDownSampler::update(const imuSample &imu_sample_new)
 void ImuDownSampler::reset()
 {
 	_imu_down_sampled = {};
-	_delta_angle_accumulated.setIdentity();
 	_accumulated_samples = 0;
-
-	// target dt in seconds safely constrained
-	float target_dt_s = math::constrain(_target_dt_us, (int32_t)1000, (int32_t)100000) * 1e-6f;
-
-	_required_samples = math::max((int)roundf(target_dt_s / _delta_ang_dt_avg), 1);
-
-	_target_dt_s = _required_samples * _delta_ang_dt_avg;
-
-	// minimum delta angle dt (in addition to number of samples)
-	_min_dt_s = math::max(_delta_ang_dt_avg * (_required_samples - 1.f), _delta_ang_dt_avg * 0.5f);
 }

src/modules/ekf2/EKF/imu_down_sampler/imu_down_sampler.hpp

@@ -51,6 +51,8 @@ public:
 	explicit ImuDownSampler(int32_t &target_dt_us);
 	~ImuDownSampler() = default;
 
+	// integrate imu samples until target dt reached
+	// returns true if target dt is reached
 	bool update(const imuSample &imu_sample_new);
 
 	imuSample getDownSampledImuAndTriggerReset()
@@ -64,16 +66,10 @@ private:
 	void reset();
 
 	imuSample _imu_down_sampled{};
-	Quatf _delta_angle_accumulated{};
-
 	int _accumulated_samples{0};
-	int _required_samples{1};
 
 	int32_t &_target_dt_us;
 
-	float _target_dt_s{0.010f};
-	float _min_dt_s{0.005f};
-
 	float _delta_ang_dt_avg{0.005f};
 };
 #endif // !EKF_IMU_DOWN_SAMPLER_HPP