diff --git a/asv_setup/config/robots/freya.yaml b/asv_setup/config/robots/freya.yaml
index 7ec64e1e..49dd7cf6 100644
--- a/asv_setup/config/robots/freya.yaml
+++ b/asv_setup/config/robots/freya.yaml
@@ -21,6 +21,8 @@
       buoyancy: 0   # kg
       center_of_mass: [0, 0, 0]        # mm (x,y,z)
       center_of_buoyancy: [0, 0, 0]   # mm (x,y,z)
+      inertia_matrix_diag: [] # kg
+      damping_matrix_diag: []
 
 
     propulsion:
diff --git a/motion/hybridpath_controller/config/hybridpath_controller_config.yaml b/motion/hybridpath_controller/config/hybridpath_controller_config.yaml
index dcf4f422..f591e88d 100644
--- a/motion/hybridpath_controller/config/hybridpath_controller_config.yaml
+++ b/motion/hybridpath_controller/config/hybridpath_controller_config.yaml
@@ -1 +1,5 @@
-# K1 and K2 should be put here
\ No newline at end of file
+/**:
+  ros_parameters:
+    gain_matrices:
+      K1_diag: [10, 10, 10] # First gain matrix
+      K2_diag: [60, 60, 60] # Second gain matrix
\ No newline at end of file
diff --git a/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py b/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py
index 74667ea5..1fb4a3cd 100644
--- a/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py
+++ b/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py
@@ -4,18 +4,11 @@
 from transforms3d.euler import quat2euler
 
 class AdaptiveBackstep:
-    def __init__(self):
-        self.init_system()
-
-    def init_system(self) -> None:
-        self.K_1 = np.diag([10, 10, 10]) # First gain matrix
-        self.K_2 = np.diag([60, 60, 30]) # Second gain matrix
-        self.tau_max = np.array([41.0, 50.0, 55.0]) # Må tilpasses thrusterne
-
-        ## Forenklet modell ## Bør også endres
-        m = 50
-        self.M = np.diag([m, m, m])
-        self.D = np.diag([10, 10, 5])
+    def __init__(self, K1: np.ndarray, K2: np.ndarray, M: np.ndarray, D: np.ndarray) -> None:
+        self.K_1 = K1
+        self.K_2 = K2
+        self.M = M
+        self.D = D
 
     def control_law(self, state: Odometry, reference: HybridpathReference) -> np.ndarray:
         """
diff --git a/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py b/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py
index 9e8b0c57..3fa95e4b 100644
--- a/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py
+++ b/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py
@@ -1,4 +1,5 @@
 import rclpy
+import numpy as np
 from rclpy.node import Node
 from hybridpath_controller.adaptive_backstep import AdaptiveBackstep
 from geometry_msgs.msg import Wrench
@@ -8,12 +9,34 @@
 class HybridPathControllerNode(Node):
     def __init__(self):
         super().__init__("hybridpath_controller_node")
+
+        self.declare_parameters(
+            namespace='',
+            parameters=[
+                ("hybridpath_controller.K1_diag", [10, 10, 10]),
+                ("hybridpath_controller.K2_diag", [60, 60, 60]),
+                ("hybridpath_controller.M_diag", [50, 50, 50]),
+                ("hybridpath_controller.D_diag", [10, 10, 5])
+            ])
         
         self.state_subscriber_ = self.state_subscriber_ = self.create_subscription(Odometry, "/sensor/seapath/odometry/ned", self.state_callback, 1)
         self.hpref_subscriber_ = self.create_subscription(HybridpathReference, "guidance/hybridpath/reference", self.reference_callback, 1)
         self.wrench_publisher_ = self.create_publisher(Wrench, "thrust/wrench_input", 1)
 
-        self.AB_controller_ = AdaptiveBackstep()
+        # Get parameters
+        K1_diag = self.get_parameter("hybridpath_controller.K1_diag").get_parameter_value().double_array_value
+        K2_diag = self.get_parameter("hybridpath_controller.K2_diag").get_parameter_value().double_array_value
+        M_diag = self.get_parameter("hybridpath_controller.M_diag").get_parameter_value().double_array_value
+        D_diag = self.get_parameter("hybridpath_controller.D_diag").get_parameter_value().double_array_value
+
+        # Transform parameters to diagonal matrices
+        K1 = np.diag(K1_diag)
+        K2 = np.diag(K2_diag)
+        M = np.diag(M_diag)
+        D = np.diag(D_diag)
+
+        # Create controller object
+        self.AB_controller_ = AdaptiveBackstep(K1, K2, M, D)
 
         controller_period = 0.1
         self.controller_timer_ = self.create_timer(controller_period, self.controller_callback)