EKF: Improve covariance prediction stability (#795)

* EKF: Improve covariance prediction stability

Eliminates collapse of vertical velocity state variance due to rounding errors that can occur under some operating conditions.

* EKF: Fix typo

* test: Fix initialisation test cases

Provide sufficient time for variances to stabilise and fix calculation of reference quaternion for alignment.

* test: Allow for accumulated rounding error in IMU sampling test

* test: Allow sufficient time for quaternion variances to reduce after initial alignment

* test: Increase allowance for tilt alignment delay and inertial nav errors

* test: Increase allowance for tilt alignment delay and inertial nav errors

* adpat reset velocity test

* test: update change indication file

* test: Adjust tests to handle alignment time and prediction errors

* README.md: Add documentation for change indicator test

EKF/covariance.cpp

@@ -135,7 +135,8 @@ void Ekf::predictCovariance()
 	const float dvy_b = _state.delta_vel_bias(1);
 	const float dvz_b = _state.delta_vel_bias(2);
 
-	const float dt = math::constrain(_imu_sample_delayed.delta_ang_dt, 0.5f * FILTER_UPDATE_PERIOD_S, 2.0f * FILTER_UPDATE_PERIOD_S);
+	// 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_inv = 1.0f / 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
@@ -734,12 +735,12 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 
 	for (int i = 4; i <= 6; i++) {
 		// NED velocity states
-		P(i,i) = math::constrain(P(i,i), 0.0f, P_lim[1]);
+		P(i,i) = math::constrain(P(i,i), 1E-6f, P_lim[1]);
 	}
 
 	for (int i = 7; i <= 9; i++) {
 		// NED position states
-		P(i,i) = math::constrain(P(i,i), 0.0f, P_lim[2]);
+		P(i,i) = math::constrain(P(i,i), 1E-6f, P_lim[2]);
 	}
 
 	for (int i = 10; i <= 12; i++) {

EKF/estimator_interface.h

@@ -391,7 +391,7 @@ public:
 
 	void print_status();
 
-	static constexpr unsigned FILTER_UPDATE_PERIOD_MS{8};	// ekf prediction period in milliseconds - this should ideally be an integer multiple of the IMU time delta
+	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};
 
 	// request the EKF reset the yaw to the estimate from the internal EKF-GSF filter

README.md

@@ -36,3 +36,26 @@ By following the steps you can run the unit tests
 ```
 make test
 ```
+### Change Indicator / Unit Tests
+Change indication is the concept of running the EKF on different data-sets and compare the state of the EKF to a previous version. If a contributor makes a functional change that is run during the change_indication tests, this will produce a different output of the EKF's state. As the tests are run in CI, this checks if a contributor forgot to run the checks themselves and add the [new EKF's state outputs](https://github.com/PX4/ecl/blob/master/test/change_indication/iris_gps.csv) to the pull request.
+
+The unit tests include a check to see if the pull request results in a difference to the [output data csv file](https://github.com/PX4/ecl/blob/master/test/change_indication/iris_gps.csv) when replaying the [sensor data csv file](https://github.com/PX4/ecl/blob/master/test/replay_data/iris_gps.csv). If a pull request results in an expected difference, then it is important that the output reference file be re-generated and included as part of the pull request. A non-functional pull request should not result in changes to this file, however the default test case does not exercise all sensor types so this test passing is a necessary, but not sufficient requirement for a non-functional pull request.
+
+The functionality that supports this test consists of:
+* Python scripts that extract sensor data from ulog files and writes them to a sensor data csv file. The default [sensor data csv file](https://github.com/PX4/ecl/blob/master/test/replay_data/iris_gps.csv) used by the unit test was generated from a ulog created from an iris SITL flight.
+* A [script file](https://github.com/PX4/ecl/blob/master/test/test_EKF_withReplayData.cpp) using functionality provided by  the [sensor simulator](https://github.com/PX4/ecl/blob/master/test/sensor_simulator/sensor_simulator.cpp), that loads sensor data from the [sensor data csv file](https://github.com/PX4/ecl/blob/master/test/replay_data/iris_gps.csv) , replays the EKF with it and logs the EKF's state and covariance data to the [output data csv file](https://github.com/PX4/ecl/blob/master/test/replay_data/iris_gps.csv).
+* CI action that checks if the logs of the test running with replay data is changing. This helps to see if there are functional changes.
+
+#### How to run the Change Indicator test during development on your own logs:
+
+* create sensor_data.csv file from ulog file 'cd test/sensor_simulator/
+python3 createSensorDataFile.py <path/to/ulog> ../replay_data/<descriptive_name>.csv'
+* Setup the test file to use the EKF with the created sensor data by copy&paste an existing test case in [test/test_EKF_withReplayData.cpp](https://github.com/PX4/ecl/blob/master/test/test_EKF_withReplayData.cpp) and adapt the paths to load the right sensor data and write it to the right place, eg
+_sensor_simulator.loadSensorDataFromFile("../../../test/replay_data/<descriptive_name>.csv");
+_ekf_logger.setFilePath("../../../test/change_indication/<descriptive_name>.csv");
+* You can feed the EKF with the data in the csv file, by running '_sensor_simulator.runReplaySeconds(duration_in_seconds)'. Be aware that replay sensor data will only be available when the corresponding sensor simulation are running. By default only imu, baro and mag sensor simulators are running. You can start a sensor simulation by calling _sensor_simulator._<sensor>.start(). Be also aware that you still have to setup the EKF yourself. This includes setting the bit mask (fusion_mode in common.h) according to what you intend to fuse.
+* In between _sensor_simulator.runReplaySeconds(duration_in_seconds) calls, write the state and covariances to the change_indication file by including a _ekf_logger.writeStateToFile(); line.
+* Run the EKF with your data and all the other tests by running 'make test' from the ecl directory. The [default output data csv file](https://github.com/PX4/ecl/blob/master/test/change_indication/iris_gps.csv) changes can then be included in the PR if differences are causing the CI test to fail.
+
+#### Known Issues
+If compiler versions other than GCC 7.5 are used to generate the output data file, then is is possible that the file will cause CI failures due to small numerical differences to file generated by the CI test.

test/test_EKF_externalVision.cpp

@@ -59,7 +59,7 @@ class EkfExternalVisionTest : public ::testing::Test {
 	void SetUp() override
 	{
 		_ekf->init(0);
-		_sensor_simulator.runSeconds(2);
+		_sensor_simulator.runSeconds(3);
 	}
 
 	// Use this method to clean up any memory, network etc. after each test
@@ -112,17 +112,19 @@ TEST_F(EkfExternalVisionTest, visionVelocityReset)
 	_sensor_simulator._vio.setVelocity(simulated_velocity);
 	_ekf_wrapper.enableExternalVisionVelocityFusion();
 	_sensor_simulator.startExternalVision();
-	_sensor_simulator.runMicroseconds(6e5);
+	// Note: test duration needs to allow time for tilt alignment to complete
+	_sensor_simulator.runMicroseconds(2e5);
 
 	// THEN: a reset to Vision velocity should be done
+	// Note: velocity will drift after reset due to INAV errors so the tolerance needs to allow for this
 	const Vector3f estimated_velocity = _ekf->getVelocity();
-	EXPECT_TRUE(isEqual(estimated_velocity, simulated_velocity, 1e-5f));
+	EXPECT_TRUE(isEqual(estimated_velocity, simulated_velocity, 0.01f));
 
 	// AND: the reset in velocity should be saved correctly
 	reset_logging_checker.capturePostResetState();
 	EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(1));
 	EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(1));
-	EXPECT_TRUE(reset_logging_checker.isVelocityDeltaLoggedCorrectly(1e-5f));
+	EXPECT_TRUE(reset_logging_checker.isVelocityDeltaLoggedCorrectly(0.01f));
 }
 
 TEST_F(EkfExternalVisionTest, visionVelocityResetWithAlignment)
@@ -143,11 +145,11 @@ TEST_F(EkfExternalVisionTest, visionVelocityResetWithAlignment)
 	_sensor_simulator._vio.setVelocity(simulated_velocity_in_vision_frame);
 	_ekf_wrapper.enableExternalVisionVelocityFusion();
 	_sensor_simulator.startExternalVision();
-	_sensor_simulator.runMicroseconds(6e5);
+	_sensor_simulator.runMicroseconds(2e5);
 
 	// THEN: a reset to Vision velocity should be done
 	const Vector3f estimated_velocity_in_ekf_frame = _ekf->getVelocity();
-	EXPECT_TRUE(isEqual(estimated_velocity_in_ekf_frame, simulated_velocity_in_ekf_frame, 1e-5f));
+	EXPECT_TRUE(isEqual(estimated_velocity_in_ekf_frame, simulated_velocity_in_ekf_frame, 0.01f));
 	// And: the frame offset should be estimated correctly
 	Quatf estimatedExternalVisionFrameOffset = _ekf->getVisionAlignmentQuaternion();
 	EXPECT_TRUE(matrix::isEqual(vision_to_ekf.canonical(),
@@ -157,7 +159,7 @@ TEST_F(EkfExternalVisionTest, visionVelocityResetWithAlignment)
 	reset_logging_checker.capturePostResetState();
 	EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(1));
 	EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(1));
-	EXPECT_TRUE(reset_logging_checker.isVelocityDeltaLoggedCorrectly(1e-5f));
+	EXPECT_TRUE(reset_logging_checker.isVelocityDeltaLoggedCorrectly(0.01f));
 }
 
 TEST_F(EkfExternalVisionTest, visionVarianceCheck)

test/test_EKF_flow.cpp

@@ -120,7 +120,7 @@ TEST_F(EkfFlowTest, resetToFlowVelocityOnGround)
 	// WHEN: start fusing flow data
 	_ekf_wrapper.enableFlowFusion();
 	_sensor_simulator.startFlow();
-	_sensor_simulator.runSeconds(0.6);
+	_sensor_simulator.runSeconds(1.0);
 
 	// THEN: estimated velocity should match simulated velocity
 	const Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity());

test/test_EKF_fusionLogic.cpp

@@ -335,14 +335,14 @@ TEST_F(EkfFusionLogicTest, doRangeHeightFusion)
 	// WHEN: commanding range height and sending range data
 	_ekf_wrapper.setRangeHeight();
 	_sensor_simulator.startRangeFinder();
-	_sensor_simulator.runSeconds(2);
+	_sensor_simulator.runSeconds(2.5);
 
 	// THEN: EKF should intend to fuse range height
 	EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion());
 
 	// WHEN: stop sending range data
 	_sensor_simulator.stopRangeFinder();
-	_sensor_simulator.runSeconds(2);
+	_sensor_simulator.runSeconds(2.5);
 
 	// THEN: EKF should stop to intend to use range height
 	EXPECT_FALSE(_ekf_wrapper.isIntendingRangeHeightFusion());

test/test_EKF_imuSampling.cpp

@@ -76,8 +76,10 @@ TEST_P(EkfImuSamplingTest, imuSamplingAtMultipleRates)
 	imu_sample.delta_vel = accel * imu_sample.delta_vel_dt;
 
 	// 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)
 	{
+		n_samples++;
 		imu_sample.time_us = _t_us;
 		_ekf.setIMUData(imu_sample);
 		_t_us += dt_us;
@@ -90,8 +92,9 @@ TEST_P(EkfImuSamplingTest, imuSamplingAtMultipleRates)
 
 	// WHEN: downsampling the imu measurement
 	// THEN: the delta vel should be accumulated correctly
-	EXPECT_TRUE(matrix::isEqual(ang_vel, imu_sample_buffered.delta_ang/imu_sample_buffered.delta_ang_dt, 1e-7f));
-	EXPECT_TRUE(matrix::isEqual(accel, imu_sample_buffered.delta_vel/imu_sample_buffered.delta_vel_dt, 1e-7f));
+	// Allow for accumulation of rounding error with each sample
+	EXPECT_TRUE(matrix::isEqual(ang_vel, imu_sample_buffered.delta_ang/imu_sample_buffered.delta_ang_dt, float(n_samples) * 1e-7f));
+	EXPECT_TRUE(matrix::isEqual(accel, imu_sample_buffered.delta_vel/imu_sample_buffered.delta_vel_dt, float(n_samples) * 1e-7f));
 }
 
 INSTANTIATE_TEST_SUITE_P(imuSamplingAtMultipleRates,

test/test_EKF_initialization.cpp

@@ -49,7 +49,7 @@ class EkfInitializationTest : public ::testing::Test {
 	SensorSimulator _sensor_simulator;
 	EkfWrapper _ekf_wrapper;
 
-	const float _init_tilt_period = 0.5; // seconds
+	const float _init_tilt_period = 1.0; // seconds
 
 	// GTests is calling this
 	void SetUp() override
@@ -155,7 +155,10 @@ TEST_F(EkfInitializationTest, initializeWithTilt)
 
 TEST_F(EkfInitializationTest, initializeWithPitch90)
 {
-	const Quatf quat_sim(0.0f, 0.7071068f, 0.0f, 0.7071068f);
+	const float pitch = math::radians(90.0f);
+	const float roll = math::radians(0.0f);
+	const Eulerf euler_angles_sim(roll, pitch, 0.0f);
+	const Quatf quat_sim(euler_angles_sim);
 
 	_sensor_simulator.simulateOrientation(quat_sim);
 	_sensor_simulator.runSeconds(_init_tilt_period);
@@ -167,7 +170,10 @@ TEST_F(EkfInitializationTest, initializeWithPitch90)
 
 TEST_F(EkfInitializationTest, initializeWithRoll90)
 {
-	const Quatf quat_sim(0.7071068f, 0.7071068f, 0.0f, 0.0f);
+	const float pitch = math::radians(0.0f);
+	const float roll = math::radians(90.0f);
+	const Eulerf euler_angles_sim(roll, pitch, 0.0f);
+	const Quatf quat_sim(euler_angles_sim);
 
 	_sensor_simulator.simulateOrientation(quat_sim);
 	_sensor_simulator.runSeconds(_init_tilt_period);