diff --git a/dotbot/controller.py b/dotbot/controller.py
index a8d89b86..4dad2a56 100644
--- a/dotbot/controller.py
+++ b/dotbot/controller.py
@@ -5,6 +5,8 @@
 import math
 import time
 import webbrowser
+from datetime import datetime
+import pytz
 
 from abc import ABC, abstractmethod
 from binascii import hexlify
@@ -137,6 +139,7 @@ def __init__(self, settings: ControllerSettings):
         self.websockets = []
         self.lh2_manager = LighthouseManager()
         self.logger = LOGGER.bind(context=__name__)
+        self.now_utc = datetime.now(tz=pytz.utc)
 
     @abstractmethod
     def init(self):
@@ -327,7 +330,13 @@ def handle_received_payload(
             dotbot.direction = payload.values.direction
             logger = logger.bind(direction=dotbot.direction)
 
-        dotbot.lh2_position = self._compute_lh2_position(payload)
+        zzzz = self._compute_lh2_position(payload) # Add this to get the raw counts into the logger
+        
+        if zzzz is not None:
+            counts, dotbot.lh2_position = zzzz
+        else: 
+            dotbot.lh2_position = None
+        
         if (
             dotbot.lh2_position is not None
             and 0 <= dotbot.lh2_position.x <= 1
@@ -338,7 +347,7 @@ def handle_received_payload(
                 y=dotbot.lh2_position.y,
                 z=dotbot.lh2_position.z,
             )
-            logger.info("lh2", x=dotbot.lh2_position.x, y=dotbot.lh2_position.y)
+            logger.info("lh2", x=dotbot.lh2_position.x, y=dotbot.lh2_position.y, c0=counts[0], c1=counts[1])  ## TODO
             if (
                 not dotbot.position_history
                 or lh2_distance(dotbot.position_history[-1], new_position)
@@ -398,6 +407,19 @@ def handle_received_payload(
         else:
             notification = DotBotNotificationModel(cmd=notification_cmd.value)
 
+        ### EKF DATA PACKET
+        if  payload.payload_type == PayloadType.EKF_DEBUG:
+            x = payload.values.x / 1e4  # [cm]
+            y = payload.values.y / 1e4  # [cm]
+            theta = payload.values.theta / 1e3  # [degrees]
+            V = payload.values.V / 10.0 # [cm/s]
+            w = payload.values.w  / 1e3 # [degrees per second]
+            angle_to_target = payload.values.angle_to_target  # [degrees]
+            # Print all this information
+            now_utc = datetime.now(tz=pytz.utc)
+            print(f"{now_utc.strftime('%Y-%m-%dT%H:%M:%S.%fZ')}\tx={x:<12.2f}\ty={y:<12.2f}\tthe={theta:<12.2f}\tV={V:<12.2f}\tw={w:<12.2f}\ttarget={angle_to_target:<12.2f}")
+            # print(payload)
+
         if self.settings.verbose is True:
             print(payload)
         self.dotbots.update({dotbot.address: dotbot})
diff --git a/dotbot/protocol.py b/dotbot/protocol.py
index 95c95d85..85179b4c 100644
--- a/dotbot/protocol.py
+++ b/dotbot/protocol.py
@@ -11,7 +11,7 @@
 from dataclasses import dataclass
 
 
-PROTOCOL_VERSION = 7
+PROTOCOL_VERSION = 8
 
 
 class PayloadType(Enum):
@@ -28,7 +28,9 @@ class PayloadType(Enum):
     LH2_WAYPOINTS = 8
     GPS_WAYPOINTS = 9
     SAILBOT_DATA = 10
-    INVALID_PAYLOAD = 11  # Increase each time a new payload type is added
+    EKF_DEBUG = 11
+
+    INVALID_PAYLOAD = 12  # Increase each time a new payload type is added
 
 
 class ApplicationType(IntEnum):
@@ -358,6 +360,39 @@ def from_bytes(_) -> ProtocolData:
         return GPSWaypoints(threshold=0, waypoints=[])
 
 
+@dataclass
+class EKFDebugData(ProtocolData):
+    """Dataclass that holds the internal state and control variables """
+
+    x: int = 0xFFFF
+    y: int = 0xFFFF
+    theta: int = 0xFFFF
+    V: int = 0xFFFF
+    w: int = 0xFFFF
+    angle_to_target: int = 0xFFFF
+
+    @property
+    def fields(self) -> List[ProtocolField]:
+        return [
+            ProtocolField(self.x, name="x", length=4, signed=True),
+            ProtocolField(self.y, name="y", length=4, signed=True),
+            ProtocolField(self.theta, name="theta", length=4, signed=True),
+            ProtocolField(self.V, name="V", length=2, signed=True),
+            ProtocolField(self.w, name="w", length=4, signed=True),
+            ProtocolField(self.angle_to_target, name="angle_to_target", length=4, signed=True),
+        ]
+
+    @staticmethod
+    def from_bytes(bytes_) -> ProtocolData:
+        return EKFDebugData(
+            x=int.from_bytes(bytes_[0:4], "little", signed=True),
+            y=int.from_bytes(bytes_[4:8], "little", signed=True),
+            theta=int.from_bytes(bytes_[8:12], "little", signed=True),
+            V=int.from_bytes(bytes_[12:14], "little", signed=True),
+            w=int.from_bytes(bytes_[14:18], "little", signed=True),
+            angle_to_target=int.from_bytes(bytes_[18:22], "little", signed=True),
+        )
+
 @dataclass
 class ProtocolPayload:
     """Manage a protocol complete payload (header + type + values)."""
@@ -411,6 +446,8 @@ def from_bytes(bytes_: bytes):
             values = LH2Waypoints.from_bytes(None)
         elif payload_type == PayloadType.GPS_WAYPOINTS:
             values = GPSWaypoints.from_bytes(None)
+        elif payload_type == PayloadType.EKF_DEBUG:
+            values = EKFDebugData.from_bytes(bytes_[21:41])            
         else:
             raise ProtocolPayloadParserException(
                 f"Unsupported payload type '{payload_type}'"