diff --git a/pycar/src/car/tracking/icp.py b/pycar/src/car/tracking/icp.py
index 9dd1876..1c454dd 100644
--- a/pycar/src/car/tracking/icp.py
+++ b/pycar/src/car/tracking/icp.py
@@ -9,12 +9,13 @@ can provide an estimation as to how the tracked groups have moved.
 import math
 import numpy as np
 
+
 def closest_points(data_shape, model_shape):
     """
     Returns the closest points from data to model in the same order as the data shape.
     """
     def calc_closest_point(data, model_vector):
-        current_closest = np.array([[0,0]])
+        current_closest = np.array([[0, 0]])
         min_distance = -1
         # Could vectorise this, but the speed advantage would be minimal
         for point in model_vector:
@@ -27,14 +28,17 @@ def closest_points(data_shape, model_shape):
     closest_func = np.vectorize(calc_closest_point)
     return closest_func(data_shape, model_shape)
 
+
 def euclid_distance(point1, point2):
     return math.sqrt(((point1[0] - point2[0]) ** 2) + ((point1[1] - point2[1]) ** 2))
 
-def calc_mse(points):
+
+def calc_mse(data, model, quaternion, translation):
     """
     Calculates the mean squared error for the points, after a registration.
     """
-    pass
+    return (translation - calc_rotation_matrix(quaternion).dot(data) - translation).sum(0) / len(data)
+
 
 def calc_cross_cov(data: np.ndarray, model: np.ndarray) -> np.ndarray:
     """
@@ -42,39 +46,45 @@ 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_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)
     """
-    Q = np.array([[[cross_cov.trace()], [delta.T]], 
-        [[delta], [cross_cov + cross_cov.T - cross_cov.trace().dot(np.identity(3))]]])
+    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]
+    return eigen_vectors[:, optimal_eigen_index]
+
 
 def calc_translation(optimal_rotation, mass_centre_data, mass_centre_model):
     """
     Calculates the optimal translation with the given (optimal) quaternion (which is converted to a rotation
     matrix within the funtion)
     """
-    pass
+    return mass_centre_model - optimal_rotation.dot(mass_centre_data)
+
 
 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]]])
-    
+    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:
     """
     Calculates the point at the centre of mass for the given set of points.
 
     The returned vector is in form 1xn, where point set is of shape mxn
     """
-    return points.sum(0) / len(points)
\ No newline at end of file
+    return points.sum(0) / len(points)