ekf2: add dedicated EKF2_CONFIG_TERRAIN in kconfig

- new estimator_aid_src_terrain_range_finder for HAGL RNG
This commit is contained in:
Daniel Agar
2023-08-31 13:21:41 -04:00
parent 845b01a00d
commit 10b54d08fc
14 changed files with 367 additions and 253 deletions
+4 -4
View File
@@ -189,10 +189,10 @@ bool Ekf::update()
// control fusion of observation data
controlFusionModes(imu_sample_delayed);
#if defined(CONFIG_EKF2_RANGE_FINDER)
#if defined(CONFIG_EKF2_TERRAIN)
// run a separate filter for terrain estimation
runTerrainEstimator(imu_sample_delayed);
#endif // CONFIG_EKF2_RANGE_FINDER
#endif // CONFIG_EKF2_TERRAIN
_output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _state.pos, _state.gyro_bias, _state.accel_bias);
@@ -229,10 +229,10 @@ bool Ekf::initialiseFilter()
// initialise the state covariance matrix now we have starting values for all the states
initialiseCovariance();
#if defined(CONFIG_EKF2_RANGE_FINDER)
#if defined(CONFIG_EKF2_TERRAIN)
// Initialise the terrain estimator
initHagl();
#endif // CONFIG_EKF2_RANGE_FINDER
#endif // CONFIG_EKF2_TERRAIN
// reset the output predictor state history to match the EKF initial values
_output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos);