diff --git a/EKF/ekf.h b/EKF/ekf.h
index 47b775b79d..f0f748bf7d 100644
--- a/EKF/ekf.h
+++ b/EKF/ekf.h
@@ -227,21 +227,26 @@ public:
 	// return the amount the local horizontal position changed in the last reset and the number of reset events
 	void get_posNE_reset(float delta[2], uint8_t *counter)
 	{
-		memcpy(delta, &_state_reset_status.posNE_change._data[0], sizeof(_state_reset_status.posNE_change._data));
+		delta[0] = _state_reset_status.posNE_change(0);
+		delta[1] = _state_reset_status.posNE_change(1);
 		*counter = _state_reset_status.posNE_counter;
 	}
 
 	// return the amount the local horizontal velocity changed in the last reset and the number of reset events
 	void get_velNE_reset(float delta[2], uint8_t *counter)
 	{
-		memcpy(delta, &_state_reset_status.velNE_change._data[0], sizeof(_state_reset_status.velNE_change._data));
+		delta[0] = _state_reset_status.velNE_change(0);
+		delta[1] = _state_reset_status.velNE_change(1);
 		*counter = _state_reset_status.velNE_counter;
 	}
 
 	// return the amount the quaternion has changed in the last reset and the number of reset events
 	void get_quat_reset(float delta_quat[4], uint8_t *counter)
 	{
-		memcpy(delta_quat, &_state_reset_status.quat_change._data[0], sizeof(_state_reset_status.quat_change._data));
+		for (size_t i = 0; i < 4; i++)
+		{
+			delta_quat[i] = _state_reset_status.quat_change(i);
+		}
 		*counter = _state_reset_status.quat_counter;
 	}
 
diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h
index 869ea3a879..6139a2ebfa 100644
--- a/EKF/estimator_interface.h
+++ b/EKF/estimator_interface.h
@@ -277,13 +277,6 @@ public:
 	// return true if the local position estimate is valid
 	bool local_position_is_valid();
 
-	void copy_quaternion(float *quat)
-	{
-		for (unsigned i = 0; i < 4; i++) {
-			quat[i] = _output_new.quat_nominal(i);
-		}
-	}
-
 	const matrix::Quatf &get_quaternion() const { return _output_new.quat_nominal; }
 
 	// return the quaternion defining the rotation from the EKF to the External Vision reference frame
diff --git a/airdata/WindEstimator.cpp b/airdata/WindEstimator.cpp
index df4d3bb971..a5e75b70be 100644
--- a/airdata/WindEstimator.cpp
+++ b/airdata/WindEstimator.cpp
@@ -175,7 +175,7 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const
 
 	const matrix::Matrix<float, 1, 1> S = H_tas * _P * H_tas.transpose() + _tas_var;
 
-	K /= (S._data[0][0]);
+	K /= S(0,0);
 	// compute innovation
 	const float airspeed_pred = _state(tas) * sqrtf((v_n - _state(w_n)) * (v_n - _state(w_n)) + (v_e - _state(w_e)) *
 				    (v_e - _state(w_e)) + v_d * v_d);
@@ -183,7 +183,7 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const
 	_tas_innov = true_airspeed - airspeed_pred;
 
 	// innovation variance
-	_tas_innov_var = S._data[0][0];
+	_tas_innov_var = S(0,0);
 
 	bool reinit_filter = false;
 	bool meas_is_rejected = false;
@@ -261,7 +261,7 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const
 
 	const matrix::Matrix<float, 1, 1> S = H_beta * _P * H_beta.transpose() + _beta_var;
 
-	K /= (S._data[0][0]);
+	K /= S(0,0);
 
 	// compute predicted side slip angle
 	matrix::Vector3f rel_wind(velI(0) - _state(w_n), velI(1) - _state(w_e), velI(2));
@@ -276,7 +276,7 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const
 	const float beta_pred = rel_wind(1) / rel_wind(0);
 
 	_beta_innov = 0.0f - beta_pred;
-	_beta_innov_var = S._data[0][0];
+	_beta_innov_var = S(0,0);
 
 	bool reinit_filter = false;
 	bool meas_is_rejected = false;