feat(kalman_filter): add gtest (#4799)

* feat(kalman_filter): add gtest Signed-off-by: Cynthia Liu <[email protected]> * feat(kalman_filter): add gtest Signed-off-by: Cynthia Liu <[email protected]> * feat(kalman_filter): add gtest Signed-off-by: Cynthia Liu <[email protected]> * feat(kalman_filter): add gtest Signed-off-by: Cynthia Liu <[email protected]> * feat(kalman_filter): add gtest Signed-off-by: Cynthia Liu <[email protected]> * feat(kalman_filter): add gtest Signed-off-by: Cynthia Liu <[email protected]> * feat(kalman_filter): add gtest Signed-off-by: Cynthia Liu <[email protected]> * feat(kalman_filter): add gtest Signed-off-by: Cynthia Liu <[email protected]> * feat(kalman_filter): add gtest Signed-off-by: Cynthia Liu <[email protected]> * feat(kalman_filter): add gtest Signed-off-by: Cynthia Liu <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: Cynthia Liu <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Oct 11, 2023 · dc8cc07 · dc8cc07
1 parent 9fe7626
commit dc8cc07
Show file tree

Hide file tree

Showing 4 changed files with 224 additions and 0 deletions.
diff --git a/common/kalman_filter/CMakeLists.txt b/common/kalman_filter/CMakeLists.txt
@@ -19,4 +19,11 @@ ament_auto_add_library(kalman_filter SHARED
   include/kalman_filter/time_delay_kalman_filter.hpp
 )
 
+if(BUILD_TESTING)
+  file(GLOB_RECURSE test_files test/*.cpp)
+  ament_add_ros_isolated_gtest(test_kalman_filter ${test_files})
+
+  target_link_libraries(test_kalman_filter kalman_filter)
+endif()
+
 ament_auto_package()
diff --git a/common/kalman_filter/package.xml b/common/kalman_filter/package.xml
@@ -18,7 +18,9 @@
   <build_depend>eigen3_cmake_module</build_depend>
 
   <test_depend>ament_cmake_cppcheck</test_depend>
+  <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

diff --git a/common/kalman_filter/test/test_kalman_filter.cpp b/common/kalman_filter/test/test_kalman_filter.cpp
@@ -0,0 +1,94 @@
+// Copyright 2023 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "kalman_filter/kalman_filter.hpp"
+
+#include <gtest/gtest.h>
+
+TEST(kalman_filter, kf)
+{
+  KalmanFilter kf_;
+
+  Eigen::MatrixXd x_t(2, 1);
+  x_t << 1, 2;
+
+  Eigen::MatrixXd P_t(2, 2);
+  P_t << 1, 0, 0, 1;
+
+  Eigen::MatrixXd Q_t(2, 2);
+  Q_t << 0.01, 0, 0, 0.01;
+
+  Eigen::MatrixXd R_t(2, 2);
+  R_t << 0.09, 0, 0, 0.09;
+
+  Eigen::MatrixXd C_t(2, 2);
+  C_t << 1, 0, 0, 1;
+
+  Eigen::MatrixXd A_t(2, 2);
+  A_t << 1, 0, 0, 1;
+
+  Eigen::MatrixXd B_t(2, 2);
+  B_t << 1, 0, 0, 1;
+
+  // Initialize the filter and check if initialization was successful
+  EXPECT_TRUE(kf_.init(x_t, A_t, B_t, C_t, Q_t, R_t, P_t));
+
+  // Perform prediction
+  Eigen::MatrixXd u_t(2, 1);
+  u_t << 0.1, 0.1;
+  EXPECT_TRUE(kf_.predict(u_t));
+
+  // Check the updated state and covariance matrix
+  Eigen::MatrixXd x_predict_expected = A_t * x_t + B_t * u_t;
+  Eigen::MatrixXd P_predict_expected = A_t * P_t * A_t.transpose() + Q_t;
+
+  Eigen::MatrixXd x_predict;
+  kf_.getX(x_predict);
+  Eigen::MatrixXd P_predict;
+  kf_.getP(P_predict);
+
+  EXPECT_NEAR(x_predict(0, 0), x_predict_expected(0, 0), 1e-5);
+  EXPECT_NEAR(x_predict(1, 0), x_predict_expected(1, 0), 1e-5);
+  EXPECT_NEAR(P_predict(0, 0), P_predict_expected(0, 0), 1e-5);
+  EXPECT_NEAR(P_predict(1, 1), P_predict_expected(1, 1), 1e-5);
+
+  // Perform update
+  Eigen::MatrixXd y_t(2, 1);
+  y_t << 1.05, 2.05;
+  EXPECT_TRUE(kf_.update(y_t));
+
+  // Check the updated state and covariance matrix
+  const Eigen::MatrixXd PCT_t = P_predict_expected * C_t.transpose();
+  const Eigen::MatrixXd K_t = PCT_t * ((R_t + C_t * PCT_t).inverse());
+  const Eigen::MatrixXd y_pred = C_t * x_predict_expected;
+  Eigen::MatrixXd x_update_expected = x_predict_expected + K_t * (y_t - y_pred);
+  Eigen::MatrixXd P_update_expected = P_predict_expected - K_t * (C_t * P_predict_expected);
+
+  Eigen::MatrixXd x_update;
+  kf_.getX(x_update);
+  Eigen::MatrixXd P_update;
+  kf_.getP(P_update);
+
+  EXPECT_NEAR(x_update(0, 0), x_update_expected(0, 0), 1e-5);
+  EXPECT_NEAR(x_update(1, 0), x_update_expected(1, 0), 1e-5);
+  EXPECT_NEAR(P_update(0, 0), P_update_expected(0, 0), 1e-5);
+  EXPECT_NEAR(P_update(1, 1), P_update_expected(1, 1), 1e-5);
+}
+
+int main(int argc, char * argv[])
+{
+  testing::InitGoogleTest(&argc, argv);
+  bool result = RUN_ALL_TESTS();
+  return result;
+}
diff --git a/common/kalman_filter/test/test_time_delay_kalman_filter.cpp b/common/kalman_filter/test/test_time_delay_kalman_filter.cpp
@@ -0,0 +1,121 @@
+// Copyright 2023 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "kalman_filter/time_delay_kalman_filter.hpp"
+
+#include <gtest/gtest.h>
+
+TEST(time_delay_kalman_filter, td_kf)
+{
+  TimeDelayKalmanFilter td_kf_;
+
+  Eigen::MatrixXd x_t(3, 1);
+  x_t << 1.0, 2.0, 3.0;
+  Eigen::MatrixXd P_t(3, 3);
+  P_t << 0.1, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.3;
+  const int max_delay_step = 5;
+  const int dim_x = x_t.rows();
+  const int dim_x_ex = dim_x * max_delay_step;
+  // Initialize the filter
+  td_kf_.init(x_t, P_t, max_delay_step);
+
+  // Check if initialization was successful
+  Eigen::MatrixXd x_init = td_kf_.getLatestX();
+  Eigen::MatrixXd P_init = td_kf_.getLatestP();
+  Eigen::MatrixXd x_ex_t = Eigen::MatrixXd::Zero(dim_x_ex, 1);
+  Eigen::MatrixXd P_ex_t = Eigen::MatrixXd::Zero(dim_x_ex, dim_x_ex);
+  for (int i = 0; i < max_delay_step; ++i) {
+    x_ex_t.block(i * dim_x, 0, dim_x, 1) = x_t;
+    P_ex_t.block(i * dim_x, i * dim_x, dim_x, dim_x) = P_t;
+  }
+
+  EXPECT_EQ(x_init.rows(), 3);
+  EXPECT_EQ(x_init.cols(), 1);
+  EXPECT_EQ(P_init.rows(), 3);
+  EXPECT_EQ(P_init.cols(), 3);
+  EXPECT_NEAR(x_init(0, 0), x_t(0, 0), 1e-5);
+  EXPECT_NEAR(x_init(1, 0), x_t(1, 0), 1e-5);
+  EXPECT_NEAR(x_init(2, 0), x_t(2, 0), 1e-5);
+  EXPECT_NEAR(P_init(0, 0), P_t(0, 0), 1e-5);
+  EXPECT_NEAR(P_init(1, 1), P_t(1, 1), 1e-5);
+  EXPECT_NEAR(P_init(2, 2), P_t(2, 2), 1e-5);
+
+  // Define prediction parameters
+  Eigen::MatrixXd A_t(3, 3);
+  A_t << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0;
+  Eigen::MatrixXd Q_t(3, 3);
+  Q_t << 0.01, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 0.03;
+  Eigen::MatrixXd x_next(3, 1);
+  x_next << 2.0, 4.0, 6.0;
+
+  // Perform prediction
+  EXPECT_TRUE(td_kf_.predictWithDelay(x_next, A_t, Q_t));
+
+  // Check the prediction state and covariance matrix
+  Eigen::MatrixXd x_predict = td_kf_.getLatestX();
+  Eigen::MatrixXd P_predict = td_kf_.getLatestP();
+  Eigen::MatrixXd x_tmp = Eigen::MatrixXd::Zero(dim_x_ex, 1);
+  x_tmp.block(0, 0, dim_x, 1) = A_t * x_t;
+  x_tmp.block(dim_x, 0, dim_x_ex - dim_x, 1) = x_ex_t.block(0, 0, dim_x_ex - dim_x, 1);
+  x_ex_t = x_tmp;
+  Eigen::MatrixXd x_predict_expected = x_ex_t.block(0, 0, dim_x, 1);
+  Eigen::MatrixXd P_tmp = Eigen::MatrixXd::Zero(dim_x_ex, dim_x_ex);
+  P_tmp.block(0, 0, dim_x, dim_x) = A_t * P_ex_t.block(0, 0, dim_x, dim_x) * A_t.transpose() + Q_t;
+  P_tmp.block(0, dim_x, dim_x, dim_x_ex - dim_x) =
+    A_t * P_ex_t.block(0, 0, dim_x, dim_x_ex - dim_x);
+  P_tmp.block(dim_x, 0, dim_x_ex - dim_x, dim_x) =
+    P_ex_t.block(0, 0, dim_x_ex - dim_x, dim_x) * A_t.transpose();
+  P_tmp.block(dim_x, dim_x, dim_x_ex - dim_x, dim_x_ex - dim_x) =
+    P_ex_t.block(0, 0, dim_x_ex - dim_x, dim_x_ex - dim_x);
+  P_ex_t = P_tmp;
+  Eigen::MatrixXd P_predict_expected = P_ex_t.block(0, 0, dim_x, dim_x);
+  EXPECT_NEAR(x_predict(0, 0), x_predict_expected(0, 0), 1e-5);
+  EXPECT_NEAR(x_predict(1, 0), x_predict_expected(1, 0), 1e-5);
+  EXPECT_NEAR(x_predict(2, 0), x_predict_expected(2, 0), 1e-5);
+  EXPECT_NEAR(P_predict(0, 0), P_predict_expected(0, 0), 1e-5);
+  EXPECT_NEAR(P_predict(1, 1), P_predict_expected(1, 1), 1e-5);
+  EXPECT_NEAR(P_predict(2, 2), P_predict_expected(2, 2), 1e-5);
+
+  // Define update parameters
+  Eigen::MatrixXd C_t(3, 3);
+  C_t << 0.5, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.5;
+  Eigen::MatrixXd R_t(3, 3);
+  R_t << 0.001, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.003;
+  Eigen::MatrixXd y_t(3, 1);
+  y_t << 1.05, 2.05, 3.05;
+  const int delay_step = 2;  // Choose an appropriate delay step
+  const int dim_y = y_t.rows();
+
+  // Perform update
+  EXPECT_TRUE(td_kf_.updateWithDelay(y_t, C_t, R_t, delay_step));
+
+  // Check the updated state and covariance matrix
+  Eigen::MatrixXd x_update = td_kf_.getLatestX();
+  Eigen::MatrixXd P_update = td_kf_.getLatestP();
+
+  Eigen::MatrixXd C_ex_t = Eigen::MatrixXd::Zero(dim_y, dim_x_ex);
+  const Eigen::MatrixXd PCT_t = P_ex_t * C_ex_t.transpose();
+  const Eigen::MatrixXd K_t = PCT_t * ((R_t + C_ex_t * PCT_t).inverse());
+  const Eigen::MatrixXd y_pred = C_ex_t * x_ex_t;
+  x_ex_t = x_ex_t + K_t * (y_t - y_pred);
+  P_ex_t = P_ex_t - K_t * (C_ex_t * P_ex_t);
+  Eigen::MatrixXd x_update_expected = x_ex_t.block(0, 0, dim_x, 1);
+  Eigen::MatrixXd P_update_expected = P_ex_t.block(0, 0, dim_x, dim_x);
+  EXPECT_NEAR(x_update(0, 0), x_update_expected(0, 0), 1e-5);
+  EXPECT_NEAR(x_update(1, 0), x_update_expected(1, 0), 1e-5);
+  EXPECT_NEAR(x_update(2, 0), x_update_expected(2, 0), 1e-5);
+  EXPECT_NEAR(P_update(0, 0), P_update_expected(0, 0), 1e-5);
+  EXPECT_NEAR(P_update(1, 1), P_update_expected(1, 1), 1e-5);
+  EXPECT_NEAR(P_update(2, 2), P_update_expected(2, 2), 1e-5);
+}