fix unit tests

src/modules/ekf2/EKF/terrain_estimator.cpp

@@ -366,4 +366,5 @@ void Ekf::fuseHaglAllStates()
 			_innov_check_fail_status.flags.reject_ver_pos = true;
 		}
 	}
+	updateTerrainValidity();
 }

src/modules/ekf2/EKF2.cpp

@@ -1017,7 +1017,7 @@ void EKF2::PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu)
 	static constexpr size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
 
 	// Get covariances to vehicle odometry
-	float covariances[24];
+	float covariances[25];
 	_ekf.covariances_diagonal().copyTo(covariances);
 
 	// initially set pose covariances to 0

src/modules/ekf2/test/sensor_simulator/ekf_logger.cpp

@@ -21,13 +21,13 @@ void EkfLogger::writeStateToFile()
 		_file << "Timestamp";
 
 		if (_state_logging_enabled) {
-			for (int i = 0; i < 24; i++) {
+			for (int i = 0; i < 25; i++) {
 				_file << ",state[" << i << "]";
 			}
 		}
 
 		if (_variance_logging_enabled) {
-			for (int i = 0; i < 24; i++) {
+			for (int i = 0; i < 25; i++) {
 				_file << ",variance[" << i << "]";
 			}
 		}
@@ -52,15 +52,15 @@ void EkfLogger::writeState()
 		_file << time;
 
 		if (_state_logging_enabled) {
-			matrix::Vector<float, 24> state = _ekf->getStateAtFusionHorizonAsVector();
+			matrix::Vector<float, 25> state = _ekf->getStateAtFusionHorizonAsVector();
 
-			for (int i = 0; i < 24; i++) {
+			for (int i = 0; i < 25; i++) {
 				_file << "," << std::setprecision(3) << state(i);
 			}
 		}
 
 		if (_variance_logging_enabled) {
-			matrix::Vector<float, 24> variance = _ekf->covariances_diagonal();
+			matrix::Vector<float, 25> variance = _ekf->covariances_diagonal();
 
 			for (int i = 0; i < 24; i++) {
 				_file << "," << variance(i);

src/modules/ekf2/test/test_EKF_initialization.cpp

@@ -169,7 +169,7 @@ TEST_F(EkfInitializationTest, gyroBias)
 		if (fabsf(accel_bias(2)) > 0.3f) {
 
 			// Print state covariance and correlation matrices for debugging
-			const matrix::SquareMatrix<float, 24> P = _ekf->covariances();
+			const matrix::SquareMatrix<float, 25> P = _ekf->covariances();
 
 			printf("State covariance:\n");