diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
index 3d34199e84..a4677e8222 100644
--- a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
+++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
@@ -95,8 +95,8 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel
 	}
 
 	if (avoidance_point_valid) {
-		pos_sp = _sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position;
-		vel_sp = _sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity;
+		pos_sp = Vector3f(_sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position);
+		vel_sp = Vector3f(_sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity);
 
 		if (!_ext_yaw_active) {
 			// inject yaw setpoints only if weathervane isn't active
diff --git a/src/lib/matrix b/src/lib/matrix
index 973999a4d3..92d1c8761e 160000
--- a/src/lib/matrix
+++ b/src/lib/matrix
@@ -1 +1 @@
-Subproject commit 973999a4d3c6d4d85bf0153230974fd9571f7846
+Subproject commit 92d1c8761ec8e5dbe9ba3f756cebd5e90956a1b2
diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp
index b34b45e423..629161a84b 100644
--- a/src/modules/navigator/follow_target.cpp
+++ b/src/modules/navigator/follow_target.cpp
@@ -56,7 +56,7 @@
 
 #include "navigator.h"
 
-using matrix::wrap_pi;
+using namespace matrix;
 
 constexpr float FollowTarget::_follow_position_matricies[4][9];
 
@@ -89,7 +89,7 @@ void FollowTarget::on_activation()
 		_follow_target_position = FOLLOW_FROM_BEHIND;
 	}
 
-	_rot_matrix = (_follow_position_matricies[_follow_target_position]);
+	_rot_matrix = Dcmf(_follow_position_matricies[_follow_target_position]);
 }
 
 void FollowTarget::on_active()