From 6f484b652f1da24ec4d6d92be604a4c00abb16ef Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 21 Nov 2024 16:02:06 -0500 Subject: [PATCH] [WIP] ekf2: EKF init immediately on first IMU sample - tilt init use filtered accel/gyro on delayed time horizon - publish estimator_status on every EKF update (even if !_filter_initialised) --- src/modules/ekf2/EKF/ekf.cpp | 129 +++++------ src/modules/ekf2/EKF/ekf.h | 21 +- src/modules/ekf2/EKF/estimator_interface.cpp | 200 +++++++++--------- src/modules/ekf2/EKF/estimator_interface.h | 12 +- src/modules/ekf2/EKF2.cpp | 8 + .../test/change_indication/ekf_gsf_reset.csv | 4 +- .../ekf2/test/change_indication/iris_gps.csv | 4 +- .../ekf2/test/test_EKF_accelerometer.cpp | 2 +- src/modules/ekf2/test/test_EKF_airspeed.cpp | 2 +- src/modules/ekf2/test/test_EKF_basics.cpp | 2 +- .../ekf2/test/test_EKF_drag_fusion.cpp | 2 +- .../ekf2/test/test_EKF_externalVision.cpp | 2 +- src/modules/ekf2/test/test_EKF_fake_pos.cpp | 2 +- src/modules/ekf2/test/test_EKF_flow.cpp | 2 +- .../ekf2/test/test_EKF_fusionLogic.cpp | 2 +- src/modules/ekf2/test/test_EKF_gnss_yaw.cpp | 2 +- src/modules/ekf2/test/test_EKF_gps.cpp | 2 +- src/modules/ekf2/test/test_EKF_gyroscope.cpp | 2 +- .../ekf2/test/test_EKF_height_fusion.cpp | 2 +- .../ekf2/test/test_EKF_imuSampling.cpp | 3 +- .../ekf2/test/test_EKF_initialization.cpp | 2 +- src/modules/ekf2/test/test_EKF_mag.cpp | 2 +- .../test/test_EKF_measurementSampling.cpp | 2 +- src/modules/ekf2/test/test_EKF_terrain.cpp | 2 +- .../ekf2/test/test_EKF_yaw_estimator.cpp | 2 +- 25 files changed, 196 insertions(+), 219 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index a0df2a2ad710..f90465c6d818 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -43,35 +43,13 @@ #include -bool Ekf::init(uint64_t timestamp) -{ - if (!_initialised) { - _initialised = initialise_interface(timestamp); - reset(); - } - - return _initialised; -} - void Ekf::reset() { ECL_INFO("reset"); + _state = {}; _state.quat_nominal.setIdentity(); - _state.vel.setZero(); - _state.pos.setZero(); - _state.gyro_bias.setZero(); - _state.accel_bias.setZero(); - -#if defined(CONFIG_EKF2_MAGNETOMETER) - _state.mag_I.setZero(); - _state.mag_B.setZero(); -#endif // CONFIG_EKF2_MAGNETOMETER -#if defined(CONFIG_EKF2_WIND) - _state.wind_vel.setZero(); -#endif // CONFIG_EKF2_WIND - // #if defined(CONFIG_EKF2_TERRAIN) // assume a ground clearance _state.terrain = -_gpos.altitude() + _params.rng_gnd_clearance; @@ -101,6 +79,13 @@ void Ekf::reset() _output_predictor.reset(); + _time_latest_us = 0; + _time_delayed_us = 0; + + _imu_updated = false; + + _filter_initialised = false; + // Ekf private fields _time_last_horizontal_aiding = 0; _time_last_v_pos_aiding = 0; @@ -133,18 +118,12 @@ void Ekf::reset() _zero_velocity_update.reset(); updateParameters(); + + initialiseCovariance(); } bool Ekf::update() { - if (!_filter_initialised) { - _filter_initialised = initialiseFilter(); - - if (!_filter_initialised) { - return false; - } - } - // Only run the filter if IMU data in the buffer has been updated if (_imu_updated) { _imu_updated = false; @@ -159,14 +138,52 @@ bool Ekf::update() float filter_update_s = 1e-6f * _params.filter_update_interval_us; _dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * math::constrain(input, 0.5f * filter_update_s, 2.f * filter_update_s); + // Filter accel for tilt initialization + if (_is_first_imu_sample) { + _accel_lpf.reset(imu_sample_delayed.delta_vel / imu_sample_delayed.delta_vel_dt); + _gyro_lpf.reset(imu_sample_delayed.delta_ang / imu_sample_delayed.delta_ang_dt); + _is_first_imu_sample = false; + + _state = {}; + initialiseTilt(_accel_lpf.getState()); + initialiseCovariance(); + + } else { + _accel_lpf.update(imu_sample_delayed.delta_vel / imu_sample_delayed.delta_vel_dt); + _gyro_lpf.update(imu_sample_delayed.delta_ang / imu_sample_delayed.delta_ang_dt); + } + + if (!_filter_initialised) { + + const float accel_norm = _accel_lpf.getState().norm(); + const float gyro_norm = _gyro_lpf.getState().norm(); + + if ((accel_norm > 0.8f * CONSTANTS_ONE_G) + && (accel_norm < 1.2f * CONSTANTS_ONE_G) + && (gyro_norm < math::radians(15.f)) + ) { + // once ready reset and init tilt from filtered accel + _state = {}; + initialiseTilt(_accel_lpf.getState()); + initialiseCovariance(); + + // reset the output predictor state history to match the EKF initial values + _output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _gpos); + + _filter_initialised = true; + } + } + updateIMUBiasInhibit(imu_sample_delayed); // perform state and covariance prediction for the main filter predictCovariance(imu_sample_delayed); predictState(imu_sample_delayed); - // control fusion of observation data - controlFusionModes(imu_sample_delayed); + if (_filter_initialised) { + // control fusion of observation data + controlFusionModes(imu_sample_delayed); + } _output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _gpos, _state.gyro_bias, _state.accel_bias); @@ -177,52 +194,10 @@ bool Ekf::update() return false; } -bool Ekf::initialiseFilter() +bool Ekf::initialiseTilt(const Vector3f &accel) { - // Filter accel for tilt initialization - const imuSample &imu_init = _imu_buffer.get_newest(); - - // protect against zero data - if (imu_init.delta_vel_dt < 1e-4f || imu_init.delta_ang_dt < 1e-4f) { - return false; - } - - if (_is_first_imu_sample) { - _accel_lpf.reset(imu_init.delta_vel / imu_init.delta_vel_dt); - _gyro_lpf.reset(imu_init.delta_ang / imu_init.delta_ang_dt); - _is_first_imu_sample = false; - - } else { - _accel_lpf.update(imu_init.delta_vel / imu_init.delta_vel_dt); - _gyro_lpf.update(imu_init.delta_ang / imu_init.delta_ang_dt); - } - - if (!initialiseTilt()) { - return false; - } - - // initialise the state covariance matrix now we have starting values for all the states - initialiseCovariance(); - - // reset the output predictor state history to match the EKF initial values - _output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _gpos); - - return true; -} - -bool Ekf::initialiseTilt() -{ - const float accel_norm = _accel_lpf.getState().norm(); - const float gyro_norm = _gyro_lpf.getState().norm(); - - if (accel_norm < 0.8f * CONSTANTS_ONE_G || - accel_norm > 1.2f * CONSTANTS_ONE_G || - gyro_norm > math::radians(15.0f)) { - return false; - } - // get initial tilt estimate from delta velocity vector, assuming vehicle is static - _state.quat_nominal = Quatf(_accel_lpf.getState(), Vector3f(0.f, 0.f, -1.f)); + _state.quat_nominal = Quatf(accel, Vector3f(0.f, 0.f, -1.f)); _R_to_earth = Dcmf(_state.quat_nominal); return true; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index f42493652ecc..9d64fabc5758 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -80,15 +80,15 @@ class Ekf final : public EstimatorInterface Ekf() { reset(); + initialise_interface(); }; - virtual ~Ekf() = default; - - // initialise variables to sane values (also interface class) - bool init(uint64_t timestamp) override; + ~Ekf() = default; void print_status(); + void reset(); + // should be called every time new data is pushed into the filter bool update(); @@ -427,10 +427,7 @@ class Ekf final : public EstimatorInterface friend class EvVelLocalFrameNed; friend class EvVelLocalFrameFrd; - // set the internal states and status to their default value - void reset(); - - bool initialiseTilt(); + bool initialiseTilt(const Vector3f &accel); // check if the EKF is dead reckoning horizontal velocity using inertial data only void updateDeadReckoningStatus(); @@ -583,11 +580,6 @@ class Ekf final : public EstimatorInterface estimator_aid_source2d_s _aid_src_aux_vel {}; #endif // CONFIG_EKF2_AUXVEL - // Variables used by the initial filter alignment - bool _is_first_imu_sample{true}; - AlphaFilter _accel_lpf{0.1f}; ///< filtered accelerometer measurement used to align tilt (m/s/s) - AlphaFilter _gyro_lpf{0.1f}; ///< filtered gyro measurement used for alignment excessive movement check (rad/sec) - #if defined(CONFIG_EKF2_BAROMETER) estimator_aid_source1d_s _aid_src_baro_hgt {}; @@ -626,9 +618,6 @@ class Ekf final : public EstimatorInterface uint64_t _time_good_vert_accel{0}; ///< last time a good vertical accel was detected (uSec) uint16_t _clip_counter[3]; ///< counter per axis that increments when clipping ad decrements when not - // initialise filter states of both the delayed ekf and the real time complementary filter - bool initialiseFilter(void); - // initialise ekf covariance matrix void initialiseCovariance(); diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 84ced94c8004..c0f30aeeadb3 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -78,11 +78,6 @@ EstimatorInterface::~EstimatorInterface() // Accumulate imu data and store to buffer at desired rate void EstimatorInterface::setIMUData(const imuSample &imu_sample) { - // TODO: resolve misplaced responsibility - if (!_initialised) { - _initialised = init(imu_sample.time_us); - } - _time_latest_us = imu_sample.time_us; // the output observer always runs @@ -122,18 +117,19 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample) #if defined(CONFIG_EKF2_MAGNETOMETER) void EstimatorInterface::setMagData(const magSample &mag_sample) { - if (!_initialised) { - return; - } - // Allocate the required buffer size if not previously done if (_mag_buffer == nullptr) { - _mag_buffer = new RingBuffer(_obs_buffer_length); + if (_obs_buffer_length > 0) { + _mag_buffer = new RingBuffer(_obs_buffer_length); + + if (_mag_buffer == nullptr || !_mag_buffer->valid()) { + delete _mag_buffer; + _mag_buffer = nullptr; + printBufferAllocationFailed("mag"); + return; + } - if (_mag_buffer == nullptr || !_mag_buffer->valid()) { - delete _mag_buffer; - _mag_buffer = nullptr; - printBufferAllocationFailed("mag"); + } else { return; } } @@ -161,18 +157,19 @@ void EstimatorInterface::setMagData(const magSample &mag_sample) #if defined(CONFIG_EKF2_GNSS) void EstimatorInterface::setGpsData(const gnssSample &gnss_sample) { - if (!_initialised) { - return; - } - // Allocate the required buffer size if not previously done if (_gps_buffer == nullptr) { - _gps_buffer = new RingBuffer(_obs_buffer_length); + if (_obs_buffer_length > 0) { + _gps_buffer = new RingBuffer(_obs_buffer_length); + + if (_gps_buffer == nullptr || !_gps_buffer->valid()) { + delete _gps_buffer; + _gps_buffer = nullptr; + printBufferAllocationFailed("GPS"); + return; + } - if (_gps_buffer == nullptr || !_gps_buffer->valid()) { - delete _gps_buffer; - _gps_buffer = nullptr; - printBufferAllocationFailed("GPS"); + } else { return; } } @@ -208,18 +205,19 @@ void EstimatorInterface::setGpsData(const gnssSample &gnss_sample) #if defined(CONFIG_EKF2_BAROMETER) void EstimatorInterface::setBaroData(const baroSample &baro_sample) { - if (!_initialised) { - return; - } - // Allocate the required buffer size if not previously done if (_baro_buffer == nullptr) { - _baro_buffer = new RingBuffer(_obs_buffer_length); + if (_obs_buffer_length > 0) { + _baro_buffer = new RingBuffer(_obs_buffer_length); + + if (_baro_buffer == nullptr || !_baro_buffer->valid()) { + delete _baro_buffer; + _baro_buffer = nullptr; + printBufferAllocationFailed("baro"); + return; + } - if (_baro_buffer == nullptr || !_baro_buffer->valid()) { - delete _baro_buffer; - _baro_buffer = nullptr; - printBufferAllocationFailed("baro"); + } else { return; } } @@ -247,18 +245,19 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample) #if defined(CONFIG_EKF2_AIRSPEED) void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample) { - if (!_initialised) { - return; - } - // Allocate the required buffer size if not previously done if (_airspeed_buffer == nullptr) { - _airspeed_buffer = new RingBuffer(_obs_buffer_length); + if (_obs_buffer_length > 0) { + _airspeed_buffer = new RingBuffer(_obs_buffer_length); - if (_airspeed_buffer == nullptr || !_airspeed_buffer->valid()) { - delete _airspeed_buffer; - _airspeed_buffer = nullptr; - printBufferAllocationFailed("airspeed"); + if (_airspeed_buffer == nullptr || !_airspeed_buffer->valid()) { + delete _airspeed_buffer; + _airspeed_buffer = nullptr; + printBufferAllocationFailed("airspeed"); + return; + } + + } else { return; } } @@ -285,18 +284,19 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample) #if defined(CONFIG_EKF2_RANGE_FINDER) void EstimatorInterface::setRangeData(const sensor::rangeSample &range_sample) { - if (!_initialised) { - return; - } - // Allocate the required buffer size if not previously done if (_range_buffer == nullptr) { - _range_buffer = new RingBuffer(_obs_buffer_length); + if (_obs_buffer_length > 0) { + _range_buffer = new RingBuffer(_obs_buffer_length); + + if (_range_buffer == nullptr || !_range_buffer->valid()) { + delete _range_buffer; + _range_buffer = nullptr; + printBufferAllocationFailed("range"); + return; + } - if (_range_buffer == nullptr || !_range_buffer->valid()) { - delete _range_buffer; - _range_buffer = nullptr; - printBufferAllocationFailed("range"); + } else { return; } } @@ -324,18 +324,19 @@ void EstimatorInterface::setRangeData(const sensor::rangeSample &range_sample) #if defined(CONFIG_EKF2_OPTICAL_FLOW) void EstimatorInterface::setOpticalFlowData(const flowSample &flow) { - if (!_initialised) { - return; - } - // Allocate the required buffer size if not previously done if (_flow_buffer == nullptr) { - _flow_buffer = new RingBuffer(_imu_buffer_length); + if (_imu_buffer_length > 0) { + _flow_buffer = new RingBuffer(_imu_buffer_length); + + if (_flow_buffer == nullptr || !_flow_buffer->valid()) { + delete _flow_buffer; + _flow_buffer = nullptr; + printBufferAllocationFailed("flow"); + return; + } - if (_flow_buffer == nullptr || !_flow_buffer->valid()) { - delete _flow_buffer; - _flow_buffer = nullptr; - printBufferAllocationFailed("flow"); + } else { return; } } @@ -362,18 +363,19 @@ void EstimatorInterface::setOpticalFlowData(const flowSample &flow) #if defined(CONFIG_EKF2_EXTERNAL_VISION) void EstimatorInterface::setExtVisionData(const extVisionSample &evdata) { - if (!_initialised) { - return; - } - // Allocate the required buffer size if not previously done if (_ext_vision_buffer == nullptr) { - _ext_vision_buffer = new RingBuffer(_obs_buffer_length); + if (_obs_buffer_length > 0) { + _ext_vision_buffer = new RingBuffer(_obs_buffer_length); + + if (_ext_vision_buffer == nullptr || !_ext_vision_buffer->valid()) { + delete _ext_vision_buffer; + _ext_vision_buffer = nullptr; + printBufferAllocationFailed("vision"); + return; + } - if (_ext_vision_buffer == nullptr || !_ext_vision_buffer->valid()) { - delete _ext_vision_buffer; - _ext_vision_buffer = nullptr; - printBufferAllocationFailed("vision"); + } else { return; } } @@ -402,18 +404,19 @@ void EstimatorInterface::setExtVisionData(const extVisionSample &evdata) #if defined(CONFIG_EKF2_AUXVEL) void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample) { - if (!_initialised) { - return; - } - // Allocate the required buffer size if not previously done if (_auxvel_buffer == nullptr) { - _auxvel_buffer = new RingBuffer(_obs_buffer_length); + if (_obs_buffer_length > 0) { + _auxvel_buffer = new RingBuffer(_obs_buffer_length); + + if (_auxvel_buffer == nullptr || !_auxvel_buffer->valid()) { + delete _auxvel_buffer; + _auxvel_buffer = nullptr; + printBufferAllocationFailed("aux vel"); + return; + } - if (_auxvel_buffer == nullptr || !_auxvel_buffer->valid()) { - delete _auxvel_buffer; - _auxvel_buffer = nullptr; - printBufferAllocationFailed("aux vel"); + } else { return; } } @@ -439,18 +442,19 @@ void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample) void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags) { - if (!_initialised) { - return; - } - // Allocate the required buffer size if not previously done if (_system_flag_buffer == nullptr) { - _system_flag_buffer = new RingBuffer(_obs_buffer_length); + if (_obs_buffer_length > 0) { + _system_flag_buffer = new RingBuffer(_obs_buffer_length); - if (_system_flag_buffer == nullptr || !_system_flag_buffer->valid()) { - delete _system_flag_buffer; - _system_flag_buffer = nullptr; - printBufferAllocationFailed("system flag"); + if (_system_flag_buffer == nullptr || !_system_flag_buffer->valid()) { + delete _system_flag_buffer; + _system_flag_buffer = nullptr; + printBufferAllocationFailed("system flag"); + return; + } + + } else { return; } } @@ -477,16 +481,21 @@ void EstimatorInterface::setDragData(const imuSample &imu) { // down-sample the drag specific force data by accumulating and calculating the mean when // sufficient samples have been collected - if (_params.drag_ctrl > 0) { + if (_params.drag_ctrl) { // Allocate the required buffer size if not previously done if (_drag_buffer == nullptr) { - _drag_buffer = new RingBuffer(_obs_buffer_length); + if (_obs_buffer_length > 0) { + _drag_buffer = new RingBuffer(_obs_buffer_length); - if (_drag_buffer == nullptr || !_drag_buffer->valid()) { - delete _drag_buffer; - _drag_buffer = nullptr; - printBufferAllocationFailed("drag"); + if (_drag_buffer == nullptr || !_drag_buffer->valid()) { + delete _drag_buffer; + _drag_buffer = nullptr; + printBufferAllocationFailed("drag"); + return; + } + + } else { return; } } @@ -536,7 +545,7 @@ void EstimatorInterface::setDragData(const imuSample &imu) } #endif // CONFIG_EKF2_DRAG_FUSION -bool EstimatorInterface::initialise_interface(uint64_t timestamp) +bool EstimatorInterface::initialise_interface() { const float filter_update_period_ms = _params.filter_update_interval_us / 1000.f; @@ -560,11 +569,6 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) return false; } - _time_delayed_us = timestamp; - _time_latest_us = timestamp; - - _fault_status.value = 0; - return true; } diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 90c26d608392..68aaa0ae29e4 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -318,9 +318,7 @@ class EstimatorInterface protected: EstimatorInterface() = default; - virtual ~EstimatorInterface(); - - virtual bool init(uint64_t timestamp) = 0; + ~EstimatorInterface(); parameters _params{}; // filter parameters @@ -346,6 +344,11 @@ class EstimatorInterface uint64_t _time_delayed_us{0}; // captures the imu sample on the delayed time horizon uint64_t _time_latest_us{0}; // imu sample capturing the newest imu data + // Variables used by the initial filter alignment + bool _is_first_imu_sample{true}; + AlphaFilter _accel_lpf{0.1f}; ///< filtered accelerometer measurement used to align tilt (m/s/s) + AlphaFilter _gyro_lpf{0.1f}; ///< filtered gyro measurement used for alignment excessive movement check (rad/sec) + OutputPredictor _output_predictor{}; #if defined(CONFIG_EKF2_AIRSPEED) @@ -378,7 +381,6 @@ class EstimatorInterface float _air_density{atmosphere::kAirDensitySeaLevelStandardAtmos}; // air density (kg/m**3) bool _imu_updated{false}; // true if the ekf should update (completed downsampling process) - bool _initialised{false}; // true if the ekf interface instance (data buffering) is initialized // Variables used to publish the WGS-84 location of the EKF local NED origin MapProjection _local_origin_lat_lon{}; @@ -449,7 +451,7 @@ class EstimatorInterface fault_status_u _fault_status{}; // allocate data buffers and initialize interface variables - bool initialise_interface(uint64_t timestamp); + bool initialise_interface(); #if defined(CONFIG_EKF2_MAGNETOMETER) uint64_t _wmm_mag_time_last_checked {0}; // time WMM update last checked by mag control diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 93c9ed617a0f..94a46f49aaf9 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -833,6 +833,14 @@ void EKF2::Run() #if defined(CONFIG_EKF2_MAGNETOMETER) UpdateMagCalibration(now); #endif // CONFIG_EKF2_MAGNETOMETER + + } else { + // if filter not initialized + + + + + } // publish ekf2_timestamps diff --git a/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv b/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv index fbc6d2b054a5..0aa7d8722edb 100644 --- a/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv +++ b/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv @@ -1,6 +1,6 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23] -10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,0,0,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,8.1e-05,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.032 -90000,1,-0.0094,-0.011,6.9e-05,-0.00047,0.0026,-0.026,0,0,-0.0023,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,0.00036,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.058 +10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,0,0,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,8.1e-05,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.032 +90000,1,-0.0094,-0.011,6.9e-05,-0.00047,0.0026,-0.026,0,0,-0.0023,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,0.00036,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.058 190000,1,-0.0094,-0.011,2.8e-05,6.9e-05,0.004,-0.041,0,0,-1.2e+02,-3e-11,-2.6e-12,5.6e-13,0,0,-5e-08,0,0,0,0,0,0,0,0,-1.2e+02,0.011,0.011,0.00084,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.082 290000,1,-0.0094,-0.011,6.3e-05,0.001,0.0064,-0.053,0,0,-1.2e+02,9.1e-10,1e-10,-1.7e-11,0,0,-2.5e-06,0,0,0,0,0,0,0,0,-1.2e+02,0.012,0.012,0.00075,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.11 390000,1,-0.0094,-0.011,7e-05,0.0024,0.0083,-0.059,0,0,-1.2e+02,-1.1e-08,2.8e-09,2.9e-10,0,0,-1.5e-05,0,0,0,0,0,0,0,0,-1.2e+02,0.012,0.012,0.0012,25,25,8.1,0.97,0.97,0.32,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.13 diff --git a/src/modules/ekf2/test/change_indication/iris_gps.csv b/src/modules/ekf2/test/change_indication/iris_gps.csv index f83bc0ad462c..746828a9d1ec 100644 --- a/src/modules/ekf2/test/change_indication/iris_gps.csv +++ b/src/modules/ekf2/test/change_indication/iris_gps.csv @@ -1,6 +1,6 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23] -10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,0,0,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,8.1e-05,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.032 -90000,1,-0.011,-0.01,0.00033,-0.001,-0.0031,-0.024,0,0,-0.0021,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,0.00036,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.058 +10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,0,0,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,8.1e-05,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.032 +90000,1,-0.011,-0.01,0.00033,-0.001,-0.0031,-0.024,0,0,-0.0021,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,0.00036,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.058 190000,1,-0.012,-0.011,0.00044,-0.0023,-0.003,-0.035,0,0,-4.9e+02,1.6e-09,-1.4e-09,-6.4e-11,0,0,-3.2e-06,0,0,0,0,0,0,0,0,-4.9e+02,0.011,0.011,0.00084,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.082 290000,1,-0.012,-0.011,0.00044,-0.0033,-0.0044,-0.038,0,0,-4.9e+02,7.3e-09,-1.1e-08,-3.9e-10,0,0,-2e-05,0,0,0,0,0,0,0,0,-4.9e+02,0.012,0.012,0.00075,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.11 390000,1,-0.012,-0.011,0.00049,-0.0025,-0.0059,-0.05,0,0,-4.9e+02,-2.1e-10,-1.4e-08,-3.2e-10,0,0,-1.4e-05,0,0,0,0,0,0,0,0,-4.9e+02,0.012,0.012,0.0012,25,25,8.1,0.97,0.97,0.32,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.13 diff --git a/src/modules/ekf2/test/test_EKF_accelerometer.cpp b/src/modules/ekf2/test/test_EKF_accelerometer.cpp index 255f6597b3f5..15719931729b 100644 --- a/src/modules/ekf2/test/test_EKF_accelerometer.cpp +++ b/src/modules/ekf2/test/test_EKF_accelerometer.cpp @@ -59,7 +59,7 @@ class EkfAccelerometerTest : public ::testing::Test void SetUp() override { // run briefly to init, then manually set in air and at rest (default for a real vehicle) - _ekf->init(0); + _ekf->reset(); _sensor_simulator.runSeconds(0.1); _ekf->set_in_air_status(false); _ekf->set_vehicle_at_rest(true); diff --git a/src/modules/ekf2/test/test_EKF_airspeed.cpp b/src/modules/ekf2/test/test_EKF_airspeed.cpp index d905fb85703c..30f27110d62a 100644 --- a/src/modules/ekf2/test/test_EKF_airspeed.cpp +++ b/src/modules/ekf2/test/test_EKF_airspeed.cpp @@ -62,7 +62,7 @@ class EkfAirspeedTest : public ::testing::Test void SetUp() override { // run briefly to init, then manually set in air and at rest (default for a real vehicle) - _ekf->init(0); + _ekf->reset(); _sensor_simulator.runSeconds(0.1); _ekf->set_in_air_status(false); _ekf->set_vehicle_at_rest(true); diff --git a/src/modules/ekf2/test/test_EKF_basics.cpp b/src/modules/ekf2/test/test_EKF_basics.cpp index d67cac0e528a..5401f08b2de6 100644 --- a/src/modules/ekf2/test/test_EKF_basics.cpp +++ b/src/modules/ekf2/test/test_EKF_basics.cpp @@ -54,7 +54,7 @@ class EkfBasicsTest : public ::testing::Test void SetUp() override { // run briefly to init, then manually set in air and at rest (default for a real vehicle) - _ekf->init(0); + _ekf->reset(); _sensor_simulator.runSeconds(0.1); _ekf->set_in_air_status(false); _ekf->set_vehicle_at_rest(true); diff --git a/src/modules/ekf2/test/test_EKF_drag_fusion.cpp b/src/modules/ekf2/test/test_EKF_drag_fusion.cpp index edd81e640c44..b0bfeb8aba5c 100644 --- a/src/modules/ekf2/test/test_EKF_drag_fusion.cpp +++ b/src/modules/ekf2/test/test_EKF_drag_fusion.cpp @@ -59,7 +59,7 @@ class EkfDragFusionTest : public ::testing::Test void SetUp() override { // run briefly to init, then manually set in air and at rest (default for a real vehicle) - _ekf->init(0); + _ekf->reset(); _ekf->set_is_fixed_wing(false); _ekf->set_in_air_status(false); _ekf->set_vehicle_at_rest(true); diff --git a/src/modules/ekf2/test/test_EKF_externalVision.cpp b/src/modules/ekf2/test/test_EKF_externalVision.cpp index f87151eeafaf..297a0decca30 100644 --- a/src/modules/ekf2/test/test_EKF_externalVision.cpp +++ b/src/modules/ekf2/test/test_EKF_externalVision.cpp @@ -62,7 +62,7 @@ class EkfExternalVisionTest : public ::testing::Test void SetUp() override { // Init, then manually set in air and at rest (default for a real vehicle) - _ekf->init(0); + _ekf->reset(); _ekf->set_in_air_status(false); _ekf->set_vehicle_at_rest(true); } diff --git a/src/modules/ekf2/test/test_EKF_fake_pos.cpp b/src/modules/ekf2/test/test_EKF_fake_pos.cpp index fb3b5991e1f9..e1687eac86ec 100644 --- a/src/modules/ekf2/test/test_EKF_fake_pos.cpp +++ b/src/modules/ekf2/test/test_EKF_fake_pos.cpp @@ -56,7 +56,7 @@ class EkfFakePosTest : public ::testing::Test void SetUp() override { // run briefly to init, then manually set in air and at rest (default for a real vehicle) - _ekf->init(0); + _ekf->reset(); _sensor_simulator.runSeconds(0.1); _ekf->set_in_air_status(false); _ekf->set_vehicle_at_rest(true); diff --git a/src/modules/ekf2/test/test_EKF_flow.cpp b/src/modules/ekf2/test/test_EKF_flow.cpp index bc33c350e792..ff6c027bf9db 100644 --- a/src/modules/ekf2/test/test_EKF_flow.cpp +++ b/src/modules/ekf2/test/test_EKF_flow.cpp @@ -65,7 +65,7 @@ class EkfFlowTest : public ::testing::Test _ekf->set_optical_flow_limits(max_flow_rate, min_ground_distance, max_ground_distance); // run briefly to init, then manually set in air and at rest (default for a real vehicle) - _ekf->init(0); + _ekf->reset(); _sensor_simulator.runSeconds(0.1); _ekf->set_in_air_status(false); _ekf->set_vehicle_at_rest(true); diff --git a/src/modules/ekf2/test/test_EKF_fusionLogic.cpp b/src/modules/ekf2/test/test_EKF_fusionLogic.cpp index 2b89c8aa8f71..4b892394cf44 100644 --- a/src/modules/ekf2/test/test_EKF_fusionLogic.cpp +++ b/src/modules/ekf2/test/test_EKF_fusionLogic.cpp @@ -60,7 +60,7 @@ class EkfFusionLogicTest : public ::testing::Test void SetUp() override { // run briefly to init, then manually set in air and at rest (default for a real vehicle) - _ekf->init(0); + _ekf->reset(); _sensor_simulator.runSeconds(0.1); _ekf->set_in_air_status(false); _ekf->set_vehicle_at_rest(true); diff --git a/src/modules/ekf2/test/test_EKF_gnss_yaw.cpp b/src/modules/ekf2/test/test_EKF_gnss_yaw.cpp index 93dd222375f9..a5ff0e6bd7de 100644 --- a/src/modules/ekf2/test/test_EKF_gnss_yaw.cpp +++ b/src/modules/ekf2/test/test_EKF_gnss_yaw.cpp @@ -62,7 +62,7 @@ class EkfGpsHeadingTest : public ::testing::Test void SetUp() override { // Init, then manually set in air and at rest (default for a real vehicle) - _ekf->init(0); + _ekf->reset(); _ekf->set_in_air_status(false); _ekf->set_vehicle_at_rest(true); diff --git a/src/modules/ekf2/test/test_EKF_gps.cpp b/src/modules/ekf2/test/test_EKF_gps.cpp index f6aae937df0b..400ee70a35c7 100644 --- a/src/modules/ekf2/test/test_EKF_gps.cpp +++ b/src/modules/ekf2/test/test_EKF_gps.cpp @@ -59,7 +59,7 @@ class EkfGpsTest : public ::testing::Test void SetUp() override { // run briefly to init, then manually set in air and at rest (default for a real vehicle) - _ekf->init(0); + _ekf->reset(); _sensor_simulator.runSeconds(0.1); _ekf->set_in_air_status(false); _ekf->set_vehicle_at_rest(true); diff --git a/src/modules/ekf2/test/test_EKF_gyroscope.cpp b/src/modules/ekf2/test/test_EKF_gyroscope.cpp index 8d3daac16957..af47f638d34b 100644 --- a/src/modules/ekf2/test/test_EKF_gyroscope.cpp +++ b/src/modules/ekf2/test/test_EKF_gyroscope.cpp @@ -59,7 +59,7 @@ class EkfGyroscopeTest : public ::testing::Test void SetUp() override { // Init, then manually set in air and at rest (default for a real vehicle) - _ekf->init(0); + _ekf->reset(); _ekf->set_in_air_status(false); _ekf->set_vehicle_at_rest(true); } diff --git a/src/modules/ekf2/test/test_EKF_height_fusion.cpp b/src/modules/ekf2/test/test_EKF_height_fusion.cpp index 17c95cad453f..207ade1c2105 100644 --- a/src/modules/ekf2/test/test_EKF_height_fusion.cpp +++ b/src/modules/ekf2/test/test_EKF_height_fusion.cpp @@ -58,7 +58,7 @@ class EkfHeightFusionTest : public ::testing::Test // Setup the Ekf with synthetic measurements void SetUp() override { - _ekf->init(0); + _ekf->reset(); _ekf_wrapper.disableBaroHeightFusion(); _ekf_wrapper.disableRangeHeightFusion(); _sensor_simulator.runSeconds(0.1); diff --git a/src/modules/ekf2/test/test_EKF_imuSampling.cpp b/src/modules/ekf2/test/test_EKF_imuSampling.cpp index 5ba380d2c079..9f48e1fcb1bd 100644 --- a/src/modules/ekf2/test/test_EKF_imuSampling.cpp +++ b/src/modules/ekf2/test/test_EKF_imuSampling.cpp @@ -47,8 +47,7 @@ class EkfImuSamplingTest : public ::testing::TestWithParaminit(0); + _ekf->reset(); _ekf->set_in_air_status(false); _ekf->set_vehicle_at_rest(true); } diff --git a/src/modules/ekf2/test/test_EKF_mag.cpp b/src/modules/ekf2/test/test_EKF_mag.cpp index ca4f440c624d..d563846c803a 100644 --- a/src/modules/ekf2/test/test_EKF_mag.cpp +++ b/src/modules/ekf2/test/test_EKF_mag.cpp @@ -58,7 +58,7 @@ class EkfMagTest : public ::testing::Test void SetUp() override { // Init, then manually set in air and at rest (default for a real vehicle) - _ekf->init(0); + _ekf->reset(); _ekf->set_in_air_status(false); _ekf->set_vehicle_at_rest(true); } diff --git a/src/modules/ekf2/test/test_EKF_measurementSampling.cpp b/src/modules/ekf2/test/test_EKF_measurementSampling.cpp index 367ae415344f..f0db67b509e1 100644 --- a/src/modules/ekf2/test/test_EKF_measurementSampling.cpp +++ b/src/modules/ekf2/test/test_EKF_measurementSampling.cpp @@ -45,7 +45,7 @@ class EkfMeasurementSamplingTest : public ::testing::Test void SetUp() override { - _ekf->init(0); + _ekf->reset(); } }; diff --git a/src/modules/ekf2/test/test_EKF_terrain.cpp b/src/modules/ekf2/test/test_EKF_terrain.cpp index 7fbf7fcd3b64..b92f82feebdc 100644 --- a/src/modules/ekf2/test/test_EKF_terrain.cpp +++ b/src/modules/ekf2/test/test_EKF_terrain.cpp @@ -58,7 +58,7 @@ class EkfTerrainTest : public ::testing::Test void SetUp() override { // run briefly to init, then manually set in air and at rest (default for a real vehicle) - _ekf->init(0); + _ekf->reset(); _sensor_simulator.runSeconds(0.1); _ekf->set_in_air_status(false); _ekf->set_vehicle_at_rest(true); diff --git a/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp b/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp index 1d1fdfb6ce8d..27556fddad49 100644 --- a/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp +++ b/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp @@ -58,7 +58,7 @@ class EKFYawEstimatorTest : public ::testing::Test void SetUp() override { // run briefly to init, then manually set in air and at rest (default for a real vehicle) - _ekf->init(0); + _ekf->reset(); _sensor_simulator.runSeconds(0.1); _ekf->set_in_air_status(false); _ekf->set_vehicle_at_rest(true);