diff --git a/src/shared/Modules/Graph/Graph.cpp b/src/shared/Modules/Graph/Graph.cpp
index 2a9f40ac38dc61bfc7c95689ded4bf3bfdd98510..4c44685b2c844b7984e958b06f80baa1cbd0c7b8 100644
--- a/src/shared/Modules/Graph/Graph.cpp
+++ b/src/shared/Modules/Graph/Graph.cpp
@@ -21,8 +21,8 @@
 #include <Components/SubscriptionsPanel/SubscriptionsPanel.h>
 #include <Core/MessageBroker/MessageBroker.h>
 
+#include <QDebug>
 #include <QTimer>
-#include <algorithm>
 
 Graph::Graph() : Module(ModuleId::GRAPH)
 {
@@ -146,42 +146,8 @@ void Graph::onFilterAdded(const Filter& filter)
 
     MessageBroker::getInstance().subscribe(
         filter, this,
-        [&](const Message& message, const Filter& filter)
-        {
-            if (stopped && !lines.contains(filter))
-                return;
-
-            if (!message.getFields().contains("timestamp"))
-                return;
-
-            for (auto field : filter.getFields())
-            {
-                if (field == "timestamp")
-                    continue;
-
-                auto& line               = lines[filter].first[field];
-                QVector<double>& bufferX = line.bufferX;
-                QVector<double>& bufferY = line.bufferY;
-
-                double x =
-                    message.getField("timestamp").getUnsignedInteger() / 1e6;
-                double y = message.getField(field).getDouble();
-
-                lines[filter].first[field].graph->setName(field + ": " +
-                                                          QString::number(y));
-
-                // Check if the timestamp resets
-                if (lastTimestamp - 1 > x)
-                    onClearClicked();
-
-                lastTimestamp = x;
-                bufferX.append(x);
-                bufferY.append(y);
-            }
-
-            // Flag the data as updated
-            lines[filter].second = true;
-        });
+        [this](const Message& message, const Filter& filter)
+        { onMessageReceived(message, filter); });
 }
 
 void Graph::onFilterRemoved(const Filter& filter)
@@ -197,6 +163,12 @@ void Graph::onFilterRemoved(const Filter& filter)
         }
 
         lines.remove(filter);
+
+        MessageBroker::getInstance().unsubscribe(filter, this);
+    }
+    else
+    {
+        qWarning() << "Removing filter not found in the graph";
     }
 }
 
@@ -246,6 +218,42 @@ void Graph::onUpdateTimerTick()
     }
 }
 
+void Graph::onMessageReceived(const Message& message, const Filter& filter)
+{
+    if (stopped && !lines.contains(filter))
+        return;
+
+    if (!message.getFields().contains("timestamp"))
+        return;
+
+    for (auto field : filter.getFields())
+    {
+        if (field == "timestamp")
+            continue;
+
+        auto& line               = lines[filter].first[field];
+        QVector<double>& bufferX = line.bufferX;
+        QVector<double>& bufferY = line.bufferY;
+
+        double x = message.getField("timestamp").getUnsignedInteger() / 1e6;
+        double y = message.getField(field).getDouble();
+
+        lines[filter].first[field].graph->setName(field + ": " +
+                                                  QString::number(y));
+
+        // Check if the timestamp resets
+        if (lastTimestamp - 1 > x)
+            onClearClicked();
+
+        lastTimestamp = x;
+        bufferX.append(x);
+        bufferY.append(y);
+    }
+
+    // Flag the data as updated
+    lines[filter].second = true;
+}
+
 void Graph::setupUi()
 {
     QHBoxLayout* outerLayout = new QHBoxLayout;
diff --git a/src/shared/Modules/Graph/Graph.h b/src/shared/Modules/Graph/Graph.h
index 099583f471e80ee62b11ca97dff6dc8ec20d6576..ff092edfa95a20c7fc43c91795fc2c814168ac75 100644
--- a/src/shared/Modules/Graph/Graph.h
+++ b/src/shared/Modules/Graph/Graph.h
@@ -49,6 +49,8 @@ private slots:
 
     void onUpdateTimerTick();
 
+    void onMessageReceived(const Message& message, const Filter& filter);
+
 private:
     void setupUi();
     void customContextMenuActionSetup();