diff --git a/.github/workflows/osrm-backend.yml b/.github/workflows/osrm-backend.yml
index 6dc5bb52851..bad5baecd86 100644
--- a/.github/workflows/osrm-backend.yml
+++ b/.github/workflows/osrm-backend.yml
@@ -211,6 +211,7 @@ jobs:
             BUILD_TYPE: Release
             CCOMPILER: gcc-11
             CXXCOMPILER: g++-11
+            ENABLE_BENCHMARKS: ON
 
           - name: gcc-10-release
             continue-on-error: false
@@ -692,7 +693,6 @@ jobs:
         cmake .. -DCMAKE_BUILD_TYPE=${BUILD_TYPE}
         make --jobs=${JOBS}
         popd
-
     - name: Run all tests
       if: ${{ matrix.NODE_PACKAGE_TESTS_ONLY != 'ON' && matrix.ENABLE_APPLE_SILICON != 'ON' }}
       run: |
@@ -710,6 +710,16 @@ jobs:
         fi
         popd
         npm test
+    - name: Run benchmarks
+      if: ${{ matrix.ENABLE_BENCHMARKS == 'ON' }}
+      run: |
+        pushd ${OSRM_BUILD_DIR}
+        make --jobs=${JOBS} benchmarks
+        ./src/benchmarks/alias-bench 
+        ./src/benchmarks/match-bench ../test/data/ch/monaco.osrm
+        ./src/benchmarks/packedvector-bench
+        ./src/benchmarks/rtree-bench ../test/data/monaco.osrm.ramIndex ../test/data/monaco.osrm.fileIndex ../test/data/monaco.osrm.nbg_nodes
+        popd
     - name: Run Node package tests only
       if: ${{ matrix.NODE_PACKAGE_TESTS_ONLY == 'ON' && matrix.ENABLE_APPLE_SILICON != 'ON' }}
       run: |
diff --git a/CHANGELOG.md b/CHANGELOG.md
index b6ec2d73ede..b66d70dce68 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -9,6 +9,7 @@
     - NodeJS:
       - FIXED: Support `skip_waypoints` in Node bindings [#6060](https://github.com/Project-OSRM/osrm-backend/pull/6060)
     - Misc:
+      - CHANGED: Improve performance of map matching via getPathDistance optimization. [#6378](https://github.com/Project-OSRM/osrm-backend/pull/6378)
       - CHANGED: Optimize RestrictionParser performance. [#6344](https://github.com/Project-OSRM/osrm-backend/pull/6344)
       - ADDED: Support floats for speed value in traffic updates CSV. [#6327](https://github.com/Project-OSRM/osrm-backend/pull/6327)
       - CHANGED: Use Lua 5.4 in Docker image. [#6346](https://github.com/Project-OSRM/osrm-backend/pull/6346)
diff --git a/include/engine/routing_algorithms/routing_base.hpp b/include/engine/routing_algorithms/routing_base.hpp
index a416a4429cc..793ac29a814 100644
--- a/include/engine/routing_algorithms/routing_base.hpp
+++ b/include/engine/routing_algorithms/routing_base.hpp
@@ -353,49 +353,21 @@ double getPathDistance(const DataFacade<Algorithm> &facade,
                        const PhantomNode &source_phantom,
                        const PhantomNode &target_phantom)
 {
-    using util::coordinate_calculation::detail::DEGREE_TO_RAD;
-    using util::coordinate_calculation::detail::EARTH_RADIUS;
-
-    double distance = 0;
-    double prev_lat =
-        static_cast<double>(util::toFloating(source_phantom.location.lat)) * DEGREE_TO_RAD;
-    double prev_lon =
-        static_cast<double>(util::toFloating(source_phantom.location.lon)) * DEGREE_TO_RAD;
-    double prev_cos = std::cos(prev_lat);
+    double distance = 0.0;
+    auto prev_coordinate = source_phantom.location;
+
     for (const auto &p : unpacked_path)
     {
         const auto current_coordinate = facade.GetCoordinateOfNode(p.turn_via_node);
 
-        const double current_lat =
-            static_cast<double>(util::toFloating(current_coordinate.lat)) * DEGREE_TO_RAD;
-        const double current_lon =
-            static_cast<double>(util::toFloating(current_coordinate.lon)) * DEGREE_TO_RAD;
-        const double current_cos = std::cos(current_lat);
-
-        const double sin_dlon = std::sin((prev_lon - current_lon) / 2.0);
-        const double sin_dlat = std::sin((prev_lat - current_lat) / 2.0);
-
-        const double aharv = sin_dlat * sin_dlat + prev_cos * current_cos * sin_dlon * sin_dlon;
-        const double charv = 2. * std::atan2(std::sqrt(aharv), std::sqrt(1.0 - aharv));
-        distance += EARTH_RADIUS * charv;
+        distance +=
+            util::coordinate_calculation::greatCircleDistance(prev_coordinate, current_coordinate);
 
-        prev_lat = current_lat;
-        prev_lon = current_lon;
-        prev_cos = current_cos;
+        prev_coordinate = current_coordinate;
     }
 
-    const double current_lat =
-        static_cast<double>(util::toFloating(target_phantom.location.lat)) * DEGREE_TO_RAD;
-    const double current_lon =
-        static_cast<double>(util::toFloating(target_phantom.location.lon)) * DEGREE_TO_RAD;
-    const double current_cos = std::cos(current_lat);
-
-    const double sin_dlon = std::sin((prev_lon - current_lon) / 2.0);
-    const double sin_dlat = std::sin((prev_lat - current_lat) / 2.0);
-
-    const double aharv = sin_dlat * sin_dlat + prev_cos * current_cos * sin_dlon * sin_dlon;
-    const double charv = 2. * std::atan2(std::sqrt(aharv), std::sqrt(1.0 - aharv));
-    distance += EARTH_RADIUS * charv;
+    distance +=
+        util::coordinate_calculation::greatCircleDistance(prev_coordinate, target_phantom.location);
 
     return distance;
 }