diff --git a/modules/CACC/CACC-Module-Test.py b/modules/CACC/CACC-Module-Test.py
index a542d0e6b0ea13efe5a24876df2e4c951b649092..e2c8c8db58e4aaad8bd6e9f3cc0e01a818baf418 100644
--- a/modules/CACC/CACC-Module-Test.py
+++ b/modules/CACC/CACC-Module-Test.py
@@ -150,24 +150,27 @@ if __name__ == "__main__":
 
 
     plt.subplot(131)
-    plt.plot(t, y_pos[0], 'r')
-    plt.plot(t, y_pos[1], 'b')
-    plt.plot(t, y_pos[2], 'g')
+    plt.plot(t, y_pos[0], 'r', label='LV')
+    plt.plot(t, y_pos[1], 'b', label='FV 1')
+    plt.plot(t, y_pos[2], 'g', label='FV 2')
+    plt.legend()
     plt.title("Position")
 
     plt.subplot(132)
-    plt.plot(t, mesPS, 'r--')
-    plt.plot(t, y_speed[0], 'r-')
-    plt.plot(t, y_speed[1], 'b')
-    plt.plot(t, y_speed[2], 'g')
+    plt.plot(t, mesPS, 'r--', label='PS')
+    plt.plot(t, y_speed[0], 'r-', label='LV')
+    plt.plot(t, y_speed[1], 'b-', label='FV 1')
+    plt.plot(t, y_speed[2], 'g-', label='FV 2')
+    plt.legend()
     plt.title("Speed")
     
     plt.subplot(133)
-    plt.plot(t, mesIPD, 'r--')
-    plt.plot(t, y_dist[1], 'b-')
-    plt.plot(t, y_cdist[1], 'b--')
-    plt.plot(t, y_dist[2], 'g-')
-    plt.plot(t, y_cdist[2], 'g--')
+    plt.plot(t, mesIPD, 'r--', label='IPD')
+    plt.plot(t, y_dist[1], 'b-', label='FV1 - LV - Distance')
+    plt.plot(t, y_cdist[1], 'b--', label='FV1 - cIPD)')
+    plt.plot(t, y_dist[2], 'g-', label='FV2 - FV1 - Distance')
+    plt.plot(t, y_cdist[2], 'g--', label='FV2 - cIPD')
+    plt.legend()
     plt.title("Distance")
 
     plt.show()