navigator follow_target move to matrix lib

src/modules/navigator/follow_target.cpp

@@ -97,7 +97,6 @@ void FollowTarget::on_activation()
 void FollowTarget::on_active()
 {
 	struct map_projection_reference_s target_ref;
-	math::Vector<3> target_reported_velocity(0, 0, 0);
 	follow_target_s target_motion_with_offset = {};
 	uint64_t current_time = hrt_absolute_time();
 	bool _radius_entered = false;
@@ -128,9 +127,6 @@ void FollowTarget::on_active()
 		_current_target_motion.lon = (_current_target_motion.lon * (double)_responsiveness) + target_motion.lon * (double)(
 						     1 - _responsiveness);
 
-		target_reported_velocity(0) = _current_target_motion.vx;
-		target_reported_velocity(1) = _current_target_motion.vy;
-
 	} else if (((current_time - _current_target_motion.timestamp) / 1000) > TARGET_TIMEOUT_MS && target_velocity_valid()) {
 		reset_target_validity();
 	}

src/modules/navigator/follow_target.h

@@ -125,13 +125,13 @@ private:
 	uint64_t _target_updates{0};
 	uint64_t _last_update_time{0};
 
-	math::Vector<3> _current_vel;
-	math::Vector<3> _step_vel;
-	math::Vector<3> _est_target_vel;
-	math::Vector<3> _target_distance;
-	math::Vector<3> _target_position_offset;
-	math::Vector<3> _target_position_delta;
-	math::Vector<3> _filtered_target_position_delta;
+	matrix::Vector3f _current_vel;
+	matrix::Vector3f _step_vel;
+	matrix::Vector3f _est_target_vel;
+	matrix::Vector3f _target_distance;
+	matrix::Vector3f _target_position_offset;
+	matrix::Vector3f _target_position_delta;
+	matrix::Vector3f _filtered_target_position_delta;
 
 	follow_target_s _current_target_motion{};
 	follow_target_s _previous_target_motion{};
@@ -148,7 +148,7 @@ private:
 		ATT_RATES = 3
 	};
 
-	math::Matrix<3, 3> _rot_matrix;
+	matrix::Dcmf _rot_matrix;
 
 	void track_target_position();
 	void track_target_velocity();