From a6953a6e828f4aa6b53645ae3583e9717502b77d Mon Sep 17 00:00:00 2001
From: Guillaume Doisy <guillaume@dexory.com>
Date: Wed, 1 Mar 2023 13:35:51 +0000
Subject: [PATCH] Savitsky Golay Filter order 9

---
 nav2_mppi_controller/CMakeLists.txt           |   9 +-
 .../nav2_mppi_controller/optimizer.hpp        |   2 +-
 .../nav2_mppi_controller/tools/utils.hpp      | 109 +++++++++++++++---
 nav2_mppi_controller/src/optimizer.cpp        |   2 +
 4 files changed, 99 insertions(+), 23 deletions(-)

diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt
index 2b496d3d58..ab663c3224 100644
--- a/nav2_mppi_controller/CMakeLists.txt
+++ b/nav2_mppi_controller/CMakeLists.txt
@@ -75,14 +75,7 @@ install(DIRECTORY include/
   DESTINATION include/
 )
 
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  find_package(ament_cmake_gtest REQUIRED)
-  set(ament_cmake_copyright_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-  add_subdirectory(test)
-  # add_subdirectory(benchmark)
-endif()
+
 
 ament_export_libraries(${libraries})
 ament_export_dependencies(${dependencies_pkgs})
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp
index e1a518ae84..061654aa65 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp
@@ -248,7 +248,7 @@ class Optimizer
 
   models::State state_;
   models::ControlSequence control_sequence_;
-  std::array<mppi::models::Control, 2> control_history_;
+  std::array<mppi::models::Control, 4> control_history_;
   models::Trajectories generated_trajectories_;
   models::Path path_;
   xt::xtensor<float, 1> costs_;
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp
index 903d35e86f..1b06f478ca 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp
@@ -437,14 +437,14 @@ inline double posePointAngle(const geometry_msgs::msg::Pose & pose, double point
  */
 inline void savitskyGolayFilter(
   models::ControlSequence & control_sequence,
-  std::array<mppi::models::Control, 2> & control_history,
+  std::array<mppi::models::Control, 4> & control_history,
   const models::OptimizerSettings & settings)
 {
-  // Savitzky-Golay Quadratic, 5-point Coefficients
-  xt::xarray<float> filter = {-3.0, 12.0, 17.0, 12.0, -3.0};
-  filter /= 35.0;
+  // Savitzky-Golay Quadratic, 7-point Coefficients
+  xt::xarray<float> filter = {-21.0, 14.0, 39.0, 54.0, 59.0, 54.0, 39.0, 14.0, -21.0};
+  filter /= 231.0;
 
-  const unsigned int num_sequences = control_sequence.vx.shape(0);
+  const unsigned int num_sequences = control_sequence.vx.shape(0) - 1;
 
   // Too short to smooth meaningfully
   if (num_sequences < 10) {
@@ -456,64 +456,145 @@ inline void savitskyGolayFilter(
     };
 
   auto applyFilterOverAxis =
-    [&](xt::xtensor<float, 1> & sequence, const float hist_0, const float hist_1) -> void
+    [&](xt::xtensor<float, 1> & sequence,
+      const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void
     {
       unsigned int idx = 0;
       sequence(idx) = applyFilter(
       {
         hist_0,
         hist_1,
+        hist_2,
+        hist_3,
         sequence(idx),
         sequence(idx + 1),
-        sequence(idx + 2)});
+        sequence(idx + 2),
+        sequence(idx + 3),
+        sequence(idx + 4)});
 
       idx++;
       sequence(idx) = applyFilter(
       {
         hist_1,
+        hist_2,
+        hist_3,
         sequence(idx - 1),
         sequence(idx),
         sequence(idx + 1),
-        sequence(idx + 2)});
+        sequence(idx + 2),
+        sequence(idx + 3),
+        sequence(idx + 4)});
+
+      idx++;
+      sequence(idx) = applyFilter(
+      {
+        hist_2,
+        hist_3,
+        sequence(idx - 2),
+        sequence(idx - 1),
+        sequence(idx),
+        sequence(idx + 1),
+        sequence(idx + 2),
+        sequence(idx + 3),
+        sequence(idx + 4)});
 
-      for (idx = 2; idx != num_sequences - 3; idx++) {
+      idx++;
+      sequence(idx) = applyFilter(
+      {
+        hist_3,
+        sequence(idx - 3),
+        sequence(idx - 2),
+        sequence(idx - 1),
+        sequence(idx),
+        sequence(idx + 1),
+        sequence(idx + 2),
+        sequence(idx + 3),
+        sequence(idx + 4)});
+
+      for (idx = 4; idx != num_sequences - 4; idx++) {
         sequence(idx) = applyFilter(
         {
+          sequence(idx - 4),
+          sequence(idx - 3),
           sequence(idx - 2),
           sequence(idx - 1),
           sequence(idx),
           sequence(idx + 1),
-          sequence(idx + 2)});
+          sequence(idx + 2),
+          sequence(idx + 3),
+          sequence(idx + 4)});
       }
 
       idx++;
       sequence(idx) = applyFilter(
       {
+        sequence(idx - 4),
+        sequence(idx - 3),
         sequence(idx - 2),
         sequence(idx - 1),
         sequence(idx),
         sequence(idx + 1),
+        sequence(idx + 2),
+        sequence(idx + 3),
+        sequence(idx + 3)});
+
+      idx++;
+      sequence(idx) = applyFilter(
+      {
+        sequence(idx - 4),
+        sequence(idx - 3),
+        sequence(idx - 2),
+        sequence(idx - 1),
+        sequence(idx),
+        sequence(idx + 1),
+        sequence(idx + 2),
+        sequence(idx + 2),
+        sequence(idx + 2)});
+
+      idx++;
+      sequence(idx) = applyFilter(
+      {
+        sequence(idx - 4),
+        sequence(idx - 3),
+        sequence(idx - 2),
+        sequence(idx - 1),
+        sequence(idx),
+        sequence(idx + 1),
+        sequence(idx + 1),
+        sequence(idx + 1),
         sequence(idx + 1)});
 
       idx++;
       sequence(idx) = applyFilter(
       {
+        sequence(idx - 4),
+        sequence(idx - 3),
         sequence(idx - 2),
         sequence(idx - 1),
         sequence(idx),
         sequence(idx),
+        sequence(idx),
+        sequence(idx),
         sequence(idx)});
     };
 
   // Filter trajectories
-  applyFilterOverAxis(control_sequence.vx, control_history[0].vx, control_history[1].vx);
-  applyFilterOverAxis(control_sequence.vy, control_history[0].vy, control_history[1].vy);
-  applyFilterOverAxis(control_sequence.wz, control_history[0].wz, control_history[1].wz);
+  applyFilterOverAxis(
+    control_sequence.vx, control_history[0].vx,
+    control_history[1].vx, control_history[2].vx, control_history[3].vx);
+  applyFilterOverAxis(
+    control_sequence.vy, control_history[0].vy,
+    control_history[1].vy, control_history[2].vy, control_history[3].vy);
+  applyFilterOverAxis(
+    control_sequence.wz, control_history[0].wz,
+    control_history[1].wz, control_history[2].wz, control_history[3].wz);
 
   // Update control history
   unsigned int offset = settings.shift_control_sequence ? 1 : 0;
   control_history[0] = control_history[1];
-  control_history[1] = {
+  control_history[1] = control_history[2];
+  control_history[2] = control_history[3];
+  control_history[3] = {
     control_sequence.vx(offset),
     control_sequence.vy(offset),
     control_sequence.wz(offset)};
diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp
index 4adf555220..55362fd7a8 100644
--- a/nav2_mppi_controller/src/optimizer.cpp
+++ b/nav2_mppi_controller/src/optimizer.cpp
@@ -119,6 +119,8 @@ void Optimizer::reset()
   control_sequence_.reset(settings_.time_steps);
   control_history_[0] = {0.0, 0.0, 0.0};
   control_history_[1] = {0.0, 0.0, 0.0};
+  control_history_[2] = {0.0, 0.0, 0.0};
+  control_history_[3] = {0.0, 0.0, 0.0};
 
   costs_ = xt::zeros<float>({settings_.batch_size});
   generated_trajectories_.reset(settings_.batch_size, settings_.time_steps);