ekf2: fix bug causing faulty imu data storage

EKF/ekf.cpp

@@ -87,7 +87,6 @@ bool Ekf::update()
 			return false;
 		}
 	}
-
 	//printStates();
 	//printStatesFast();
 	// prediction
@@ -289,7 +288,7 @@ bool Ekf::collect_imu(imuSample &imu)
 
 	if ((_dt_imu_avg * _imu_ticks >= (float)(FILTER_UPDATE_PERRIOD_MS) / 1000 && _start_predict_enabled) || 
 		_dt_imu_avg * _imu_ticks >= 0.02f){
-		_imu_sample_new = {
+		imu = {
 			delta_ang		: _q_down_sampled.to_axis_angle(),
 			delta_vel		: _imu_down_sampled.delta_vel,
 			delta_ang_dt	: _imu_down_sampled.delta_ang_dt,

EKF/estimator_base.cpp

@@ -74,8 +74,6 @@ void EstimatorBase::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64
 	if (_time_last_imu > 0) {
 		_dt_imu_avg = 0.8f * _dt_imu_avg + 0.2f * dt;
 	}
-	delta_ang_dt = delta_ang_dt / 1e6f;
-	delta_vel_dt = delta_vel_dt / 1e6f;
 
 	// copy data
 	imuSample imu_sample_new = {};
@@ -88,6 +86,7 @@ void EstimatorBase::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64
 	imu_sample_new.time_us = time_usec;
 	_imu_ticks++;
 
+
 	if (collect_imu(imu_sample_new)) {
 		_imu_buffer.push(imu_sample_new);
 		_imu_ticks = 0;