Update uwb_navigation_node.py

ros2/src/robobin/robobin/uwb_navigation_node.py

@@ -308,10 +308,10 @@ class UWBNode(Node):
         bounds = ([x_min, y_min, z_min], [x_max, y_max, z_max])
 
         def error_function(vars):
-            x, y = vars
+            x, y, z = vars
             residuals = []
-            for (bx, by), d_measured in zip(available_beacons, available_distances):
-                d_computed = np.sqrt((x - bx)**2 + (y - by)**2)
+            for (bx, by, bz), d_measured in zip(available_beacons, available_distances):
+                d_computed = np.sqrt((x - bx)**2 + (y - by)**2 + (z - bz)**2)
                 residuals.append(d_computed - d_measured)
             return residuals