diff --git a/applications/SystemIdentificationApplication/custom_python/add_custom_utilities_to_python.cpp b/applications/SystemIdentificationApplication/custom_python/add_custom_utilities_to_python.cpp
index c17abab431e..baf309ba5cb 100644
--- a/applications/SystemIdentificationApplication/custom_python/add_custom_utilities_to_python.cpp
+++ b/applications/SystemIdentificationApplication/custom_python/add_custom_utilities_to_python.cpp
@@ -23,6 +23,7 @@
 #include "custom_utilities/control_utils.h"
 #include "custom_utilities/smooth_clamper.h"
 #include "custom_utilities/sensor_generator_utils.h"
+#include "custom_utilities/distance_matrix.h"
 
 // Include base h
 #include "custom_python/add_custom_utilities_to_python.h"
@@ -62,6 +63,14 @@ void AddCustomUtilitiesToPython(pybind11::module& m)
 
     auto sensor_generator_utils = m.def_submodule("SensorGeneratorUtils");
     sensor_generator_utils.def("IsPointInGeometry", &SensorGeneratorUtils::IsPointInGeometry, py::arg("point"), py::arg("geometry"));
+
+    py::class_<DistanceMatrix, DistanceMatrix::Pointer>(m, "DistanceMatrix")
+        .def(py::init<>())
+        .def("Update", &DistanceMatrix::Update, py::arg("values_container_expression"))
+        .def("GetDistance", py::overload_cast<const IndexType, const IndexType>(&DistanceMatrix::GetDistance, py::const_), py::arg("index_i"), py::arg("index_j"))
+        .def("GetEntriesSize", &DistanceMatrix::GetEntriesSize)
+        .def("GetNumberOfItems", &DistanceMatrix::GetNumberOfItems)
+        ;
 }
 
 } // namespace Kratos::Python
diff --git a/applications/SystemIdentificationApplication/custom_utilities/distance_matrix.cpp b/applications/SystemIdentificationApplication/custom_utilities/distance_matrix.cpp
new file mode 100644
index 00000000000..8cc1b9b5f85
--- /dev/null
+++ b/applications/SystemIdentificationApplication/custom_utilities/distance_matrix.cpp
@@ -0,0 +1,119 @@
+//    |  /           |
+//    ' /   __| _` | __|  _ \   __|
+//    . \  |   (   | |   (   |\__ `
+//   _|\_\_|  \__,_|\__|\___/ ____/
+//                   Multi-Physics
+//
+//  License:         SystemIdentificationApplication/license.txt
+//
+//  Main authors:    Suneth Warnakulasuriya
+//
+
+// System includes
+#include <cmath>
+
+// External includes
+
+// Project includes
+#include "utilities/parallel_utilities.h"
+
+// Application includes
+
+// Include base h
+#include "distance_matrix.h"
+
+namespace Kratos {
+///@name Kratos Classes
+///@{
+
+DistanceMatrix::DistanceMatrix()
+    : mN(0)
+{
+}
+
+void DistanceMatrix::Update(
+    std::variant<
+        ContainerExpression<ModelPart::NodesContainerType>::Pointer,
+        ContainerExpression<ModelPart::ConditionsContainerType>::Pointer,
+        ContainerExpression<ModelPart::ElementsContainerType>::Pointer> pDistancesExpression)
+{
+    KRATOS_TRY
+
+    std::visit([&](const auto& pExp) {
+        this->mN = pExp->GetContainer().size();
+        this->mDistances.resize(this->GetEntriesSize());
+
+        const auto& r_expression = pExp->GetExpression();
+        const auto dimensionality = r_expression.GetItemComponentCount();
+
+        IndexPartition<IndexType>(this->mDistances.size()).for_each([&](const auto Index) {
+            const auto& index_pair = GetIndexPair(Index);
+
+            const auto i_index = std::get<0>(index_pair);
+            const auto i_data_begin = i_index * dimensionality;
+            const auto j_index = std::get<1>(index_pair);
+            const auto j_data_begin = j_index * dimensionality;
+
+            double distance = 0.0;
+
+            for (IndexType i_comp = 0; i_comp < dimensionality; ++i_comp) {
+                const double i_value = r_expression.Evaluate(i_index, i_data_begin, i_comp);
+                const double j_value = r_expression.Evaluate(j_index, j_data_begin, i_comp);
+                distance += std::pow(i_value - j_value, 2.0);
+            }
+
+            mDistances[Index] = std::sqrt(distance);
+        });
+    }, pDistancesExpression);
+
+    KRATOS_CATCH("");
+}
+
+double DistanceMatrix::GetDistance(
+    const IndexType iIndex,
+    const IndexType jIndex) const
+{
+    auto ordered_i = iIndex;
+    auto ordered_j = jIndex;
+    if (ordered_i != ordered_j) {
+        if (ordered_j < ordered_i) {
+            std::swap(ordered_i, ordered_j);
+        }
+        return mDistances[GetEntryIndex(ordered_i, ordered_j)];
+    } else {
+        return 0.0;
+    }
+}
+
+double DistanceMatrix::GetDistance(const IndexType EntryIndex) const
+{
+    return mDistances[EntryIndex];
+}
+
+IndexType DistanceMatrix::GetEntriesSize() const
+{
+    return this->mN * (this->mN - 1) / 2;
+}
+
+IndexType DistanceMatrix::GetNumberOfItems() const
+{
+    return this->mN;
+}
+
+IndexType DistanceMatrix::GetEntryIndex(
+    const IndexType iIndex,
+    const IndexType jIndex) const
+{
+    return this->mN * iIndex + jIndex - ((iIndex + 2) * (iIndex + 1)) / 2;
+}
+
+std::tuple<IndexType, IndexType> DistanceMatrix::GetIndexPair(const IndexType EntryIndex) const
+{
+
+    const IndexType i = static_cast<IndexType>(std::floor(this->mN - 0.5 - std::sqrt(std::pow(this->mN - 0.5, 2) - 2 * EntryIndex)));
+    const IndexType j = static_cast<IndexType>(EntryIndex - this->mN * i + (i + 1) * (i + 2) / 2);
+
+    return std::make_tuple(i, j);
+}
+
+} /* namespace Kratos.*/
\ No newline at end of file
diff --git a/applications/SystemIdentificationApplication/custom_utilities/distance_matrix.h b/applications/SystemIdentificationApplication/custom_utilities/distance_matrix.h
new file mode 100644
index 00000000000..ccb1f201fc6
--- /dev/null
+++ b/applications/SystemIdentificationApplication/custom_utilities/distance_matrix.h
@@ -0,0 +1,88 @@
+//    |  /           |
+//    ' /   __| _` | __|  _ \   __|
+//    . \  |   (   | |   (   |\__ `
+//   _|\_\_|  \__,_|\__|\___/ ____/
+//                   Multi-Physics
+//
+//  License:         SystemIdentificationApplication/license.txt
+//
+//  Main authors:    Suneth Warnakulasuriya
+//
+
+#pragma once
+
+// System includes
+#include <tuple>
+#include <variant>
+
+// External includes
+
+// Project includes
+#include "includes/define.h"
+#include "includes/model_part.h"
+#include "expression/container_expression.h"
+
+// Application includes
+
+namespace Kratos {
+///@name Kratos Classes
+///@{
+
+class KRATOS_API(SYSTEM_IDENTIFICATION_APPLICATION) DistanceMatrix
+{
+public:
+    ///@name Type definitions
+    ///@{
+
+    using IndexType = std::size_t;
+
+    KRATOS_CLASS_POINTER_DEFINITION(DistanceMatrix);
+
+    ///@}
+    ///@name Life cycle
+    ///@{
+
+    DistanceMatrix();
+
+    ///@}
+    ///@name Public operations
+    ///@{
+
+    double GetDistance(
+        const IndexType iIndex,
+        const IndexType jIndex) const;
+
+    double GetDistance(const IndexType EntryIndex) const;
+
+    IndexType GetEntriesSize() const;
+
+    IndexType GetEntryIndex(
+        const IndexType iIndex,
+        const IndexType jIndex) const;
+
+    std::tuple<IndexType, IndexType> GetIndexPair(const IndexType iEntryIndex) const;
+
+    IndexType GetNumberOfItems() const;
+
+    void Update(
+        std::variant<
+            ContainerExpression<ModelPart::NodesContainerType>::Pointer,
+            ContainerExpression<ModelPart::ConditionsContainerType>::Pointer,
+            ContainerExpression<ModelPart::ElementsContainerType>::Pointer> pDistancesExpression);
+
+    ///@}
+
+private:
+    ///@name Private member variables
+    ///@{
+
+    std::vector<double> mDistances;
+
+    IndexType mN;
+
+    ///@}
+};
+
+///@} // Kratos Classes
+
+} /* namespace Kratos.*/
\ No newline at end of file
diff --git a/applications/SystemIdentificationApplication/tests/test_SystemIdentificationApplication.py b/applications/SystemIdentificationApplication/tests/test_SystemIdentificationApplication.py
index 165cb4ae4bc..1b312983155 100644
--- a/applications/SystemIdentificationApplication/tests/test_SystemIdentificationApplication.py
+++ b/applications/SystemIdentificationApplication/tests/test_SystemIdentificationApplication.py
@@ -6,6 +6,7 @@
 import test_system_identification
 import responses.test_damage_response
 import test_smooth_clamper
+import test_distance_matrix
 
 def AssembleTestSuites():
     suites = KratosUnittest.KratosSuites
@@ -15,6 +16,7 @@ def AssembleTestSuites():
     smallSuite.addTests(KratosUnittest.TestLoader().loadTestsFromTestCases([test_adjoint_sensors.TestStrainSensor]))
     smallSuite.addTests(KratosUnittest.TestLoader().loadTestsFromTestCases([test_sensor_output_process.TestSensorOutputProcess]))
     smallSuite.addTests(KratosUnittest.TestLoader().loadTestsFromTestCases([test_smooth_clamper.TestSmoothClamper]))
+    smallSuite.addTests(KratosUnittest.TestLoader().loadTestsFromTestCases([test_distance_matrix.TestDistanceMatrix]))
     smallSuite.addTests(KratosUnittest.TestLoader().loadTestsFromTestCases([test_system_identification.TestSystemIdentification]))
     smallSuite.addTests(KratosUnittest.TestLoader().loadTestsFromTestCases([responses.test_damage_response.TestDamageDetectionAdjointResponseFunction]))
     smallSuite.addTests(KratosUnittest.TestLoader().loadTestsFromTestCases([responses.test_damage_response.TestDamageDetectionResponse]))
diff --git a/applications/SystemIdentificationApplication/tests/test_distance_matrix.py b/applications/SystemIdentificationApplication/tests/test_distance_matrix.py
new file mode 100644
index 00000000000..a9522e87cd7
--- /dev/null
+++ b/applications/SystemIdentificationApplication/tests/test_distance_matrix.py
@@ -0,0 +1,27 @@
+import KratosMultiphysics as Kratos
+import KratosMultiphysics.SystemIdentificationApplication as KratosSI
+import KratosMultiphysics.KratosUnittest as UnitTest
+
+class TestDistanceMatrix(UnitTest.TestCase):
+    @classmethod
+    def setUpClass(cls) -> None:
+        cls.model = Kratos.Model()
+        cls.model_part = cls.model.CreateModelPart("test")
+
+        for i in range(101):
+            cls.model_part.CreateNewNode(i + 1, i + 1, i + 2, i + 3)
+
+        cls.distance_matrix = KratosSI.DistanceMatrix()
+
+        exp = Kratos.Expression.NodalExpression(cls.model_part)
+        Kratos.Expression.NodalPositionExpressionIO.Read(exp, Kratos.Configuration.Initial)
+        cls.distance_matrix.Update(exp)
+
+    def test_GetDistance1(self) -> None:
+        for i, node_i in enumerate(self.model_part.Nodes):
+            for j, node_j in enumerate(self.model_part.Nodes):
+                distance = ((node_i.X - node_j.X) ** 2 + (node_i.Y - node_j.Y) ** 2 + (node_i.Z - node_j.Z) ** 2) ** (0.5)
+                self.assertAlmostEqual(self.distance_matrix.GetDistance(i, j), distance)
+
+if __name__ == '__main__':
+    UnitTest.main()
\ No newline at end of file