diff --git a/pycar/src/car/tracking/icp.py b/pycar/src/car/tracking/icp.py
index 439bad1..9dd1876 100644
--- a/pycar/src/car/tracking/icp.py
+++ b/pycar/src/car/tracking/icp.py
@@ -42,11 +42,19 @@ def calc_cross_cov(data: np.ndarray, model: np.ndarray) -> np.ndarray:
     """
     return (data.dot(model.T).sum(0) / data.shape[0]) - calc_mass_centre(data).dot(calc_mass_centre(model).T)
 
-def calc_quaternion(cross_cov):
+def calc_delta(cross_cov):
+    A = (cross_cov - cross_cov.T)
+    return np.ndarray([[A[2][3]], [A[3][1]], [A[1][2]]])
+
+def calc_quaternion(cross_cov, delta):
     """
     Calculates the optimal unit quaternion (the optimal rotation for this iteration3,3)
     """
-    pass
+    Q = np.array([[[cross_cov.trace()], [delta.T]], 
+        [[delta], [cross_cov + cross_cov.T - cross_cov.trace().dot(np.identity(3))]]])
+    eigen_values, eigen_vectors = np.linalg.eig(Q)
+    optimal_eigen_index = eigen_values[eigen_values == eigen_values.max()][0]
+    return eigen_vectors[:,optimal_eigen_index]
 
 def calc_translation(optimal_rotation, mass_centre_data, mass_centre_model):
     """
@@ -60,7 +68,8 @@ def calc_rotation_matrix(q):
     Returns the rotation matrix for the given unit quaternion.
     """
     return np.array([[[q[0] ** 2 + q[1] ** 2 + q[2] ** 2 + q[3] ** 2], [2 * (q[1] * q[2] - q[0] * q[3])], [2 * (q[1] * q[3] - q[0] * q[2])]], 
-    []])
+    [[2 * (q[1] * q[2] + q[0] * q[3])], [q[0] ** 2 + q[2] ** 2 - q[1] ** 2 - q[3] **2], [2 * (q[2] * q[3] - q[0] * q[1])]],
+    [[2 * (q[1] * q[3] - q[0] * q[2])], [2 * (q[2] * q[3] + q[0] * q[1])], [2 * (q[2] * q[3] + q[0] * q[1])], [q[0] ** 2 + q[3] ** 2- q[1] ** 2 - q[2] ** 2]]])
     
 def calc_mass_centre(points: np.ndarray) -> np.ndarray:
     """