diff --git a/CMakeLists.txt b/CMakeLists.txt
index dbc700ca9c..c6c10dc94a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -132,7 +132,7 @@ if(CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR)
 	include(ExternalProject)
 	ExternalProject_Add(matrix
 			GIT_REPOSITORY "https://github.com/PX4/Matrix.git"
-			GIT_TAG d18be0d0fa17d9a0b60ab34bad3e33160f907b09
+			GIT_TAG d613055462bcbeacfd188cbd53de56c1dbc7b94d
 			UPDATE_COMMAND ""
 			PATCH_COMMAND ""
 			CONFIGURE_COMMAND ""
diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
index dcea2df13c..261e160cba 100644
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@@ -112,11 +112,9 @@ void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel) {
 	_state.vel.xy() = new_horz_vel;
 
 	for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
-		_output_buffer[index].vel(0) += delta_horz_vel(0);
-		_output_buffer[index].vel(1) += delta_horz_vel(1);
+		_output_buffer[index].vel.xy() += delta_horz_vel;
 	}
-	_output_new.vel(0) += delta_horz_vel(0);
-	_output_new.vel(1) += delta_horz_vel(1);
+	_output_new.vel.xy() += delta_horz_vel;
 
 	_state_reset_status.velNE_change = delta_horz_vel;
 	_state_reset_status.velNE_counter++;
@@ -193,11 +191,9 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos) {
 	_state.pos.xy() = new_horz_pos;
 
 	for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
-		_output_buffer[index].pos(0) += delta_horz_pos(0);
-		_output_buffer[index].pos(1) += delta_horz_pos(1);
+		_output_buffer[index].pos.xy() += delta_horz_pos;
 	}
-	_output_new.pos(0) += delta_horz_pos(0);
-	_output_new.pos(1) += delta_horz_pos(1);
+	_output_new.pos.xy() += delta_horz_pos;
 
 	_state_reset_status.posNE_change = delta_horz_pos;
 	_state_reset_status.posNE_counter++;