diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt index 79d0abc7b7c9b..0b1fc2517cbee 100644 --- a/localization/ekf_localizer/CMakeLists.txt +++ b/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) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index c9a7a0dc6014c..0c3fa0ae8c3f1 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/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 current_twist_info_queue_; //!< @brief current measured pose std::queue 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_ */ diff --git a/localization/ekf_localizer/include/ekf_localizer/matrix_types.hpp b/localization/ekf_localizer/include/ekf_localizer/matrix_types.hpp new file mode 100644 index 0000000000000..732abd6c8e2e7 --- /dev/null +++ b/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 + +using Vector6d = Eigen::Matrix; +using Matrix6d = Eigen::Matrix; + +#endif // EKF_LOCALIZER__MATRIX_TYPES_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/state_index.hpp b/localization/ekf_localizer/include/ekf_localizer/state_index.hpp new file mode 100644 index 0000000000000..b2bfdb0bd9e2e --- /dev/null +++ b/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_ diff --git a/localization/ekf_localizer/include/ekf_localizer/state_transition.hpp b/localization/ekf_localizer/include/ekf_localizer/state_transition.hpp new file mode 100644 index 0000000000000..85a65828e4ee4 --- /dev/null +++ b/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_ diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 41ae99015f871..00f6b760efaef 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/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 #include @@ -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; diff --git a/localization/ekf_localizer/src/state_transition.cpp b/localization/ekf_localizer/src/state_transition.cpp new file mode 100644 index 0000000000000..0f8bb091c3662 --- /dev/null +++ b/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 + +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; +} diff --git a/localization/ekf_localizer/test/test_state_transition.cpp b/localization/ekf_localizer/test/test_state_transition.cpp new file mode 100644 index 0000000000000..9cb7975a964a9 --- /dev/null +++ b/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 +#include + +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.); +}