ekf2: make filter update period configurable

- introduces new parameter EKF2_PREDICT_US to configure the filter
update period in microseconds
 - actual filter update period will be an integer multiple of IMU

src/modules/ekf2/EKF/common.h

@@ -95,10 +95,10 @@ struct outputVert {
 
 struct imuSample {
 	uint64_t    time_us{0};		///< timestamp of the measurement (uSec)
-	Vector3f    delta_ang;		///< delta angle in body frame (integrated gyro measurements) (rad)
+	Vector3f    delta_ang{};		///< delta angle in body frame (integrated gyro measurements) (rad)
-	Vector3f    delta_vel;		///< delta velocity in body frame (integrated accelerometer measurements) (m/sec)
+	Vector3f    delta_vel{};		///< delta velocity in body frame (integrated accelerometer measurements) (m/sec)
-	float       delta_ang_dt;	///< delta angle integration period (sec)
+	float       delta_ang_dt{0.f};	///< delta angle integration period (sec)
-	float       delta_vel_dt;	///< delta velocity integration period (sec)
+	float       delta_vel_dt{0.f};	///< delta velocity integration period (sec)
 	bool        delta_vel_clipping[3] {}; ///< true (per axis) if this sample contained any accelerometer clipping
 };
 
@@ -214,6 +214,9 @@ enum TerrainFusionMask : int32_t {
 #define GNDEFFECT_TIMEOUT	10E6	///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec)
 
 struct parameters {
+
+	int32_t filter_update_interval_us{10000}; ///< filter update interval in microseconds
+
 	// measurement source control
 	int32_t fusion_mode{MASK_USE_GPS};		///< bitmasked integer that selects which aiding sources will be used
 	int32_t vdist_sensor_type{VDIST_SENSOR_BARO};	///< selects the primary source for height data

src/modules/ekf2/EKF/control.cpp

@@ -800,7 +800,7 @@ void Ekf::checkVerticalAccelerationHealth()
 	}
 
 	// Check for more than 50% clipping affected IMU samples within the past 1 second
-	const uint16_t clip_count_limit = 1000 / FILTER_UPDATE_PERIOD_MS;
+	const uint16_t clip_count_limit = 1.f / _dt_ekf_avg;
 	const bool is_clipping = _imu_sample_delayed.delta_vel_clipping[0] ||
 				 _imu_sample_delayed.delta_vel_clipping[1] ||
 				 _imu_sample_delayed.delta_vel_clipping[2];

src/modules/ekf2/EKF/covariance.cpp

@@ -56,7 +56,7 @@ void Ekf::initialiseCovariance()
 	_delta_angle_bias_var_accum.setZero();
 	_delta_vel_bias_var_accum.setZero();
 
-	const float dt = FILTER_UPDATE_PERIOD_S;
+	const float dt = _dt_ekf_avg;
 
 	resetQuatCov();
 
@@ -122,8 +122,8 @@ void Ekf::predictCovariance()
 	const float &dvz_b = _state.delta_vel_bias(2);
 
 	// Use average update interval to reduce accumulated covariance prediction errors due to small single frame dt values
-	const float dt = FILTER_UPDATE_PERIOD_S;
+	const float dt = _dt_ekf_avg;
-	const float dt_inv = 1.0f / dt;
+	const float dt_inv = 1.f / dt;
 
 	// convert rate of change of rate gyro bias (rad/s**2) as specified by the parameter to an expected change in delta angle (rad) since the last update
 	const float d_ang_bias_sig = dt * dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1.0f);

src/modules/ekf2/EKF/ekf.cpp

@@ -285,16 +285,16 @@ void Ekf::predictState()
 	float input = 0.5f * (_imu_sample_delayed.delta_vel_dt + _imu_sample_delayed.delta_ang_dt);
 
 	// filter and limit input between -50% and +100% of nominal value
-	input = math::constrain(input, 0.5f * FILTER_UPDATE_PERIOD_S, 2.0f * FILTER_UPDATE_PERIOD_S);
+	const float filter_update_s = 1e-6f * _params.filter_update_interval_us;
+	input = math::constrain(input, 0.5f * filter_update_s, 2.f * filter_update_s);
 	_dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * input;
 
 	// some calculations elsewhere in code require a raw angular rate vector so calculate here to avoid duplication
-	// protect angainst possible small timesteps resulting from timing slip on previous frame that can drive spikes into the rate
+	// protect against possible small timesteps resulting from timing slip on previous frame that can drive spikes into the rate
 	// due to insufficient averaging
-	if (_imu_sample_delayed.delta_ang_dt > 0.25f * FILTER_UPDATE_PERIOD_S) {
+	if (_imu_sample_delayed.delta_ang_dt > 0.25f * _dt_ekf_avg) {
 		_ang_rate_delayed_raw = _imu_sample_delayed.delta_ang / _imu_sample_delayed.delta_ang_dt;
 	}
-
 }
 
 /*

src/modules/ekf2/EKF/ekf.h

@@ -344,8 +344,6 @@ private:
 		Quatf quat_change;	///< quaternion delta due to last reset - multiply pre-reset quaternion by this to get post-reset quaternion
 	} _state_reset_status{};	///< reset event monitoring structure containing velocity, position, height and yaw reset information
 
-	float _dt_ekf_avg{FILTER_UPDATE_PERIOD_S}; ///< average update rate of the ekf
-
 	Vector3f _ang_rate_delayed_raw{};	///< uncorrected angular rate vector at fusion time horizon (rad/sec)
 
 	stateSample _state{};		///< state struct of the ekf running at the delayed time horizon
@@ -387,9 +385,7 @@ private:
 	uint64_t _time_last_gps_yaw_data{0};	///< time the last GPS yaw measurement was available (uSec)
 	uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
 
-
 	Vector2f _last_known_posNE{};		///< last known local NE position vector (m)
-	float _imu_collection_time_adj{0.0f};	///< the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec)
 
 	uint64_t _time_acc_bias_check{0};	///< last time the  accel bias check passed (uSec)
 	uint64_t _delta_time_baro_us{0};	///< delta time between two consecutive delayed baro samples from the buffer (uSec)

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -897,7 +897,7 @@ void Ekf::resetGyroBias()
 
 	// Zero the corresponding covariances and set
 	// variances to the values use for initial alignment
-	P.uncorrelateCovarianceSetVariance<3>(10, sq(_params.switch_on_gyro_bias * FILTER_UPDATE_PERIOD_S));
+	P.uncorrelateCovarianceSetVariance<3>(10, sq(_params.switch_on_gyro_bias * _dt_ekf_avg));
 }
 
 void Ekf::resetAccelBias()
@@ -907,7 +907,7 @@ void Ekf::resetAccelBias()
 
 	// Zero the corresponding covariances and set
 	// variances to the values use for initial alignment
-	P.uncorrelateCovarianceSetVariance<3>(13, sq(_params.switch_on_accel_bias * FILTER_UPDATE_PERIOD_S));
+	P.uncorrelateCovarianceSetVariance<3>(13, sq(_params.switch_on_accel_bias * _dt_ekf_avg));
 
 	// Set previous frame values
 	_prev_dvel_bias_var = P.slice<3, 3>(13, 13).diag();

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -141,7 +141,7 @@ void EstimatorInterface::setMagData(const magSample &mag_sample)
 
 		mag_sample_new.time_us = mag_sample.time_us;
 		mag_sample_new.time_us -= static_cast<uint64_t>(_params.mag_delay_ms * 1000);
-		mag_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
+		mag_sample_new.time_us -= static_cast<uint64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
 
 		mag_sample_new.mag = mag_sample.mag;
 
@@ -175,7 +175,7 @@ void EstimatorInterface::setGpsData(const gps_message &gps)
 		gpsSample gps_sample_new;
 
 		gps_sample_new.time_us = gps.time_usec - static_cast<uint64_t>(_params.gps_delay_ms * 1000);
-		gps_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
+		gps_sample_new.time_us -= static_cast<uint64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
 
 		gps_sample_new.vel = gps.vel_ned;
 
@@ -237,7 +237,7 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample)
 
 		baro_sample_new.time_us = baro_sample.time_us;
 		baro_sample_new.time_us -= static_cast<uint64_t>(_params.baro_delay_ms * 1000);
-		baro_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
+		baro_sample_new.time_us -= static_cast<uint64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
 
 		_baro_buffer->push(baro_sample_new);
 	} else {
@@ -270,7 +270,7 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
 		airspeedSample airspeed_sample_new = airspeed_sample;
 
 		airspeed_sample_new.time_us -= static_cast<uint64_t>(_params.airspeed_delay_ms * 1000);
-		airspeed_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
+		airspeed_sample_new.time_us -= static_cast<uint64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
 
 		_airspeed_buffer->push(airspeed_sample_new);
 	}
@@ -300,7 +300,7 @@ void EstimatorInterface::setRangeData(const rangeSample &range_sample)
 
 		rangeSample range_sample_new = range_sample;
 		range_sample_new.time_us -= static_cast<uint64_t>(_params.range_delay_ms * 1000);
-		range_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
+		range_sample_new.time_us -= static_cast<uint64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
 
 		_range_buffer->push(range_sample_new);
 	}
@@ -331,7 +331,7 @@ void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
 		flowSample optflow_sample_new = flow;
 
 		optflow_sample_new.time_us -= static_cast<uint64_t>(_params.flow_delay_ms * 1000);
-		optflow_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
+		optflow_sample_new.time_us -= static_cast<uint64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
 
 		_flow_buffer->push(optflow_sample_new);
 	}
@@ -363,7 +363,7 @@ void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
 		extVisionSample ev_sample_new = evdata;
 		// calculate the system time-stamp for the mid point of the integration period
 		ev_sample_new.time_us -= static_cast<uint64_t>(_params.ev_delay_ms * 1000);
-		ev_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
+		ev_sample_new.time_us -= static_cast<uint64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
 
 		_ext_vision_buffer->push(ev_sample_new);
 	}
@@ -394,7 +394,7 @@ void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)
 		auxVelSample auxvel_sample_new = auxvel_sample;
 
 		auxvel_sample_new.time_us -= static_cast<uint64_t>(_params.auxvel_delay_ms * 1000);
-		auxvel_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
+		auxvel_sample_new.time_us -= static_cast<uint64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
 
 		_auxvel_buffer->push(auxvel_sample_new);
 	}
@@ -490,14 +490,16 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 		max_time_delay_ms = math::max(_params.ev_delay_ms, max_time_delay_ms);
 	}
 
-	// calculate the IMU buffer length required to accommodate the maximum delay with some allowance for jitter
+	const float filter_update_period_ms = _params.filter_update_interval_us / 1000.f;
-	_imu_buffer_length = ceilf(max_time_delay_ms / FILTER_UPDATE_PERIOD_MS);
+
+	// calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter
+	_imu_buffer_length = ceilf(max_time_delay_ms / filter_update_period_ms);
 
 	// set the observation buffer length to handle the minimum time of arrival between observations in combination
 	// with the worst case delay from current time to ekf fusion time
 	// allow for worst case 50% extension of the ekf fusion time horizon delay due to timing jitter
 	const float ekf_delay_ms = max_time_delay_ms * 1.5f;
-	_obs_buffer_length = roundf(ekf_delay_ms / FILTER_UPDATE_PERIOD_MS);
+	_obs_buffer_length = roundf(ekf_delay_ms / filter_update_period_ms);
 
 	// limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate)
 	_obs_buffer_length = math::min(_obs_buffer_length, _imu_buffer_length);
@@ -558,6 +560,8 @@ void EstimatorInterface::printBufferAllocationFailed(const char *buffer_name)
 void EstimatorInterface::print_status()
 {
 	printf("IMU average dt: %.6f seconds\n", (double)_dt_imu_avg);
+	printf("EKF average dt: %.6f seconds\n", (double)_dt_ekf_avg);
+
 	printf("IMU buffer: %d (%d Bytes)\n", _imu_buffer.get_length(), _imu_buffer.get_total_size());
 
 	printf("minimum observation interval %d us\n", _min_obs_interval_us);

src/modules/ekf2/EKF/estimator_interface.h

@@ -253,6 +253,9 @@ public:
 	// Getter for the average imu update period in s
 	float get_dt_imu_avg() const { return _dt_imu_avg; }
 
+	// Getter for the average EKF update period in s
+	float get_dt_ekf_avg() const { return _dt_ekf_avg; }
+
 	// Getter for the imu sample on the delayed time horizon
 	const imuSample &get_imu_sample_delayed() const { return _imu_sample_delayed; }
 
@@ -264,9 +267,6 @@ public:
 
 	void print_status();
 
-	static constexpr unsigned FILTER_UPDATE_PERIOD_MS{10};	// ekf prediction period in milliseconds - this should ideally be an integer multiple of the IMU time delta
-	static constexpr float FILTER_UPDATE_PERIOD_S{FILTER_UPDATE_PERIOD_MS * 0.001f};
-
 protected:
 
 	EstimatorInterface() = default;
@@ -293,7 +293,8 @@ protected:
 	*/
 	uint8_t _imu_buffer_length{0};
 
-	float _dt_imu_avg{0.0f};	// average imu update period in s
+	float _dt_imu_avg{0.005f};	// average imu update period in s
+	float _dt_ekf_avg{0.010f}; ///< average update rate of the ekf in s
 
 	imuSample _imu_sample_delayed{};	// captures the imu sample on the delayed time horizon
 
@@ -426,7 +427,7 @@ private:
 
 	void printBufferAllocationFailed(const char *buffer_name);
 
-	ImuDownSampler _imu_down_sampler{FILTER_UPDATE_PERIOD_S};
+	ImuDownSampler _imu_down_sampler{_params.filter_update_interval_us};
 
 	unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec)
 

src/modules/ekf2/EKF/imu_down_sampler.cpp

@@ -1,23 +1,26 @@
 #include "imu_down_sampler.hpp"
 
-ImuDownSampler::ImuDownSampler(float target_dt_sec) : _target_dt{target_dt_sec} { reset(); }
+#include <lib/mathlib/mathlib.h>
+
+ImuDownSampler::ImuDownSampler(int32_t &target_dt_us) : _target_dt_us(target_dt_us)
+{
+	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)
 {
-	if (_do_reset) {
+	_delta_ang_dt_avg = 0.9f * _delta_ang_dt_avg + 0.1f * imu_sample_new.delta_ang_dt;
-		reset();
-	}
 
 	// 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.time_us = imu_sample_new.time_us;
+	_imu_down_sampled.delta_vel_clipping[0] |= imu_sample_new.delta_vel_clipping[0];
-	_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[1] += imu_sample_new.delta_vel_clipping[1];
+	_imu_down_sampled.delta_vel_clipping[2] |= imu_sample_new.delta_vel_clipping[2];
-	_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
@@ -33,33 +36,34 @@ bool ImuDownSampler::update(const imuSample &imu_sample_new)
 	// 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
+	_accumulated_samples++;
-	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
+	// required number of samples accumulated and the total time is at least half of the target
-		_imu_collection_time_adj += 0.01f * (_imu_down_sampled.delta_ang_dt - _target_dt);
+	//  OR total time already exceeds the target
-		_imu_collection_time_adj = math::constrain(_imu_collection_time_adj, -0.5f * _target_dt,
+	if ((_accumulated_samples >= _required_samples && _imu_down_sampled.delta_ang_dt > _min_dt_s)
-					   0.5f * _target_dt);
+	    || (_imu_down_sampled.delta_ang_dt > _target_dt_s)) {
 
 		_imu_down_sampled.delta_ang = AxisAnglef(_delta_angle_accumulated);
-
 		return true;
-
+	}
-	} else {
 
 	return false;
-	}
 }
 
 void ImuDownSampler::reset()
 {
-	_imu_down_sampled.delta_ang.setZero();
+	_imu_down_sampled = {};
-	_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.delta_vel_clipping[0] = false;
-	_imu_down_sampled.delta_vel_clipping[1] = false;
-	_imu_down_sampled.delta_vel_clipping[2] = false;
 	_delta_angle_accumulated.setIdentity();
-	_do_reset = false;
+	_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.hpp

@@ -48,15 +48,16 @@ using namespace estimator;
 class ImuDownSampler
 {
 public:
-	explicit ImuDownSampler(float target_dt_sec);
+	explicit ImuDownSampler(int32_t &target_dt_us);
 	~ImuDownSampler() = default;
 
 	bool update(const imuSample &imu_sample_new);
 
 	imuSample getDownSampledImuAndTriggerReset()
 	{
-		_do_reset = true;
+		imuSample imu{_imu_down_sampled};
-		return _imu_down_sampled;
+		reset();
+		return imu;
 	}
 
 private:
@@ -64,8 +65,15 @@ private:
 
 	imuSample _imu_down_sampled{};
 	Quatf _delta_angle_accumulated{};
-	const float _target_dt;  // [sec]
+
-	float _imu_collection_time_adj{0.f};
+	int _accumulated_samples{0};
-	bool _do_reset{true};
+	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

src/modules/ekf2/EKF/mag_control.cpp

@@ -155,7 +155,7 @@ void Ekf::runOnGroundYawReset()
 			if (_mag_inhibit_yaw_reset_req) {
 				_mag_inhibit_yaw_reset_req = false;
 				// Zero the yaw bias covariance and set the variance to the initial alignment uncertainty
-				P.uncorrelateCovarianceSetVariance<1>(12, sq(_params.switch_on_gyro_bias * FILTER_UPDATE_PERIOD_S));
+				P.uncorrelateCovarianceSetVariance<1>(12, sq(_params.switch_on_gyro_bias * _dt_ekf_avg));
 			}
 		}
 	}
@@ -188,7 +188,7 @@ void Ekf::runInAirYawReset()
 			if (_mag_inhibit_yaw_reset_req) {
 				_mag_inhibit_yaw_reset_req = false;
 				// Zero the yaw bias covariance and set the variance to the initial alignment uncertainty
-				P.uncorrelateCovarianceSetVariance<1>(12, sq(_params.switch_on_gyro_bias * FILTER_UPDATE_PERIOD_S));
+				P.uncorrelateCovarianceSetVariance<1>(12, sq(_params.switch_on_gyro_bias * _dt_ekf_avg));
 			}
 		}
 

src/modules/ekf2/EKF2.cpp

@@ -57,6 +57,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_odometry_pub(multi_mode ? ORB_ID(estimator_odometry) : ORB_ID(vehicle_odometry)),
 	_wind_pub(multi_mode ? ORB_ID(estimator_wind) : ORB_ID(wind)),
 	_params(_ekf.getParamHandle()),
+	_param_ekf2_predict_us(_params->filter_update_interval_us),
 	_param_ekf2_mag_delay(_params->mag_delay_ms),
 	_param_ekf2_baro_delay(_params->baro_delay_ms),
 	_param_ekf2_gps_delay(_params->gps_delay_ms),
@@ -228,8 +229,10 @@ bool EKF2::multi_init(int imu, int mag)
 
 int EKF2::print_status()
 {
-	PX4_INFO_RAW("ekf2:%d attitude: %d, local position: %d, global position: %d\n", _instance, _ekf.attitude_valid(),
+	PX4_INFO_RAW("ekf2:%d EKF dt: %.4fs, IMU dt: %.4fs, attitude: %d, local position: %d, global position: %d\n",
+		     _instance, (double)_ekf.get_dt_ekf_avg(), (double)_ekf.get_dt_imu_avg(), _ekf.attitude_valid(),
 		     _ekf.local_position_is_valid(), _ekf.global_position_is_valid());
+
 	perf_print_counter(_ecl_ekf_update_perf);
 	perf_print_counter(_ecl_ekf_update_full_perf);
 	perf_print_counter(_msg_missed_imu_perf);

src/modules/ekf2/EKF2.hpp

@@ -315,6 +315,7 @@ private:
 	parameters *_params;	///< pointer to ekf parameter struct (located in _ekf class instance)
 
 	DEFINE_PARAMETERS(
+		(ParamExtInt<px4::params::EKF2_PREDICT_US>) _param_ekf2_predict_us,
 		(ParamExtFloat<px4::params::EKF2_MAG_DELAY>)
 		_param_ekf2_mag_delay,	///< magnetometer measurement delay relative to the IMU (mSec)
 		(ParamExtFloat<px4::params::EKF2_BARO_DELAY>)

src/modules/ekf2/ekf2_params.c

@@ -39,6 +39,19 @@
  *
  */
 
+/**
+ * EKF prediction period
+ *
+ * EKF prediction period in microseconds. This should ideally be an integer multiple of the IMU time delta.
+ * Actual filter update will be an integer multiple of IMU update.
+ *
+ * @group EKF2
+ * @min 1000
+ * @max 20000
+ * @unit us
+ */
+PARAM_DEFINE_INT32(EKF2_PREDICT_US, 10000);
+
 /**
  * Magnetometer measurement delay relative to IMU measurements
  *

src/modules/ekf2/test/test_EKF_fusionLogic.cpp

@@ -387,9 +387,9 @@ TEST_F(EkfFusionLogicTest, doRangeHeightFusion)
 	// THEN: EKF should intend to fuse range height
 	EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion());
 
-	const float dt = 8e-3f;
+	const float dt = 5e-3f;
 
-	for (int i = 0; i < 5; i++) {
+	for (int i = 0; i < 10; i++) {
 		_sensor_simulator.runSeconds(dt);
 		// THEN: EKF should intend to fuse range height, even if
 		// there is no new data at each EKF iteration (EKF rate > sensor rate)

src/modules/ekf2/test/test_EKF_imuSampling.cpp

@@ -64,8 +64,8 @@ TEST_P(EkfImuSamplingTest, imuSamplingAtMultipleRates)
 	// WHEN: adding imu samples at a same or lower rate than the update loop
 	// THEN: imu sample should reach buffer unchanged
 
-	uint32_t dt_us = std::get<0>(GetParam()) * (_ekf.FILTER_UPDATE_PERIOD_MS * 1000);
+	uint32_t dt_us = std::get<0>(GetParam()) * (10 * 1000);
-	uint32_t expected_dt_us = std::get<1>(GetParam()) * (_ekf.FILTER_UPDATE_PERIOD_MS * 1000);
+	uint32_t expected_dt_us = std::get<1>(GetParam()) * (10 * 1000);
 
 	Vector3f ang_vel = std::get<2>(GetParam());
 	Vector3f accel = std::get<3>(GetParam());
@@ -78,7 +78,7 @@ TEST_P(EkfImuSamplingTest, imuSamplingAtMultipleRates)
 	// The higher the imu rate is the more measurements we have to set before reaching the FILTER_UPDATE_PERIOD
 	int n_samples = 0;
 
-	for (int i = 0; i < (int)20 / std::get<0>(GetParam()); ++i) {
+	for (int i = 0; i < (int)30 / std::get<0>(GetParam()); ++i) {
 		n_samples++;
 		imu_sample.time_us = _t_us;
 		_ekf.setIMUData(imu_sample);
@@ -122,7 +122,8 @@ INSTANTIATE_TEST_SUITE_P(imuSamplingAtMultipleRates,
 
 TEST_F(EkfImuSamplingTest, accelDownSampling)
 {
-	ImuDownSampler sampler(0.008f);
+	int32_t target_dt_us = 8000;
+	ImuDownSampler sampler(target_dt_us);
 
 	Vector3f ang_vel{0.0f, 0.0f, 0.0f};
 	Vector3f accel{-0.46f, 0.87f, 0.0f};
@@ -150,7 +151,8 @@ TEST_F(EkfImuSamplingTest, accelDownSampling)
 
 TEST_F(EkfImuSamplingTest, gyroDownSampling)
 {
-	ImuDownSampler sampler(0.008f);
+	int32_t target_dt_us = 8000;
+	ImuDownSampler sampler(target_dt_us);
 
 	Vector3f ang_vel{0.0f, 0.0f, 1.0f};
 	Vector3f accel{0.0f, 0.0f, 0.0f};

src/modules/ekf2/test/test_EKF_terrain_estimator.cpp

@@ -73,7 +73,7 @@ public:
 		_ekf_wrapper.enableGpsFusion();
 		_ekf_wrapper.setBaroHeight();
 		_sensor_simulator.runSeconds(2); // Run to pass the GPS checks
-		_sensor_simulator.runSeconds(3.2); // And a bit more to start the GPS fusion TODO: this shouldn't be necessary
+		_sensor_simulator.runSeconds(3.5); // And a bit more to start the GPS fusion TODO: this shouldn't be necessary
 		EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
 
 		const Vector3f simulated_velocity(0.5f, -1.0f, 0.f);

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -145,9 +145,9 @@ bool VehicleIMU::ParametersUpdate(bool force)
 			}
 		}
 
-		// constrain IMU integration time 1-10 milliseconds (100-1000 Hz)
+		// constrain IMU integration time 1-20 milliseconds (50-1000 Hz)
 		int32_t imu_integration_rate_hz = constrain(_param_imu_integ_rate.get(),
-						  (int32_t)100, math::max(_param_imu_gyro_ratemax.get(), (int32_t) 1000));
+						  (int32_t)50, math::max(_param_imu_gyro_ratemax.get(), (int32_t) 1000));
 
 		if (imu_integration_rate_hz != _param_imu_integ_rate.get()) {
 			PX4_WARN("IMU_INTEG_RATE updated %" PRId32 " -> %" PRIu32, _param_imu_integ_rate.get(), imu_integration_rate_hz);