EKF: Move to 1000 Hz sampling / 250 Hz prediction / 80 Hz updates @ 27% CPU load

src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h

@@ -133,6 +133,8 @@ public:
      */
     int set_debuglevel(unsigned debug) { _debug = debug; return 0; }
 
+    static constexpr unsigned MAX_PREDICITION_STEPS = 3; /**< maximum number of prediction steps between updates */
+
 private:
     bool        _task_should_exit;      /**< if true, sensor task should exit */
     bool        _task_running;          /**< if true, task is running in its mainloop */
@@ -174,6 +176,7 @@ private:
 
     hrt_abstime _last_accel;
     hrt_abstime _last_mag;
+    unsigned _prediction_steps;
 
     struct sensor_combined_s            _sensor_combined;
 

src/modules/ekf_att_pos_estimator/CMakeLists.txt

@@ -42,7 +42,6 @@ px4_add_module(
 	COMPILE_FLAGS ${MODULE_CFLAGS}
 	SRCS
 		ekf_att_pos_estimator_main.cpp
-		ekf_att_pos_estimator_params.c
 		estimator_22states.cpp
 		estimator_utilities.cpp
 	DEPENDS

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -156,6 +156,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 
 	_last_accel(0),
 	_last_mag(0),
+	_prediction_steps(0),
 
 	_sensor_combined{},
 
@@ -995,6 +996,14 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
 	_ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU;
 	_covariancePredictionDt += _ekf->dtIMU;
 
+	// only fuse every few steps
+	if (_prediction_steps < MAX_PREDICITION_STEPS) {
+		_prediction_steps++;
+		return;
+	} else {
+		_prediction_steps = 0;
+	}
+
 	// perform a covariance prediction if the total delta angle has exceeded the limit
 	// or the time limit will be exceeded at the next IMU update
 	if ((_covariancePredictionDt >= (_ekf->covTimeStepMax - _ekf->dtIMU))
@@ -1111,7 +1120,7 @@ int AttitudePositionEstimatorEKF::start()
 	_estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 20,
-					 4800,
+					 4200,
 					 (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
 					 nullptr);
 

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c

@@ -36,13 +36,9 @@
  *
  * Parameters defined by the attitude and position estimator task
  *
- * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Lorenz Meier <lorenz@px4.io>
  */
 
-#include <px4_config.h>
-
-#include <systemlib/param/param.h>
-
 /*
  * Estimator parameters, accessible via MAVLink
  *