refactor(ekf_localizer): isolate state transition functions (autoware…

…foundation#1758)

* Isolate state transition functions from the EKF class and add tests

localization/ekf_localizer/CMakeLists.txt

@@ -11,11 +11,39 @@ include_directories(
     ${EIGEN3_INCLUDE_DIR}
 )
 
-ament_auto_add_executable(ekf_localizer
-  src/ekf_localizer_node.cpp
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(ekf_localizer_lib SHARED
   src/ekf_localizer.cpp
+  src/state_transition.cpp
 )
-ament_target_dependencies(ekf_localizer kalman_filter)
+
+target_link_libraries(ekf_localizer_lib Eigen3::Eigen)
+
+ament_auto_add_executable(ekf_localizer src/ekf_localizer_node.cpp)
+target_link_libraries(ekf_localizer ekf_localizer_lib)
+target_include_directories(ekf_localizer PUBLIC include)
+
+function(add_testcase filepath)
+  get_filename_component(filename ${filepath} NAME)
+  string(REGEX REPLACE ".cpp" "" test_name ${filename})
+
+  ament_add_gtest(${test_name} ${filepath})
+  target_link_libraries("${test_name}" ekf_localizer_lib)
+  ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS})
+endfunction()
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+
+  set(TEST_FILES
+    test/test_state_transition.cpp)
+
+  foreach(filepath ${TEST_FILES})
+    add_testcase(${filepath})
+  endforeach()
+endif()
+
 
 # if(BUILD_TESTING)
 #   find_package(ament_cmake_ros REQUIRED)

localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp

@@ -198,15 +198,6 @@ class EKFLocalizer : public rclcpp::Node
 
   bool is_initialized_;
 
-  enum IDX {
-    X = 0,
-    Y = 1,
-    YAW = 2,
-    YAWB = 3,
-    VX = 4,
-    WZ = 5,
-  };
-
   /* for model prediction */
   std::queue<TwistInfo> current_twist_info_queue_;    //!< @brief current measured pose
   std::queue<PoseInfo> current_pose_info_queue_;      //!< @brief current measured pose
@@ -291,13 +282,6 @@ class EKFLocalizer : public rclcpp::Node
     std::string parent_frame, std::string child_frame,
     geometry_msgs::msg::TransformStamped & transform);
 
-  /**
-   * @brief normalize yaw angle
-   * @param yaw yaw angle
-   * @return normalized yaw
-   */
-  double normalizeYaw(const double & yaw) const;
-
   /**
    * @brief set current EKF estimation result to current_ekf_pose_ & current_ekf_twist_
    */

localization/ekf_localizer/include/ekf_localizer/matrix_types.hpp

@@ -0,0 +1,23 @@
+// Copyright 2022 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.
+
+#ifndef EKF_LOCALIZER__MATRIX_TYPES_HPP_
+#define EKF_LOCALIZER__MATRIX_TYPES_HPP_
+
+#include <Eigen/Core>
+
+using Vector6d = Eigen::Matrix<double, 6, 1>;
+using Matrix6d = Eigen::Matrix<double, 6, 6>;
+
+#endif  // EKF_LOCALIZER__MATRIX_TYPES_HPP_

localization/ekf_localizer/include/ekf_localizer/state_index.hpp

@@ -0,0 +1,27 @@
+// Copyright 2022 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.
+
+#ifndef EKF_LOCALIZER__STATE_INDEX_HPP_
+#define EKF_LOCALIZER__STATE_INDEX_HPP_
+
+enum IDX {
+  X = 0,
+  Y = 1,
+  YAW = 2,
+  YAWB = 3,
+  VX = 4,
+  WZ = 5,
+};
+
+#endif  // EKF_LOCALIZER__STATE_INDEX_HPP_

localization/ekf_localizer/include/ekf_localizer/state_transition.hpp

@@ -0,0 +1,26 @@
+// Copyright 2022 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.
+
+#ifndef EKF_LOCALIZER__STATE_TRANSITION_HPP_
+#define EKF_LOCALIZER__STATE_TRANSITION_HPP_
+
+#include "ekf_localizer/matrix_types.hpp"
+
+double normalizeYaw(const double & yaw);
+Vector6d predictNextState(const Vector6d & X_curr, const double dt);
+Matrix6d createStateTransitionMatrix(const Vector6d & X_curr, const double dt);
+Matrix6d processNoiseCovariance(
+  const double proc_cov_yaw_d, const double proc_cov_vx_d, const double proc_cov_wz_d);
+
+#endif  // EKF_LOCALIZER__STATE_TRANSITION_HPP_

localization/ekf_localizer/src/ekf_localizer.cpp

@@ -14,6 +14,10 @@
 
 #include "ekf_localizer/ekf_localizer.hpp"
 
+#include "ekf_localizer/matrix_types.hpp"
+#include "ekf_localizer/state_index.hpp"
+#include "ekf_localizer/state_transition.hpp"
+
 #include <rclcpp/logging.hpp>
 #include <tier4_autoware_utils/math/unit_conversion.hpp>
 
@@ -406,47 +410,17 @@ void EKFLocalizer::predictKinematicsModel()
    */
 
   Eigen::MatrixXd X_curr(dim_x_, 1);  // current state
-  Eigen::MatrixXd X_next(dim_x_, 1);  // predicted state
   ekf_.getLatestX(X_curr);
   DEBUG_PRINT_MAT(X_curr.transpose());
 
   Eigen::MatrixXd P_curr;
   ekf_.getLatestP(P_curr);
 
-  const double yaw = X_curr(IDX::YAW);
-  const double yaw_bias = X_curr(IDX::YAWB);
-  const double vx = X_curr(IDX::VX);
-  const double wz = X_curr(IDX::WZ);
   const double dt = ekf_dt_;
 
-  /* Update for latest state */
-  X_next(IDX::X) = X_curr(IDX::X) + vx * cos(yaw + yaw_bias) * dt;  // dx = v * cos(yaw)
-  X_next(IDX::Y) = X_curr(IDX::Y) + vx * sin(yaw + yaw_bias) * dt;  // dy = v * sin(yaw)
-  X_next(IDX::YAW) = X_curr(IDX::YAW) + (wz)*dt;                    // dyaw = omega + omega_bias
-  X_next(IDX::YAWB) = yaw_bias;
-  X_next(IDX::VX) = vx;
-  X_next(IDX::WZ) = wz;
-
-  X_next(IDX::YAW) = std::atan2(std::sin(X_next(IDX::YAW)), std::cos(X_next(IDX::YAW)));
-
-  /* Set A matrix for latest state */
-  Eigen::MatrixXd A = Eigen::MatrixXd::Identity(dim_x_, dim_x_);
-  A(IDX::X, IDX::YAW) = -vx * sin(yaw + yaw_bias) * dt;
-  A(IDX::X, IDX::YAWB) = -vx * sin(yaw + yaw_bias) * dt;
-  A(IDX::X, IDX::VX) = cos(yaw + yaw_bias) * dt;
-  A(IDX::Y, IDX::YAW) = vx * cos(yaw + yaw_bias) * dt;
-  A(IDX::Y, IDX::YAWB) = vx * cos(yaw + yaw_bias) * dt;
-  A(IDX::Y, IDX::VX) = sin(yaw + yaw_bias) * dt;
-  A(IDX::YAW, IDX::WZ) = dt;
-
-  Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(dim_x_, dim_x_);
-
-  Q(IDX::X, IDX::X) = 0.0;
-  Q(IDX::Y, IDX::Y) = 0.0;
-  Q(IDX::YAW, IDX::YAW) = proc_cov_yaw_d_;  // for yaw
-  Q(IDX::YAWB, IDX::YAWB) = 0.0;
-  Q(IDX::VX, IDX::VX) = proc_cov_vx_d_;  // for vx
-  Q(IDX::WZ, IDX::WZ) = proc_cov_wz_d_;  // for wz
+  const Vector6d X_next = predictNextState(X_curr, dt);
+  const Matrix6d A = createStateTransitionMatrix(X_curr, dt);
+  const Matrix6d Q = processNoiseCovariance(proc_cov_yaw_d_, proc_cov_vx_d_, proc_cov_wz_d_);
 
   ekf_.predictWithDelay(X_next, A, Q);
 
@@ -760,11 +734,6 @@ void EKFLocalizer::publishEstimateResult()
   pub_debug_->publish(msg);
 }
 
-double EKFLocalizer::normalizeYaw(const double & yaw) const
-{
-  return std::atan2(std::sin(yaw), std::cos(yaw));
-}
-
 void EKFLocalizer::updateSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose)
 {
   double z = pose.pose.pose.position.z;

localization/ekf_localizer/src/state_transition.cpp

@@ -0,0 +1,75 @@
+// Copyright 2022 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 "ekf_localizer/matrix_types.hpp"
+#include "ekf_localizer/state_index.hpp"
+
+#include <cmath>
+
+double normalizeYaw(const double & yaw)
+{
+  // FIXME(IshitaTakeshi) I think the computation here can be simplified
+  // FIXME(IshitaTakeshi) Rename the function. This is not normalization
+  return std::atan2(std::sin(yaw), std::cos(yaw));
+}
+
+Vector6d predictNextState(const Vector6d & X_curr, const double dt)
+{
+  const double x = X_curr(IDX::X);
+  const double y = X_curr(IDX::Y);
+  const double yaw = X_curr(IDX::YAW);
+  const double yaw_bias = X_curr(IDX::YAWB);
+  const double vx = X_curr(IDX::VX);
+  const double wz = X_curr(IDX::WZ);
+
+  Vector6d X_next;
+  X_next(IDX::X) = x + vx * std::cos(yaw + yaw_bias) * dt;  // dx = v * cos(yaw)
+  X_next(IDX::Y) = y + vx * std::sin(yaw + yaw_bias) * dt;  // dy = v * sin(yaw)
+  X_next(IDX::YAW) = normalizeYaw(yaw + wz * dt);           // dyaw = omega + omega_bias
+  X_next(IDX::YAWB) = yaw_bias;
+  X_next(IDX::VX) = vx;
+  X_next(IDX::WZ) = wz;
+  return X_next;
+}
+
+// TODO(TakeshiIshita) show where the equation come from
+Matrix6d createStateTransitionMatrix(const Vector6d & X_curr, const double dt)
+{
+  const double yaw = X_curr(IDX::YAW);
+  const double yaw_bias = X_curr(IDX::YAWB);
+  const double vx = X_curr(IDX::VX);
+
+  Matrix6d A = Matrix6d::Identity();
+  A(IDX::X, IDX::YAW) = -vx * sin(yaw + yaw_bias) * dt;
+  A(IDX::X, IDX::YAWB) = -vx * sin(yaw + yaw_bias) * dt;
+  A(IDX::X, IDX::VX) = cos(yaw + yaw_bias) * dt;
+  A(IDX::Y, IDX::YAW) = vx * cos(yaw + yaw_bias) * dt;
+  A(IDX::Y, IDX::YAWB) = vx * cos(yaw + yaw_bias) * dt;
+  A(IDX::Y, IDX::VX) = sin(yaw + yaw_bias) * dt;
+  A(IDX::YAW, IDX::WZ) = dt;
+  return A;
+}
+
+Matrix6d processNoiseCovariance(
+  const double proc_cov_yaw_d, const double proc_cov_vx_d, const double proc_cov_wz_d)
+{
+  Matrix6d Q = Matrix6d::Zero();
+  Q(IDX::X, IDX::X) = 0.0;
+  Q(IDX::Y, IDX::Y) = 0.0;
+  Q(IDX::YAW, IDX::YAW) = proc_cov_yaw_d;  // for yaw
+  Q(IDX::YAWB, IDX::YAWB) = 0.0;
+  Q(IDX::VX, IDX::VX) = proc_cov_vx_d;  // for vx
+  Q(IDX::WZ, IDX::WZ) = proc_cov_wz_d;  // for wz
+  return Q;
+}

localization/ekf_localizer/test/test_state_transition.cpp

@@ -0,0 +1,95 @@
+// Copyright 2018-2019 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.
+
+#define _USE_MATH_DEFINES
+#include "ekf_localizer/state_index.hpp"
+#include "ekf_localizer/state_transition.hpp"
+
+#include <gtest/gtest.h>
+#include <math.h>
+
+TEST(StateTransition, NormalizeYaw)
+{
+  const double tolerance = 1e-6;
+  EXPECT_NEAR(normalizeYaw(M_PI * 4 / 3), -M_PI * 2 / 3, tolerance);
+  EXPECT_NEAR(normalizeYaw(-M_PI * 4 / 3), M_PI * 2 / 3, tolerance);
+  EXPECT_NEAR(normalizeYaw(M_PI * 9 / 2), M_PI * 1 / 2, tolerance);
+  EXPECT_NEAR(normalizeYaw(M_PI * 4), M_PI * 0, tolerance);
+}
+
+TEST(PredictNextState, PredictNextState)
+{
+  // This function is the definition of state transition so we just check
+  // if the calculation is done according to the formula
+  Vector6d X_curr;
+  X_curr(0) = 2.;
+  X_curr(1) = 3.;
+  X_curr(2) = M_PI / 2.;
+  X_curr(3) = M_PI / 4.;
+  X_curr(4) = 10.;
+  X_curr(5) = 2. * M_PI / 3.;
+
+  const double dt = 0.5;
+
+  const Vector6d X_next = predictNextState(X_curr, dt);
+
+  const double tolerance = 1e-10;
+  EXPECT_NEAR(X_next(0), 2. + 10. * std::cos(M_PI / 2. + M_PI / 4.) * 0.5, tolerance);
+  EXPECT_NEAR(X_next(1), 3. + 10. * std::sin(M_PI / 2. + M_PI / 4.) * 0.5, tolerance);
+  EXPECT_NEAR(X_next(2), normalizeYaw(M_PI / 2. + M_PI / 3.), tolerance);
+  EXPECT_NEAR(X_next(3), X_curr(3), tolerance);
+  EXPECT_NEAR(X_next(4), X_curr(4), tolerance);
+  EXPECT_NEAR(X_next(5), X_curr(5), tolerance);
+}
+
+TEST(CreateStateTransitionMatrix, NumericalApproximation)
+{
+  // The transition matrix A = df / dx
+  // We check if df = A * dx approximates f(x + dx) - f(x)
+
+  {
+    // check around x = 0
+    const double dt = 0.1;
+    const Vector6d dx = 0.1 * Vector6d::Ones();
+    const Vector6d x = Vector6d::Zero();
+
+    const Matrix6d A = createStateTransitionMatrix(x, dt);
+    const Vector6d df = predictNextState(x + dx, dt) - predictNextState(x, dt);
+
+    EXPECT_LT((df - A * dx).norm(), 2e-3);
+  }
+
+  {
+    // check around random x
+    const double dt = 0.1;
+    const Vector6d dx = 0.1 * Vector6d::Ones();
+    const Vector6d x = (Vector6d() << 0.1, 0.2, 0.1, 0.4, 0.1, 0.3).finished();
+
+    const Matrix6d A = createStateTransitionMatrix(x, dt);
+    const Vector6d df = predictNextState(x + dx, dt) - predictNextState(x, dt);
+
+    EXPECT_LT((df - A * dx).norm(), 5e-3);
+  }
+}
+
+TEST(ProcessNoiseCovariance, ProcessNoiseCovariance)
+{
+  const Matrix6d Q = processNoiseCovariance(1., 2., 3.);
+  EXPECT_EQ(Q(2, 2), 1.);  // for yaw
+  EXPECT_EQ(Q(4, 4), 2.);  // for vx
+  EXPECT_EQ(Q(5, 5), 3.);  // for wz
+
+  // Make sure other elements are zero
+  EXPECT_EQ(processNoiseCovariance(0, 0, 0).norm(), 0.);
+}