diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt
deleted file mode 100644
index b53457b0215ba..0000000000000
--- a/localization/ekf_localizer/CMakeLists.txt
+++ /dev/null
@@ -1,82 +0,0 @@
-cmake_minimum_required(VERSION 3.0.2)
-project(ekf_localizer)
-
-add_compile_options(-std=c++14)
-
-find_package(catkin REQUIRED
- roscpp
- std_msgs
- tf2
- tf2_ros
- geometry_msgs
- sensor_msgs
- rostest
- rosunit
-)
-
-find_package(Eigen3 REQUIRED)
-
-catkin_package(
- INCLUDE_DIRS include
- CATKIN_DEPENDS
- roscpp
- std_msgs
- tf2
- tf2_ros
- geometry_msgs
- sensor_msgs
-)
-
-###########
-## Build ##
-###########
-
-include_directories(
- include
- ${catkin_INCLUDE_DIRS}
- ${EIGEN3_INCLUDE_DIR}
-)
-
-add_executable(ekf_localizer
-src/ekf_localizer_node.cpp
-src/ekf_localizer.cpp
-src/kalman_filter/kalman_filter.cpp
-src/kalman_filter/time_delay_kalman_filter.cpp)
-add_dependencies(ekf_localizer ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(ekf_localizer ${catkin_LIBRARIES})
-
-#############
-## Install ##
-#############
-
-## Install executables and/or libraries
-install(TARGETS ekf_localizer
- ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
- LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
- RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
-
-## Install project namespaced headers
-install(DIRECTORY include/ekf_localizer/
- DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
-
-install(
- DIRECTORY
- launch
- DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-
-#############
-## Testing ##
-#############
-
-if(CATKIN_ENABLE_TESTING)
- find_package(rostest REQUIRED)
-
- add_rostest_gtest(ekf_localizer-test test/test_ekf_localizer.test
- test/src/test_ekf_localizer.cpp
- src/ekf_localizer.cpp
- src/kalman_filter/kalman_filter.cpp
- src/kalman_filter/time_delay_kalman_filter.cpp)
-
- target_link_libraries(ekf_localizer-test ${catkin_LIBRARIES})
-endif()
diff --git a/localization/ekf_localizer/README.md b/localization/ekf_localizer/README.md
deleted file mode 100644
index 0411d5228d577..0000000000000
--- a/localization/ekf_localizer/README.md
+++ /dev/null
@@ -1,222 +0,0 @@
-# Overview
-
-The **Extend Kalman Filter Localizer** estimates robust and less noisy robot pose and twist by integrating the 2D vehicle dynamics model with input ego-pose and ego-twist messages. The algorithm is designed especially for fast moving robot such as autonomous driving system.
-
-
-## Flowchart
-
-The overall flowchart of the ekf_localizer is described below.
-
-
- 
-
-
-
-## Features
-This package includes the following features:
-
- - **Time delay compensation** for input messages, which enables proper integration of input information with varying time delay. This is important especially for high speed moving robot, such as autonomous driving vehicle. (see following figure).
-- **Automatic estimation of yaw bias** prevents modeling errors caused by sensor mounting angle errors, which can improve estimation accuracy.
-- **Mahalanobis distance gate** enables probabilistic outlier detection to determine which inputs should be used or ignored.
-- **Smooth update**, the Kalman Filter measurement update is typically performed when a measurement is obtained, but it can cause large changes in the estimated value especially for low frequency measurements. Since the algorithm can consider the measurement time, the measurement data can be divided into multiple pieces and integrated smoothly while maintaining consistency (see following figure).
-
-
-
-
-
-
-####
-
-
- 
-
-
-# Launch
-
-The `ekf_localizer` starts with the default parameters with the following command.
-
-```
-roslaunch ekf_localizer ekf_localizer.launch
-```
-
-The parameters and input topic names can be set in the `ekf_localizer.launch` file.
-
-
-# Node
-
-## Subscribed Topics
- - measured_pose_with_covariance (geometry_msgs/PoseWithCovarianceStamped)
-
- Input pose source with measurement covariance matrix, used when `use_pose_with_covariance` is true.
-
- - measured_twist_with_covariance (geometry_msgs/PoseWithCovarianceStamped)
-
- Input twist source with measurement covariance matrix, used when `use_twist_with_covariance` is true.
-
- - measured_pose (geometry_msgs/PoseStamped)
-
- Input pose source, used when `use_pose_with_covariance` is false.
-
- - measured_twist (geometry_msgs/TwistStamped)
-
- Input twist source, used when `use_twist_with_covariance` is false.
-
- - initialpose (geometry_msgs/PoseWithCovarianceStamped)
-
- Initial pose for EKF. The estimated pose is initialized with zeros at start. It is initialized with this message whenever published.
-
-
-## Published Topics
-
- - ekf_pose (geometry_msgs/PoseStamped)
-
- Estimated pose.
-
- - ekf_pose_with_covariance (geometry_msgs/PoseWithCovarianceStamped)
-
- Estimated pose with covariance.
- - ekf_pose_with_covariance (geometry_msgs/PoseStamped)
-
- Estimated pose without yawbias effect.
-
- - ekf_pose_with_covariance_without_yawbias (geometry_msgs/PoseWithCovarianceStamped)
-
- Estimated pose with covariance without yawbias effect.
-
- - ekf_twist (geometry_msgs/TwistStamped)
-
- Estimated twist.
-
- - ekf_twist_with_covariance (geometry_msgs/TwistWithCovarianceStamped)
-
- Estimated twist with covariance.
-
-
-## Published TF
-
- - base_link
-
- TF from "map" coordinate to estimated pose.
-
-
-# Functions
-
-
-## Predict
-
-The current robot state is predicted from previously estimated data using a given prediction model. This calculation is called at constant interval (`predict_frequency [Hz]`). The prediction equation is described at the end of this page.
-
-
-## Measurement Update
-
-Before update, the Mahalanobis distance is calculated between the measured input and the predicted state, the measurement update is not performed for inputs where the Mahalanobis distance exceeds the given threshold.
-
-The predicted state is updated with the latest measured inputs, measured_pose and measured_twist. The updates are performed with the same frequency as prediction, usually at a high frequency, in order to enable smooth state estimation.
-
-
-
-
-
-# Parameter description
-
-The parameters are set in `launch/ekf_localizer.launch` .
-
-
-## For Node
-
-|Name|Type|Description|Default value|
-|:---|:---|:---|:---|
-|show_debug_info|bool|Flag to display debug info|false|
-|predict_frequency|double|Frequency for filtering and publishing [Hz]|50.0|
-|tf_rate|double|Frqcuency for tf broadcasting [Hz]|10.0|
-|extend_state_step|int|Max delay step which can be dealt with in EKF. Large number increases computational cost. |50|
-|enable_yaw_bias_estimation| bool |Flag to enable yaw bias estimation|true|
-
-## For pose measurement
-
-|Name|Type|Description|Default value|
-|:---|:---|:---|:---|
-|pose_additional_delay|double|Additional delay time for pose measurement [s]|0.0|
-|pose_measure_uncertainty_time|double|Measured time uncertainty used for covariance calculation [s]|0.01|
-|pose_rate|double|Approximated input pose rate used for covariance calculation [Hz]|10.0|
-|pose_gate_dist|double|Limit of Mahalanobis distance used for outliers detection|10000.0|
-|use_pose_with_covariance|bool|Flag to use covariance in pose_with_covarianve message|false|
-|pose_stddev_x|double|Standard deviation for pose position x [m] (used when use_pose_with_covariance is false)|0.05|
-|pose_stddev_y|double|Standard deviation for pose position y [m] (used when use_pose_with_covariance is false)|0.05|
-|pose_stddev_yaw|double|Standard deviation for pose yaw angle [rad] (used when use_pose_with_covariance is false)|0.025|
-
-## For twist measurement
-|Name|Type|Description|Default value|
-|:---|:---|:---|:---|
-|twist_additional_delay|double|Additional delay time for twist [s]|0.0|
-|twist_rate|double|Approximated input twist rate used for covariance calculation [Hz]|10.0|
-|twist_gate_dist|double|Limit of Mahalanobis distance used for outliers detection|10000.0|
-|use_twist_with_covariance|bool|Flag to use covariance in twist_with_covariance message|false|
-|twist_stddev_vx|double|Standard deviation for twist linear x [m/s] (used when use_twist_with_covariance is false) |0.2|
-|twist_stddev_wz|double|Standard deviation for twist angular z [rad/s] (used when use_twist_with_covariance is false) |0.03|
-
-## For process noise
-|Name|Type|Description|Default value|
-|:---|:---|:---|:---|
-|proc_stddev_vx_c|double|Standard deviation of process noise in time differentiation expression of linear velocity x, noise for d_vx = 0|2.0|
-|proc_stddev_wz_c|double|Standard deviation of process noise in time differentiation expression of angular velocity z, noise for d_wz = 0|0.2|
-|proc_stddev_yaw_c|double|Standard deviation of process noise in time differentiation expression of yaw, noise for d_yaw = omege |0.005|
-|proc_stddev_yaw_bias_c|double|Standard deviation of process noise in time differentiation expression of yaw_bias, noise for d_yaw_bias = 0|0.001|
-
-note: process noise for position x & y are calculated automatically from nonlinear dynamics.
-
-# How to turn EKF parameters
-
-**0. Preliminaries**
- - Check header time in pose and twist message is set to sensor time appropriately, because time delay is calculated from this value. If it is difficult to set appropriate time due to timer synchronization problem, use `twist_additional_delay` and `pose_additional_delay` to correct the time.
- - Check the relation between measurement pose and twist is appropriate (whether the derivative of pose has similar value to twist). This discrepancy is caused mainly by unit error (such as comfusing radian/degree) or bias noise, and it causes large estimation errors.
-
-
-**1. Set sensor parameters**
-
-Set sensor-rate and standard-deviation from the basic information of the sensor. The `pose_measure_uncertainty_time` is for uncertainty of the header timestamp data.
-
- - `pose_measure_uncertainty_time`
- - `pose_rate`
- - `pose_stddev_x`
- - `pose_stddev_y`
- - `pose_stddev_yaw`
- - `twist_rate`
- - `twist_stddev_vx`
- - `twist_stddev_wz`
-
-**2. Set process model parameters**
-
-
- - `proc_stddev_vx_c` : set to maximum linear acceleration
- - `proc_stddev_wz_c` : set to maximum angular acceleration
- - `proc_stddev_yaw_c` : This parameter describes the correlation between the yaw and yaw-rate. Large value means the change in yaw does not correlate to the estiamted yaw-rate. If this is set to 0, it means the change in estimate yaw is equal to yaw-rate. Usually this should be set to 0.
- - `proc_stddev_yaw_bias_c` : This parameter is the standard deviation for the rate of change in yaw bias. In most cases, yaw bias is constant, so it can be very small, but must be non-zero.
-
- **3. Tune sensor standard deviation parameters with rosbag simulation.**
-
- If the position measurement seems more reliable, make these parameters smaller. If the estimated position seems to be noisy due to pose measurement noise, make these values bigger.
-
- - `pose_stddev_x`
- - `pose_stddev_y`
- - `pose_stddev_yaw`
-
- If the twist measurement seems more reliable, make these parameters smaller. If the estimated twist seems to be noisy due to pose measurement noise, make these values bigger.
- - `twist_stddev_vx`
- - `twist_stddev_wz`
-
-# Kalman Filter Model
-
-## kinematics model in update function
-
-
-where `b_k` is the yaw-bias.
-
-## time delay model
-
-
-# Test Result with Autoware NDT
-
-
-
-
diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.h b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.h
deleted file mode 100644
index 6a319608ea742..0000000000000
--- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.h
+++ /dev/null
@@ -1,233 +0,0 @@
-/*
- * Copyright 2018-2019 Autoware Foundation. All rights reserved.
- *
- * 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
-#include
-#include
-#include
-
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-
-#include "kalman_filter/kalman_filter.hpp"
-#include "kalman_filter/time_delay_kalman_filter.hpp"
-
-class EKFLocalizer
-{
-public:
- EKFLocalizer();
- ~EKFLocalizer();
-
-private:
- ros::NodeHandle nh_; //!< @brief ros public node handler
- ros::NodeHandle pnh_; //!< @brief ros private node handler
- ros::Publisher pub_pose_; //!< @brief ekf estimated pose publisher
- ros::Publisher pub_pose_cov_; //!< @brief estimated ekf pose with covariance publisher
- ros::Publisher pub_twist_; //!< @brief ekf estimated twist publisher
- ros::Publisher pub_twist_cov_; //!< @brief ekf estimated twist with covariance publisher
- ros::Publisher pub_debug_; //!< @brief debug info publisher
- ros::Publisher pub_measured_pose_; //!< @brief debug measurement pose publisher
- ros::Publisher pub_yaw_bias_; //!< @brief ekf estimated yaw bias publisher
- ros::Publisher pub_pose_no_yawbias_; //!< @brief ekf estimated yaw bias publisher
- ros::Publisher pub_pose_cov_no_yawbias_; //!< @brief ekf estimated yaw bias publisher
- ros::Subscriber sub_initialpose_; //!< @brief initial pose subscriber
- ros::Subscriber sub_pose_; //!< @brief measurement pose subscriber
- ros::Subscriber sub_twist_; //!< @brief measurement twist subscriber
- ros::Subscriber sub_pose_with_cov_; //!< @brief measurement pose with covariance subscriber
- ros::Subscriber sub_twist_with_cov_; //!< @brief measurement twist with covariance subscriber
- ros::Timer timer_control_; //!< @brief time for ekf calculation callback
- ros::Timer timer_tf_; //!< @brief timer to send transform
- tf2_ros::TransformBroadcaster tf_br_; //!< @brief tf broadcaster
-
- TimeDelayKalmanFilter ekf_; //!< @brief extended kalman filter instance.
-
- /* parameters */
- bool show_debug_info_;
- double ekf_rate_; //!< @brief EKF predict rate
- double ekf_dt_; //!< @brief = 1 / ekf_rate_
- double tf_rate_; //!< @brief tf publish rate
- bool
- enable_yaw_bias_estimation_; //!< @brief for LiDAR mount error. if true, publish /estimate_yaw_bias
- std::string pose_frame_id_;
-
- int dim_x_; //!< @brief dimension of EKF state
- int extend_state_step_; //!< @brief for time delay compensation
- int dim_x_ex_; //!< @brief dimension of extended EKF state (dim_x_ * extended_state_step)
-
- /* Pose */
- double
- pose_additional_delay_; //!< @brief compensated pose delay time = (pose.header.stamp - now) +
- //!< additional_delay [s]
- double pose_measure_uncertainty_time_; //!< @brief added for measurement covariance
- double pose_rate_; //!< @brief pose rate [s], used for covariance calculation
- double
- pose_gate_dist_; //!< @brief the maharanobis distance threshold to ignore pose measurement
- double pose_stddev_x_; //!< @brief standard deviation for pose position x [m]
- double pose_stddev_y_; //!< @brief standard deviation for pose position y [m]
- double pose_stddev_yaw_; //!< @brief standard deviation for pose position yaw [rad]
- bool use_pose_with_covariance_; //!< @brief use covariance in pose_with_covarianve message
- bool use_twist_with_covariance_; //!< @brief use covariance in twist_with_covarianve message
-
- /* twist */
- double
- twist_additional_delay_; //!< @brief compensated delay = (twist.header.stamp - now) + additional_delay [s]
- double twist_rate_; //!< @brief rate [s], used for covariance calculation
- double
- twist_gate_dist_; //!< @brief measurement is ignored if the maharanobis distance is larger than this value.
- double twist_stddev_vx_; //!< @brief standard deviation for linear vx
- double twist_stddev_wz_; //!< @brief standard deviation for angular wx
-
- /* process noise variance for discrete model */
- double proc_cov_yaw_d_; //!< @brief discrete yaw process noise
- double proc_cov_yaw_bias_d_; //!< @brief discrete yaw bias process noise
- double proc_cov_vx_d_; //!< @brief discrete process noise in d_vx=0
- double proc_cov_wz_d_; //!< @brief discrete process noise in d_wz=0
-
- enum IDX {
- X = 0,
- Y = 1,
- YAW = 2,
- YAWB = 3,
- VX = 4,
- WZ = 5,
- };
-
- /* for model prediction */
- std::shared_ptr
- current_twist_ptr_; //!< @brief current measured twist
- std::shared_ptr current_pose_ptr_; //!< @brief current measured pose
- geometry_msgs::PoseStamped current_ekf_pose_; //!< @brief current estimated pose
- geometry_msgs::PoseStamped
- current_ekf_pose_no_yawbias_; //!< @brief current estimated pose w/o yaw bias
- geometry_msgs::TwistStamped current_ekf_twist_; //!< @brief current estimated twist
- boost::array current_pose_covariance_;
- boost::array current_twist_covariance_;
-
- /**
- * @brief computes update & prediction of EKF for each ekf_dt_[s] time
- */
- void timerCallback(const ros::TimerEvent & e);
-
- /**
- * @brief publish tf for tf_rate [Hz]
- */
- void timerTFCallback(const ros::TimerEvent & e);
-
- /**
- * @brief set pose measurement
- */
- void callbackPose(const geometry_msgs::PoseStamped::ConstPtr & msg);
-
- /**
- * @brief set twist measurement
- */
- void callbackTwist(const geometry_msgs::TwistStamped::ConstPtr & msg);
-
- /**
- * @brief set poseWithCovariance measurement
- */
- void callbackPoseWithCovariance(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr & msg);
-
- /**
- * @brief set twistWithCovariance measurement
- */
- void callbackTwistWithCovariance(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr & msg);
-
- /**
- * @brief set initial_pose to current EKF pose
- */
- void callbackInitialPose(const geometry_msgs::PoseWithCovarianceStamped & msg);
-
- /**
- * @brief initialization of EKF
- */
- void initEKF();
-
- /**
- * @brief compute EKF prediction
- */
- void predictKinematicsModel();
-
- /**
- * @brief compute EKF update with pose measurement
- * @param pose measurement value
- */
- void measurementUpdatePose(const geometry_msgs::PoseStamped & pose);
-
- /**
- * @brief compute EKF update with pose measurement
- * @param twist measurement value
- */
- void measurementUpdateTwist(const geometry_msgs::TwistStamped & twist);
-
- /**
- * @brief check whether a measurement value falls within the mahalanobis distance threshold
- * @param dist_max mahalanobis distance threshold
- * @param estimated current estimated state
- * @param measured measured state
- * @param estimated_cov current estimation covariance
- * @return whether it falls within the mahalanobis distance threshold
- */
- bool mahalanobisGate(
- const double & dist_max, const Eigen::MatrixXd & estimated, const Eigen::MatrixXd & measured,
- const Eigen::MatrixXd & estimated_cov) const;
-
- /**
- * @brief get transform from frame_id
- */
- bool getTransformFromTF(
- std::string parent_frame, std::string child_frame, geometry_msgs::TransformStamped & transform);
-
- /**
- * @brief normalize yaw angle
- * @param yaw yaw angle
- * @return normalized yaw
- */
- double normalizeYaw(const double & yaw) const;
-
- /**
- * @brief create quaternion from roll, pitch and yaw.
- */
- geometry_msgs::Quaternion createQuaternionFromRPY(double r, double p, double y) const;
-
- /**
- * @brief set current EKF estimation result to current_ekf_pose_ & current_ekf_twist_
- */
- void setCurrentResult();
-
- /**
- * @brief publish current EKF estimation result
- */
- void publishEstimateResult();
-
- /**
- * @brief for debug
- */
- void showCurrentX();
-
- friend class EKFLocalizerTestSuite; // for test code
-};
diff --git a/localization/ekf_localizer/include/kalman_filter/kalman_filter.hpp b/localization/ekf_localizer/include/kalman_filter/kalman_filter.hpp
deleted file mode 100644
index b2a538f2b8a5f..0000000000000
--- a/localization/ekf_localizer/include/kalman_filter/kalman_filter.hpp
+++ /dev/null
@@ -1,208 +0,0 @@
-/*
- * Copyright 2018-2019 Autoware Foundation. All rights reserved.
- *
- * 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.
- */
-
-#pragma once
-
-#include
-#include
-
-/**
- * @file kalman_filter.h
- * @brief kalman filter class
- * @author Takamasa Horibe
- * @date 2019.05.01
- */
-
-class KalmanFilter
-{
-public:
- /**
- * @brief No initialization constructor.
- */
- KalmanFilter();
-
- /**
- * @brief constructor with initialization
- * @param x initial state
- * @param A coefficient matrix of x for process model
- * @param B coefficient matrix of u for process model
- * @param C coefficient matrix of x for measurement model
- * @param Q covariace matrix for process model
- * @param R covariance matrix for measurement model
- * @param P initial covariance of estimated state
- */
- KalmanFilter(
- const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B,
- const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R,
- const Eigen::MatrixXd & P);
-
- /**
- * @brief destructor
- */
- ~KalmanFilter();
-
- /**
- * @brief initialization of kalman filter
- * @param x initial state
- * @param A coefficient matrix of x for process model
- * @param B coefficient matrix of u for process model
- * @param C coefficient matrix of x for measurement model
- * @param Q covariace matrix for process model
- * @param R covariance matrix for measurement model
- * @param P initial covariance of estimated state
- */
- bool init(
- const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B,
- const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R,
- const Eigen::MatrixXd & P);
-
- /**
- * @brief initialization of kalman filter
- * @param x initial state
- * @param P initial covariance of estimated state
- */
- bool init(const Eigen::MatrixXd & x, const Eigen::MatrixXd & P0);
-
- /**
- * @brief set A of process model
- * @param A coefficient matrix of x for process model
- */
- void setA(const Eigen::MatrixXd & A);
-
- /**
- * @brief set B of process model
- * @param B coefficient matrix of u for process model
- */
- void setB(const Eigen::MatrixXd & B);
-
- /**
- * @brief set C of measurement model
- * @param C coefficient matrix of x for measurement model
- */
- void setC(const Eigen::MatrixXd & C);
-
- /**
- * @brief set covariace matrix Q for process model
- * @param Q covariace matrix for process model
- */
- void setQ(const Eigen::MatrixXd & Q);
-
- /**
- * @brief set covariance matrix R for measurement model
- * @param R covariance matrix for measurement model
- */
- void setR(const Eigen::MatrixXd & R);
-
- /**
- * @brief get current kalman filter state
- * @param x kalman filter state
- */
- void getX(Eigen::MatrixXd & x);
-
- /**
- * @brief get current kalman filter covariance
- * @param P kalman filter covariance
- */
- void getP(Eigen::MatrixXd & P);
-
- /**
- * @brief get component of current kalman filter state
- * @param i index of kalman filter state
- * @return value of i's component of the kalman filter state x[i]
- */
- double getXelement(unsigned int i);
-
- /**
- * @brief calculate kalman filter state and covariance by prediction model with A, B, Q matrix. This is mainly for EKF
- * with variable matrix.
- * @param u input for model
- * @param A coefficient matrix of x for process model
- * @param B coefficient matrix of u for process model
- * @param Q covariace matrix for process model
- * @return bool to check matrix operations are being performed properly
- */
- bool predict(
- const Eigen::MatrixXd & u, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B,
- const Eigen::MatrixXd & Q);
-
- /**
- * @brief calculate kalman filter covariance with prediction model with x, A, Q matrix. This is mainly for EKF with
- * variable matrix.
- * @param x_next predicted state
- * @param A coefficient matrix of x for process model
- * @param Q covariace matrix for process model
- * @return bool to check matrix operations are being performed properly
- */
- bool predict(
- const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q);
-
- /**
- * @brief calculate kalman filter covariance with prediction model with x, A, Q matrix. This is mainly for EKF with
- * variable matrix.
- * @param x_next predicted state
- * @param A coefficient matrix of x for process model
- * @return bool to check matrix operations are being performed properly
- */
- bool predict(const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A);
-
- /**
- * @brief calculate kalman filter state by prediction model with A, B and Q being class menber variables.
- * @param u input for the model
- * @return bool to check matrix operations are being performed properly
- */
- bool predict(const Eigen::MatrixXd & u);
-
- /**
- * @brief calculate kalman filter state by measurement model with y_pred, C and R matrix. This is mainly for EKF with
- * variable matrix.
- * @param y measured values
- * @param y output values expected from measurement model
- * @param C coefficient matrix of x for measurement model
- * @param R covariance matrix for measurement model
- * @return bool to check matrix operations are being performed properly
- */
- bool update(
- const Eigen::MatrixXd & y, const Eigen::MatrixXd & y_pred, const Eigen::MatrixXd & C,
- const Eigen::MatrixXd & R);
-
- /**
- * @brief calculate kalman filter state by measurement model with C and R matrix. This is mainly for EKF with variable
- * matrix.
- * @param y measured values
- * @param C coefficient matrix of x for measurement model
- * @param R covariance matrix for measurement model
- * @return bool to check matrix operations are being performed properly
- */
- bool update(const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R);
-
- /**
- * @brief calculate kalman filter state by measurement model with C and R being class menber variables.
- * @param y measured values
- * @return bool to check matrix operations are being performed properly
- */
- bool update(const Eigen::MatrixXd & y);
-
-protected:
- Eigen::MatrixXd x_; //!< @brief current estimated state
- Eigen::MatrixXd
- A_; //!< @brief coefficient matrix of x for process model x[k+1] = A*x[k] + B*u[k]
- Eigen::MatrixXd
- B_; //!< @brief coefficient matrix of u for process model x[k+1] = A*x[k] + B*u[k]
- Eigen::MatrixXd C_; //!< @brief coefficient matrix of x for measurement model y[k] = C * x[k]
- Eigen::MatrixXd Q_; //!< @brief covariace matrix for process model x[k+1] = A*x[k] + B*u[k]
- Eigen::MatrixXd R_; //!< @brief covariance matrix for measurement model y[k] = C * x[k]
- Eigen::MatrixXd P_; //!< @brief covariance of estimated state
-};
\ No newline at end of file
diff --git a/localization/ekf_localizer/include/kalman_filter/time_delay_kalman_filter.hpp b/localization/ekf_localizer/include/kalman_filter/time_delay_kalman_filter.hpp
deleted file mode 100644
index 3fc884a3e700f..0000000000000
--- a/localization/ekf_localizer/include/kalman_filter/time_delay_kalman_filter.hpp
+++ /dev/null
@@ -1,85 +0,0 @@
-/*
- * Copyright 2018-2019 Autoware Foundation. All rights reserved.
- *
- * 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.
- */
-
-#pragma once
-
-#include
-#include
-#include
-#include "kalman_filter/kalman_filter.hpp"
-
-/**
- * @file time_delay_kalman_filter.h
- * @brief kalman filter with delayed measurement class
- * @author Takamasa Horibe
- * @date 2019.05.01
- */
-
-class TimeDelayKalmanFilter : public KalmanFilter
-{
-public:
- /**
- * @brief No initialization constructor.
- */
- TimeDelayKalmanFilter();
-
- /**
- * @brief initialization of kalman filter
- * @param x initial state
- * @param P0 initial covariance of estimated state
- * @param max_delay_step Maximum number of delay steps, which determines the dimension of the extended kalman filter
- */
- void init(const Eigen::MatrixXd & x, const Eigen::MatrixXd & P, const int max_delay_step);
-
- /**
- * @brief get latest time estimated state
- * @param x latest time estimated state
- */
- void getLatestX(Eigen::MatrixXd & x);
-
- /**
- * @brief get latest time estimation covariance
- * @param P latest time estimation covariance
- */
- void getLatestP(Eigen::MatrixXd & P);
-
- /**
- * @brief calculate kalman filter covariance by predicion model with time delay. This is mainly for EKF of nonlinear
- * process model.
- * @param x_next predicted state by prediction model
- * @param A coefficient matrix of x for process model
- * @param Q covariace matrix for process model
- */
- bool predictWithDelay(
- const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q);
-
- /**
- * @brief calculate kalman filter covariance by measurement model with time delay. This is mainly for EKF of nonlinear
- * process model.
- * @param y measured values
- * @param C coefficient matrix of x for measurement model
- * @param R covariance matrix for measurement model
- * @param delay_step measurement delay
- */
- bool updateWithDelay(
- const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R,
- const int delay_step);
-
-private:
- int max_delay_step_; //!< @brief maximum number of delay steps
- int dim_x_; //!< @brief dimension of latest state
- int dim_x_ex_; //!< @brief dimension of extended state with dime delay
-};
\ No newline at end of file
diff --git a/localization/ekf_localizer/launch/ekf_localizer.launch b/localization/ekf_localizer/launch/ekf_localizer.launch
deleted file mode 100644
index 38b278c67ce30..0000000000000
--- a/localization/ekf_localizer/launch/ekf_localizer.launch
+++ /dev/null
@@ -1,100 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/localization/ekf_localizer/media/delay_model_eq.png b/localization/ekf_localizer/media/delay_model_eq.png
deleted file mode 100644
index 41777d756b974..0000000000000
Binary files a/localization/ekf_localizer/media/delay_model_eq.png and /dev/null differ
diff --git a/localization/ekf_localizer/media/ekf_autoware_res.png b/localization/ekf_localizer/media/ekf_autoware_res.png
deleted file mode 100644
index c771e3620b161..0000000000000
Binary files a/localization/ekf_localizer/media/ekf_autoware_res.png and /dev/null differ
diff --git a/localization/ekf_localizer/media/ekf_delay_comp.png b/localization/ekf_localizer/media/ekf_delay_comp.png
deleted file mode 100644
index 54d934caecc92..0000000000000
Binary files a/localization/ekf_localizer/media/ekf_delay_comp.png and /dev/null differ
diff --git a/localization/ekf_localizer/media/ekf_dynamics.png b/localization/ekf_localizer/media/ekf_dynamics.png
deleted file mode 100644
index 63c81261a718e..0000000000000
Binary files a/localization/ekf_localizer/media/ekf_dynamics.png and /dev/null differ
diff --git a/localization/ekf_localizer/media/ekf_flowchart.png b/localization/ekf_localizer/media/ekf_flowchart.png
deleted file mode 100644
index 0a80cb06b85f0..0000000000000
Binary files a/localization/ekf_localizer/media/ekf_flowchart.png and /dev/null differ
diff --git a/localization/ekf_localizer/media/ekf_smooth_update.png b/localization/ekf_localizer/media/ekf_smooth_update.png
deleted file mode 100644
index 7ac26fc604c6f..0000000000000
Binary files a/localization/ekf_localizer/media/ekf_smooth_update.png and /dev/null differ
diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml
deleted file mode 100644
index a229d8f34a366..0000000000000
--- a/localization/ekf_localizer/package.xml
+++ /dev/null
@@ -1,17 +0,0 @@
-
-
- ekf_localizer
- 0.1.0
- The ekf_localizer package
- horibe
- Apache 2.0
-
- catkin
-
- geometry_msgs
- roscpp
- sensor_msgs
- std_msgs
- tf2
- tf2_ros
-
diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp
deleted file mode 100644
index 95acfc2efeca8..0000000000000
--- a/localization/ekf_localizer/src/ekf_localizer.cpp
+++ /dev/null
@@ -1,736 +0,0 @@
-/*
- * Copyright 2018-2019 Autoware Foundation. All rights reserved.
- *
- * 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/ekf_localizer.h"
-
-// clang-format off
-#define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl
-#define DEBUG_INFO(...) { if (show_debug_info_) { ROS_INFO(__VA_ARGS__); } }
-#define DEBUG_PRINT_MAT(X) { if (show_debug_info_) { std::cout << #X << ": " << X << std::endl; } }
-
-// clang-format on
-EKFLocalizer::EKFLocalizer() : nh_(""), pnh_("~"), dim_x_(6 /* x, y, yaw, yaw_bias, vx, wz */)
-{
- pnh_.param("show_debug_info", show_debug_info_, bool(false));
- pnh_.param("predict_frequency", ekf_rate_, double(50.0));
- ekf_dt_ = 1.0 / std::max(ekf_rate_, 0.1);
- pnh_.param("tf_rate", tf_rate_, double(10.0));
- pnh_.param("enable_yaw_bias_estimation", enable_yaw_bias_estimation_, bool(true));
- pnh_.param("extend_state_step", extend_state_step_, int(50));
- pnh_.param("pose_frame_id", pose_frame_id_, std::string("map"));
-
- /* pose measurement */
- pnh_.param("pose_additional_delay", pose_additional_delay_, double(0.0));
- pnh_.param("pose_measure_uncertainty_time", pose_measure_uncertainty_time_, double(0.01));
- pnh_.param("pose_rate", pose_rate_, double(10.0)); // used for covariance calculation
- pnh_.param("pose_gate_dist", pose_gate_dist_, double(10000.0)); // Mahalanobis limit
- pnh_.param("pose_stddev_x", pose_stddev_x_, double(0.05));
- pnh_.param("pose_stddev_y", pose_stddev_y_, double(0.05));
- pnh_.param("pose_stddev_yaw", pose_stddev_yaw_, double(0.035));
- pnh_.param("use_pose_with_covariance", use_pose_with_covariance_, bool(false));
-
- /* twist measurement */
- pnh_.param("twist_additional_delay", twist_additional_delay_, double(0.0));
- pnh_.param("twist_rate", twist_rate_, double(10.0)); // used for covariance calculation
- pnh_.param("twist_gate_dist", twist_gate_dist_, double(10000.0)); // Mahalanobis limit
- pnh_.param("twist_stddev_vx", twist_stddev_vx_, double(0.2));
- pnh_.param("twist_stddev_wz", twist_stddev_wz_, double(0.03));
- pnh_.param("use_twist_with_covariance", use_twist_with_covariance_, bool(false));
-
- /* process noise */
- double proc_stddev_yaw_c, proc_stddev_yaw_bias_c, proc_stddev_vx_c, proc_stddev_wz_c;
- pnh_.param("proc_stddev_yaw_c", proc_stddev_yaw_c, double(0.005));
- pnh_.param("proc_stddev_yaw_bias_c", proc_stddev_yaw_bias_c, double(0.001));
- pnh_.param("proc_stddev_vx_c", proc_stddev_vx_c, double(5.0));
- pnh_.param("proc_stddev_wz_c", proc_stddev_wz_c, double(1.0));
- if (!enable_yaw_bias_estimation_) {
- proc_stddev_yaw_bias_c = 0.0;
- }
-
- /* convert to continuous to discrete */
- proc_cov_vx_d_ = std::pow(proc_stddev_vx_c * ekf_dt_, 2.0);
- proc_cov_wz_d_ = std::pow(proc_stddev_wz_c * ekf_dt_, 2.0);
- proc_cov_yaw_d_ = std::pow(proc_stddev_yaw_c * ekf_dt_, 2.0);
- proc_cov_yaw_bias_d_ = std::pow(proc_stddev_yaw_bias_c * ekf_dt_, 2.0);
-
- /* initialize ros system */
-
- timer_control_ = nh_.createTimer(ros::Duration(ekf_dt_), &EKFLocalizer::timerCallback, this);
- timer_tf_ = nh_.createTimer(ros::Duration(1.0 / tf_rate_), &EKFLocalizer::timerTFCallback, this);
- pub_pose_ = nh_.advertise("ekf_pose", 1);
- pub_pose_cov_ =
- nh_.advertise("ekf_pose_with_covariance", 1);
- pub_twist_ = nh_.advertise("ekf_twist", 1);
- pub_twist_cov_ =
- nh_.advertise("ekf_twist_with_covariance", 1);
- pub_yaw_bias_ = pnh_.advertise("estimated_yaw_bias", 1);
- pub_pose_no_yawbias_ = nh_.advertise("ekf_pose_without_yawbias", 1);
- pub_pose_cov_no_yawbias_ = nh_.advertise(
- "ekf_pose_with_covariance_without_yawbias", 1);
- sub_initialpose_ = nh_.subscribe("initialpose", 1, &EKFLocalizer::callbackInitialPose, this);
- sub_pose_with_cov_ =
- nh_.subscribe("in_pose_with_covariance", 1, &EKFLocalizer::callbackPoseWithCovariance, this);
- sub_pose_ = nh_.subscribe("in_pose", 1, &EKFLocalizer::callbackPose, this);
- sub_twist_with_cov_ =
- nh_.subscribe("in_twist_with_covariance", 1, &EKFLocalizer::callbackTwistWithCovariance, this);
- sub_twist_ = nh_.subscribe("in_twist", 1, &EKFLocalizer::callbackTwist, this);
-
- dim_x_ex_ = dim_x_ * extend_state_step_;
-
- initEKF();
-
- /* debug */
- pub_debug_ = pnh_.advertise("debug", 1);
- pub_measured_pose_ = pnh_.advertise("debug/measured_pose", 1);
-};
-
-EKFLocalizer::~EKFLocalizer(){};
-
-/*
- * timerCallback
- */
-void EKFLocalizer::timerCallback(const ros::TimerEvent & e)
-{
- DEBUG_INFO("========================= timer called =========================");
-
- /* predict model in EKF */
- auto start = std::chrono::system_clock::now();
- DEBUG_INFO("------------------------- start prediction -------------------------");
- predictKinematicsModel();
- double elapsed =
- std::chrono::duration_cast(std::chrono::system_clock::now() - start)
- .count();
- DEBUG_INFO("[EKF] predictKinematicsModel calculation time = %f [ms]", elapsed * 1.0e-6);
- DEBUG_INFO("------------------------- end prediction -------------------------\n");
-
- /* pose measurement update */
- if (current_pose_ptr_ != nullptr) {
- DEBUG_INFO("------------------------- start Pose -------------------------");
- start = std::chrono::system_clock::now();
- measurementUpdatePose(*current_pose_ptr_);
- elapsed =
- std::chrono::duration_cast(std::chrono::system_clock::now() - start)
- .count();
- DEBUG_INFO("[EKF] measurementUpdatePose calculation time = %f [ms]", elapsed * 1.0e-6);
- DEBUG_INFO("------------------------- end Pose -------------------------\n");
- }
-
- /* twist measurement update */
- if (current_twist_ptr_ != nullptr) {
- DEBUG_INFO("------------------------- start twist -------------------------");
- start = std::chrono::system_clock::now();
- measurementUpdateTwist(*current_twist_ptr_);
- elapsed =
- std::chrono::duration_cast(std::chrono::system_clock::now() - start)
- .count();
- DEBUG_INFO("[EKF] measurementUpdateTwist calculation time = %f [ms]", elapsed * 1.0e-6);
- DEBUG_INFO("------------------------- end twist -------------------------\n");
- }
-
- /* set current pose, twist */
- setCurrentResult();
-
- /* publish ekf result */
- publishEstimateResult();
-}
-
-void EKFLocalizer::showCurrentX()
-{
- if (show_debug_info_) {
- Eigen::MatrixXd X(dim_x_, 1);
- ekf_.getLatestX(X);
- DEBUG_PRINT_MAT(X.transpose());
- }
-}
-
-/*
- * setCurrentResult
- */
-void EKFLocalizer::setCurrentResult()
-{
- current_ekf_pose_.header.frame_id = pose_frame_id_;
- current_ekf_pose_.header.stamp = ros::Time::now();
- current_ekf_pose_.pose.position.x = ekf_.getXelement(IDX::X);
- current_ekf_pose_.pose.position.y = ekf_.getXelement(IDX::Y);
-
- tf2::Quaternion q_tf;
- double roll, pitch, yaw;
- if (current_pose_ptr_ != nullptr) {
- current_ekf_pose_.pose.position.z = current_pose_ptr_->pose.position.z;
- tf2::fromMsg(current_pose_ptr_->pose.orientation, q_tf); /* use Pose pitch and roll */
- tf2::Matrix3x3(q_tf).getRPY(roll, pitch, yaw);
- } else {
- // current_ekf_pose_.pose.position.z = 0.0;
- // roll = 0;
- // pitch = 0;
- }
- yaw = ekf_.getXelement(IDX::YAW) + ekf_.getXelement(IDX::YAWB);
- current_ekf_pose_.pose.orientation = createQuaternionFromRPY(roll, pitch, yaw);
-
- current_ekf_pose_no_yawbias_ = current_ekf_pose_;
- current_ekf_pose_no_yawbias_.pose.orientation =
- createQuaternionFromRPY(roll, pitch, ekf_.getXelement(IDX::YAW));
-
- current_ekf_twist_.header.frame_id = "base_link";
- current_ekf_twist_.header.stamp = ros::Time::now();
- current_ekf_twist_.twist.linear.x = ekf_.getXelement(IDX::VX);
- current_ekf_twist_.twist.angular.z = ekf_.getXelement(IDX::WZ);
-}
-
-/*
- * timerTFCallback
- */
-void EKFLocalizer::timerTFCallback(const ros::TimerEvent & e)
-{
- if (current_ekf_pose_.header.frame_id == "") return;
-
- geometry_msgs::TransformStamped transformStamped;
- transformStamped.header.stamp = ros::Time::now();
- transformStamped.header.frame_id = current_ekf_pose_.header.frame_id;
- transformStamped.child_frame_id = "base_link";
- transformStamped.transform.translation.x = current_ekf_pose_.pose.position.x;
- transformStamped.transform.translation.y = current_ekf_pose_.pose.position.y;
- transformStamped.transform.translation.z = current_ekf_pose_.pose.position.z;
-
- transformStamped.transform.rotation.x = current_ekf_pose_.pose.orientation.x;
- transformStamped.transform.rotation.y = current_ekf_pose_.pose.orientation.y;
- transformStamped.transform.rotation.z = current_ekf_pose_.pose.orientation.z;
- transformStamped.transform.rotation.w = current_ekf_pose_.pose.orientation.w;
-
- tf_br_.sendTransform(transformStamped);
-}
-
-/*
- * getTransformFromTF
- */
-bool EKFLocalizer::getTransformFromTF(
- std::string parent_frame, std::string child_frame, geometry_msgs::TransformStamped & transform)
-{
- tf2_ros::Buffer tf_buffer;
- tf2_ros::TransformListener tf_listener(tf_buffer);
- ros::Duration(0.1).sleep();
- if (parent_frame.front() == '/') parent_frame.erase(0, 1);
- if (child_frame.front() == '/') child_frame.erase(0, 1);
-
- for (int i = 0; i < 50; ++i) {
- try {
- transform = tf_buffer.lookupTransform(parent_frame, child_frame, ros::Time(0));
- return true;
- } catch (tf2::TransformException & ex) {
- ROS_WARN("%s", ex.what());
- ros::Duration(0.1).sleep();
- }
- }
- return false;
-}
-
-/*
- * callbackInitialPose
- */
-void EKFLocalizer::callbackInitialPose(const geometry_msgs::PoseWithCovarianceStamped & initialpose)
-{
- geometry_msgs::TransformStamped transform;
- if (!getTransformFromTF(pose_frame_id_, initialpose.header.frame_id, transform)) {
- ROS_ERROR(
- "[EKF] TF transform failed. parent = %s, child = %s", pose_frame_id_.c_str(),
- initialpose.header.frame_id.c_str());
- };
-
- Eigen::MatrixXd X(dim_x_, 1);
- Eigen::MatrixXd P = Eigen::MatrixXd::Zero(dim_x_, dim_x_);
-
- // TODO need mutex
-
- X(IDX::X) = initialpose.pose.pose.position.x + transform.transform.translation.x;
- X(IDX::Y) = initialpose.pose.pose.position.y + transform.transform.translation.y;
- current_ekf_pose_.pose.position.z =
- initialpose.pose.pose.position.z + transform.transform.translation.z;
- X(IDX::YAW) =
- tf2::getYaw(initialpose.pose.pose.orientation) + tf2::getYaw(transform.transform.rotation);
- X(IDX::YAWB) = 0.0;
- X(IDX::VX) = 0.0;
- X(IDX::WZ) = 0.0;
-
- P(IDX::X, IDX::X) = initialpose.pose.covariance[0];
- P(IDX::Y, IDX::Y) = initialpose.pose.covariance[6 + 1];
- P(IDX::YAW, IDX::YAW) = initialpose.pose.covariance[6 * 5 + 5];
- P(IDX::YAWB, IDX::YAWB) = 0.0001;
- P(IDX::VX, IDX::VX) = 0.01;
- P(IDX::WZ, IDX::WZ) = 0.01;
-
- ekf_.init(X, P, extend_state_step_);
-
- current_pose_ptr_ = nullptr;
-};
-
-/*
- * callbackPose
- */
-void EKFLocalizer::callbackPose(const geometry_msgs::PoseStamped::ConstPtr & msg)
-{
- if (!use_pose_with_covariance_) {
- current_pose_ptr_ = std::make_shared(*msg);
- }
-};
-
-/*
- * callbackPoseWithCovariance
- */
-void EKFLocalizer::callbackPoseWithCovariance(
- const geometry_msgs::PoseWithCovarianceStamped::ConstPtr & msg)
-{
- if (use_pose_with_covariance_) {
- geometry_msgs::PoseStamped pose;
- pose.header = msg->header;
- pose.pose = msg->pose.pose;
- current_pose_ptr_ = std::make_shared(pose);
- current_pose_covariance_ = msg->pose.covariance;
- }
-};
-
-/*
- * callbackTwist
- */
-void EKFLocalizer::callbackTwist(const geometry_msgs::TwistStamped::ConstPtr & msg)
-{
- if (!use_twist_with_covariance_) {
- current_twist_ptr_ = std::make_shared(*msg);
- }
-};
-
-/*
- * callbackTwistWithCovariance
- */
-void EKFLocalizer::callbackTwistWithCovariance(
- const geometry_msgs::TwistWithCovarianceStamped::ConstPtr & msg)
-{
- if (use_twist_with_covariance_) {
- geometry_msgs::TwistStamped twist;
- twist.header = msg->header;
- twist.twist = msg->twist.twist;
- current_twist_ptr_ = std::make_shared(twist);
- current_twist_covariance_ = msg->twist.covariance;
- }
-};
-
-/*
- * initEKF
- */
-void EKFLocalizer::initEKF()
-{
- Eigen::MatrixXd X = Eigen::MatrixXd::Zero(dim_x_, 1);
- Eigen::MatrixXd P = Eigen::MatrixXd::Identity(dim_x_, dim_x_) * 1.0E15; // for x & y
- P(IDX::YAW, IDX::YAW) = 50.0; // for yaw
- P(IDX::YAWB, IDX::YAWB) = proc_cov_yaw_bias_d_; // for yaw bias
- P(IDX::VX, IDX::VX) = 1000.0; // for vx
- P(IDX::WZ, IDX::WZ) = 50.0; // for wz
-
- ekf_.init(X, P, extend_state_step_);
-}
-
-/*
- * predictKinematicsModel
- */
-void EKFLocalizer::predictKinematicsModel()
-{
- /* == Nonlinear model ==
- *
- * x_{k+1} = x_k + vx_k * cos(yaw_k + b_k) * dt
- * y_{k+1} = y_k + vx_k * sin(yaw_k + b_k) * dt
- * yaw_{k+1} = yaw_k + (wz_k) * dt
- * b_{k+1} = b_k
- * vx_{k+1} = vz_k
- * wz_{k+1} = wz_k
- *
- * (b_k : yaw_bias_k)
- */
-
- /* == Linearized model ==
- *
- * A = [ 1, 0, -vx*sin(yaw+b)*dt, -vx*sin(yaw+b)*dt, cos(yaw+b)*dt, 0]
- * [ 0, 1, vx*cos(yaw+b)*dt, vx*cos(yaw+b)*dt, sin(yaw+b)*dt, 0]
- * [ 0, 0, 1, 0, 0, dt]
- * [ 0, 0, 0, 1, 0, 0]
- * [ 0, 0, 0, 0, 1, 0]
- * [ 0, 0, 0, 0, 0, 1]
- */
-
- Eigen::MatrixXd X_curr(dim_x_, 1); // curent 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 int d_dim_x = dim_x_ex_ - dim_x_;
- 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) = proc_cov_yaw_bias_d_; // for yaw bias
- Q(IDX::VX, IDX::VX) = proc_cov_vx_d_; // for vx
- Q(IDX::WZ, IDX::WZ) = proc_cov_wz_d_; // for wz
-
- ekf_.predictWithDelay(X_next, A, Q);
-
- // debug
- Eigen::MatrixXd X_result(dim_x_, 1);
- ekf_.getLatestX(X_result);
- DEBUG_PRINT_MAT(X_result.transpose());
- DEBUG_PRINT_MAT((X_result - X_curr).transpose());
-}
-
-/*
- * measurementUpdatePose
- */
-void EKFLocalizer::measurementUpdatePose(const geometry_msgs::PoseStamped & pose)
-{
- if (pose.header.frame_id != pose_frame_id_) {
- ROS_WARN_DELAYED_THROTTLE(
- 2, "pose frame_id is %s, but pose_frame is set as %s. They must be same.",
- pose.header.frame_id.c_str(), pose_frame_id_.c_str());
- }
- Eigen::MatrixXd X_curr(dim_x_, 1); // curent state
- ekf_.getLatestX(X_curr);
- DEBUG_PRINT_MAT(X_curr.transpose());
-
- constexpr int dim_y = 3; // pos_x, pos_y, yaw, depending on Pose output
- const ros::Time t_curr = ros::Time::now();
-
- /* Calculate delay step */
- double delay_time = (t_curr - pose.header.stamp).toSec() + pose_additional_delay_;
- if (delay_time < 0.0) {
- delay_time = 0.0;
- ROS_WARN_DELAYED_THROTTLE(
- 1.0, "Pose time stamp is inappropriate, set delay to 0[s]. delay = %f", delay_time);
- }
- int delay_step = std::roundf(delay_time / ekf_dt_);
- if (delay_step > extend_state_step_ - 1) {
- ROS_WARN_DELAYED_THROTTLE(
- 1.0,
- "Pose delay exceeds the compensation limit, ignored. delay: %f[s], limit = "
- "extend_state_step * ekf_dt : %f [s]",
- delay_time, extend_state_step_ * ekf_dt_);
- return;
- }
- DEBUG_INFO("delay_time: %f [s]", delay_time);
-
- /* Set yaw */
- const double yaw_curr = ekf_.getXelement((unsigned int)(delay_step * dim_x_ + IDX::YAW));
- double yaw = tf2::getYaw(pose.pose.orientation);
- const double ekf_yaw = ekf_.getXelement(delay_step * dim_x_ + IDX::YAW);
- const double yaw_error = normalizeYaw(yaw - ekf_yaw); // normalize the error not to exceed 2 pi
- yaw = yaw_error + ekf_yaw;
-
- /* Set measurement matrix */
- Eigen::MatrixXd y(dim_y, 1);
- y << pose.pose.position.x, pose.pose.position.y, yaw;
-
- if (isnan(y.array()).any() || isinf(y.array()).any()) {
- ROS_WARN(
- "[EKF] pose measurement matrix includes NaN of Inf. ignore update. check pose message.");
- return;
- }
-
- /* Gate */
- Eigen::MatrixXd y_ekf(dim_y, 1);
- y_ekf << ekf_.getXelement(delay_step * dim_x_ + IDX::X),
- ekf_.getXelement(delay_step * dim_x_ + IDX::Y), ekf_yaw;
- Eigen::MatrixXd P_curr, P_y;
- ekf_.getLatestP(P_curr);
- P_y = P_curr.block(0, 0, dim_y, dim_y);
- if (!mahalanobisGate(pose_gate_dist_, y_ekf, y, P_y)) {
- ROS_WARN_DELAYED_THROTTLE(
- 2.0,
- "[EKF] Pose measurement update, mahalanobis distance is over limit. ignore "
- "measurement data.");
- return;
- }
-
- DEBUG_PRINT_MAT(y.transpose());
- DEBUG_PRINT_MAT(y_ekf.transpose());
- DEBUG_PRINT_MAT((y - y_ekf).transpose());
-
- /* Set measurement matrix */
- Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, dim_x_);
- C(0, IDX::X) = 1.0; // for pos x
- C(1, IDX::Y) = 1.0; // for pos y
- C(2, IDX::YAW) = 1.0; // for yaw
-
- /* Set measurement noise covariancs */
- Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);
- if (use_pose_with_covariance_) {
- R(0, 0) = current_pose_covariance_.at(0); // x - x
- R(0, 1) = current_pose_covariance_.at(1); // x - y
- R(0, 2) = current_pose_covariance_.at(5); // x - yaw
- R(1, 0) = current_pose_covariance_.at(6); // y - x
- R(1, 1) = current_pose_covariance_.at(7); // y - y
- R(1, 2) = current_pose_covariance_.at(11); // y - yaw
- R(2, 0) = current_pose_covariance_.at(30); // yaw - x
- R(2, 1) = current_pose_covariance_.at(31); // yaw - y
- R(2, 2) = current_pose_covariance_.at(35); // yaw - yaw
- } else {
- const double ekf_yaw = ekf_.getXelement(delay_step * dim_x_ + IDX::YAW);
- const double vx = ekf_.getXelement(delay_step * dim_x_ + IDX::VX);
- const double wz = ekf_.getXelement(delay_step * dim_x_ + IDX::WZ);
- const double cov_tu_pos_x = std::pow(pose_measure_uncertainty_time_ * vx * cos(ekf_yaw), 2.0);
- const double cov_tu_pos_y = std::pow(pose_measure_uncertainty_time_ * vx * sin(ekf_yaw), 2.0);
- const double cov_tu_yaw = std::pow(pose_measure_uncertainty_time_ * wz, 2.0);
- R(0, 0) = (pose_stddev_x_ * pose_stddev_x_) + cov_tu_pos_x; // pos_x
- R(1, 1) = (pose_stddev_y_ * pose_stddev_y_) + cov_tu_pos_y; // pos_y
- R(2, 2) = (pose_stddev_yaw_ * pose_stddev_yaw_) + cov_tu_yaw; // yaw
- }
-
- /* In order to avoid a large change at the time of updating, measuremeent update is performed by dividing at every
- * step. */
- R *= (ekf_rate_ / pose_rate_);
-
- ekf_.updateWithDelay(y, C, R, delay_step);
-
- // debug
- Eigen::MatrixXd X_result(dim_x_, 1);
- ekf_.getLatestX(X_result);
- DEBUG_PRINT_MAT(X_result.transpose());
- DEBUG_PRINT_MAT((X_result - X_curr).transpose());
-}
-
-/*
- * measurementUpdateTwist
- */
-void EKFLocalizer::measurementUpdateTwist(const geometry_msgs::TwistStamped & twist)
-{
- if (twist.header.frame_id != "base_link") {
- ROS_WARN_DELAYED_THROTTLE(2.0, "twist frame_id must be base_link");
- }
-
- Eigen::MatrixXd X_curr(dim_x_, 1); // curent state
- ekf_.getLatestX(X_curr);
- DEBUG_PRINT_MAT(X_curr.transpose());
-
- constexpr int dim_y = 2; // vx, wz
- const ros::Time t_curr = ros::Time::now();
-
- /* Calculate delay step */
- double delay_time = (t_curr - twist.header.stamp).toSec() + twist_additional_delay_;
- if (delay_time < 0.0) {
- ROS_WARN_DELAYED_THROTTLE(
- 1.0, "Twist time stamp is inappropriate (delay = %f [s]), set delay to 0[s].", delay_time);
- delay_time = 0.0;
- }
- int delay_step = std::roundf(delay_time / ekf_dt_);
- if (delay_step > extend_state_step_ - 1) {
- ROS_WARN_DELAYED_THROTTLE(
- 1.0,
- "Twist delay exceeds the compensation limit, ignored. delay: %f[s], limit = "
- "extend_state_step * ekf_dt : %f [s]",
- delay_time, extend_state_step_ * ekf_dt_);
- return;
- }
- DEBUG_INFO("delay_time: %f [s]", delay_time);
-
- /* Set measurement matrix */
- Eigen::MatrixXd y(dim_y, 1);
- y << twist.twist.linear.x, twist.twist.angular.z;
-
- if (isnan(y.array()).any() || isinf(y.array()).any()) {
- ROS_WARN(
- "[EKF] twist measurement matrix includes NaN of Inf. ignore update. check twist message.");
- return;
- }
-
- /* Gate */
- Eigen::MatrixXd y_ekf(dim_y, 1);
- y_ekf << ekf_.getXelement(delay_step * dim_x_ + IDX::VX),
- ekf_.getXelement(delay_step * dim_x_ + IDX::WZ);
- Eigen::MatrixXd P_curr, P_y;
- ekf_.getLatestP(P_curr);
- P_y = P_curr.block(4, 4, dim_y, dim_y);
- if (!mahalanobisGate(twist_gate_dist_, y_ekf, y, P_y)) {
- ROS_WARN_DELAYED_THROTTLE(
- 2.0,
- "[EKF] Twist measurement update, mahalanobis distance is over limit. ignore "
- "measurement data.");
- return;
- }
-
- DEBUG_PRINT_MAT(y.transpose());
- DEBUG_PRINT_MAT(y_ekf.transpose());
- DEBUG_PRINT_MAT((y - y_ekf).transpose());
-
- /* Set measurement matrix */
- Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, dim_x_);
- C(0, IDX::VX) = 1.0; // for vx
- C(1, IDX::WZ) = 1.0; // for wz
-
- /* Set measurement noise covariancs */
- Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);
- if (use_twist_with_covariance_) {
- R(0, 0) = current_twist_covariance_.at(0); // vx - vx
- R(0, 1) = current_twist_covariance_.at(5); // vx - wz
- R(1, 0) = current_twist_covariance_.at(30); // wz - vx
- R(1, 1) = current_twist_covariance_.at(35); // wz - wz
- } else {
- R(0, 0) = twist_stddev_vx_ * twist_stddev_vx_; // for vx
- R(1, 1) = twist_stddev_wz_ * twist_stddev_wz_; // for wz
- }
-
- /* In order to avoid a large change by update, measurement update is performed by dividing at every step. */
- R *= (ekf_rate_ / twist_rate_);
-
- ekf_.updateWithDelay(y, C, R, delay_step);
-
- // debug
- Eigen::MatrixXd X_result(dim_x_, 1);
- ekf_.getLatestX(X_result);
- DEBUG_PRINT_MAT(X_result.transpose());
- DEBUG_PRINT_MAT((X_result - X_curr).transpose());
-};
-
-/*
- * mahalanobisGate
- */
-bool EKFLocalizer::mahalanobisGate(
- const double & dist_max, const Eigen::MatrixXd & x, const Eigen::MatrixXd & obj_x,
- const Eigen::MatrixXd & cov) const
-{
- Eigen::MatrixXd mahalanobis_squared = (x - obj_x).transpose() * cov.inverse() * (x - obj_x);
- DEBUG_INFO(
- "measurement update: mahalanobis = %f, gate limit = %f", std::sqrt(mahalanobis_squared(0)),
- dist_max);
- if (mahalanobis_squared(0) > dist_max * dist_max) {
- return false;
- }
-
- return true;
-}
-
-/*
- * createQuaternionFromRPY
- */
-geometry_msgs::Quaternion EKFLocalizer::createQuaternionFromRPY(double r, double p, double y) const
-{
- tf2::Quaternion q;
- q.setRPY(r, p, y);
- return tf2::toMsg(q);
-}
-
-/*
- * publishEstimateResult
- */
-void EKFLocalizer::publishEstimateResult()
-{
- ros::Time current_time = ros::Time::now();
- Eigen::MatrixXd X(dim_x_, 1);
- Eigen::MatrixXd P(dim_x_, dim_x_);
- ekf_.getLatestX(X);
- ekf_.getLatestP(P);
-
- /* publish latest pose */
- pub_pose_.publish(current_ekf_pose_);
- pub_pose_no_yawbias_.publish(current_ekf_pose_no_yawbias_);
-
- /* publish latest pose with covariance */
- geometry_msgs::PoseWithCovarianceStamped pose_cov;
- pose_cov.header.stamp = current_time;
- pose_cov.header.frame_id = current_ekf_pose_.header.frame_id;
- pose_cov.pose.pose = current_ekf_pose_.pose;
- pose_cov.pose.covariance[0] = P(IDX::X, IDX::X);
- pose_cov.pose.covariance[1] = P(IDX::X, IDX::Y);
- pose_cov.pose.covariance[5] = P(IDX::X, IDX::YAW);
- pose_cov.pose.covariance[6] = P(IDX::Y, IDX::X);
- pose_cov.pose.covariance[7] = P(IDX::Y, IDX::Y);
- pose_cov.pose.covariance[11] = P(IDX::Y, IDX::YAW);
- pose_cov.pose.covariance[30] = P(IDX::YAW, IDX::X);
- pose_cov.pose.covariance[31] = P(IDX::YAW, IDX::Y);
- pose_cov.pose.covariance[35] = P(IDX::YAW, IDX::YAW);
- pub_pose_cov_.publish(pose_cov);
-
- geometry_msgs::PoseWithCovarianceStamped pose_cov_no_yawbias = pose_cov;
- pose_cov_no_yawbias.pose.pose = current_ekf_pose_no_yawbias_.pose;
- pub_pose_cov_no_yawbias_.publish(pose_cov_no_yawbias);
-
- /* publish latest twist */
- pub_twist_.publish(current_ekf_twist_);
-
- /* publish latest twist with covariance */
- geometry_msgs::TwistWithCovarianceStamped twist_cov;
- twist_cov.header.stamp = current_time;
- twist_cov.header.frame_id = current_ekf_twist_.header.frame_id;
- twist_cov.twist.twist = current_ekf_twist_.twist;
- twist_cov.twist.covariance[0] = P(IDX::VX, IDX::VX);
- twist_cov.twist.covariance[5] = P(IDX::VX, IDX::WZ);
- twist_cov.twist.covariance[30] = P(IDX::WZ, IDX::VX);
- twist_cov.twist.covariance[35] = P(IDX::WZ, IDX::WZ);
- pub_twist_cov_.publish(twist_cov);
-
- /* publish yaw bias */
- std_msgs::Float64 yawb;
- yawb.data = X(IDX::YAWB);
- pub_yaw_bias_.publish(yawb);
-
- /* debug measured pose */
- if (current_pose_ptr_ != nullptr) {
- geometry_msgs::PoseStamped p;
- p = *current_pose_ptr_;
- p.header.stamp = current_time;
- pub_measured_pose_.publish(p);
- }
-
- /* debug publish */
- double RAD2DEG = 180.0 / 3.141592;
- double pose_yaw = 0.0;
- if (current_pose_ptr_ != nullptr)
- pose_yaw = tf2::getYaw(current_pose_ptr_->pose.orientation) * RAD2DEG;
-
- std_msgs::Float64MultiArray msg;
- msg.data.push_back(X(IDX::YAW) * RAD2DEG); // [0] ekf yaw angle
- msg.data.push_back(pose_yaw); // [1] measurement yaw angle
- msg.data.push_back(X(IDX::YAWB) * RAD2DEG); // [2] yaw bias
- pub_debug_.publish(msg);
-}
-
-double EKFLocalizer::normalizeYaw(const double & yaw) const
-{
- return std::atan2(std::sin(yaw), std::cos(yaw));
-}
diff --git a/localization/ekf_localizer/src/ekf_localizer_node.cpp b/localization/ekf_localizer/src/ekf_localizer_node.cpp
deleted file mode 100644
index 45d2f56bab576..0000000000000
--- a/localization/ekf_localizer/src/ekf_localizer_node.cpp
+++ /dev/null
@@ -1,27 +0,0 @@
-/*
- * Copyright 2018-2019 Autoware Foundation. All rights reserved.
- *
- * 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/ekf_localizer.h"
-
-int main(int argc, char ** argv)
-{
- ros::init(argc, argv, "ekf_localizer");
- EKFLocalizer obj;
-
- ros::spin();
-
- return 0;
-};
diff --git a/localization/ekf_localizer/src/kalman_filter/kalman_filter.cpp b/localization/ekf_localizer/src/kalman_filter/kalman_filter.cpp
deleted file mode 100644
index 8610c3986f834..0000000000000
--- a/localization/ekf_localizer/src/kalman_filter/kalman_filter.cpp
+++ /dev/null
@@ -1,126 +0,0 @@
-/*
- * Copyright 2018-2019 Autoware Foundation. All rights reserved.
- *
- * 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"
-
-KalmanFilter::KalmanFilter() {}
-KalmanFilter::KalmanFilter(
- const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B,
- const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R,
- const Eigen::MatrixXd & P)
-{
- init(x, A, B, C, Q, R, P);
-}
-KalmanFilter::~KalmanFilter() {}
-bool KalmanFilter::init(
- const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B,
- const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R,
- const Eigen::MatrixXd & P)
-{
- if (
- x.cols() == 0 || x.rows() == 0 || A.cols() == 0 || A.rows() == 0 || B.cols() == 0 ||
- B.rows() == 0 || C.cols() == 0 || C.rows() == 0 || Q.cols() == 0 || Q.rows() == 0 ||
- R.cols() == 0 || R.rows() == 0 || P.cols() == 0 || P.rows() == 0) {
- return false;
- }
- x_ = x;
- A_ = A;
- B_ = B;
- C_ = C;
- Q_ = Q;
- R_ = R;
- P_ = P;
- return true;
-}
-bool KalmanFilter::init(const Eigen::MatrixXd & x, const Eigen::MatrixXd & P0)
-{
- if (x.cols() == 0 || x.rows() == 0 || P0.cols() == 0 || P0.rows() == 0) {
- return false;
- }
- x_ = x;
- P_ = P0;
- return true;
-}
-
-void KalmanFilter::setA(const Eigen::MatrixXd & A) { A_ = A; }
-void KalmanFilter::setB(const Eigen::MatrixXd & B) { B_ = B; }
-void KalmanFilter::setC(const Eigen::MatrixXd & C) { C_ = C; }
-void KalmanFilter::setQ(const Eigen::MatrixXd & Q) { Q_ = Q; }
-void KalmanFilter::setR(const Eigen::MatrixXd & R) { R_ = R; }
-void KalmanFilter::getX(Eigen::MatrixXd & x) { x = x_; };
-void KalmanFilter::getP(Eigen::MatrixXd & P) { P = P_; };
-double KalmanFilter::getXelement(unsigned int i) { return x_(i); };
-
-bool KalmanFilter::predict(
- const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q)
-{
- if (
- x_.rows() != x_next.rows() || A.cols() != P_.rows() || Q.cols() != Q.rows() ||
- A.rows() != Q.cols()) {
- return false;
- }
- x_ = x_next;
- P_ = A * P_ * A.transpose() + Q;
- return true;
-}
-bool KalmanFilter::predict(const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A)
-{
- return predict(x_next, A, Q_);
-}
-
-bool KalmanFilter::predict(
- const Eigen::MatrixXd & u, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B,
- const Eigen::MatrixXd & Q)
-{
- if (A.cols() != x_.rows() || B.cols() != u.rows()) {
- return false;
- }
- const Eigen::MatrixXd x_next = A * x_ + B * u;
- return predict(x_next, A, Q);
-}
-bool KalmanFilter::predict(const Eigen::MatrixXd & u) { return predict(u, A_, B_, Q_); }
-
-bool KalmanFilter::update(
- const Eigen::MatrixXd & y, const Eigen::MatrixXd & y_pred, const Eigen::MatrixXd & C,
- const Eigen::MatrixXd & R)
-{
- if (
- P_.cols() != C.cols() || R.rows() != R.cols() || R.rows() != C.rows() ||
- y.rows() != y_pred.rows() || y.rows() != C.rows()) {
- return false;
- }
- const Eigen::MatrixXd PCT = P_ * C.transpose();
- const Eigen::MatrixXd K = PCT * ((R + C * PCT).inverse());
-
- if (isnan(K.array()).any() || isinf(K.array()).any()) {
- return false;
- };
-
- x_ = x_ + K * (y - y_pred);
- P_ = P_ - K * (C * P_);
- return true;
-}
-
-bool KalmanFilter::update(
- const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R)
-{
- if (C.cols() != x_.rows()) {
- return false;
- }
- const Eigen::MatrixXd y_pred = C * x_;
- return update(y, y_pred, C, R);
-}
-bool KalmanFilter::update(const Eigen::MatrixXd & y) { return update(y, C_, R_); }
diff --git a/localization/ekf_localizer/src/kalman_filter/time_delay_kalman_filter.cpp b/localization/ekf_localizer/src/kalman_filter/time_delay_kalman_filter.cpp
deleted file mode 100644
index 5b309f9c43f65..0000000000000
--- a/localization/ekf_localizer/src/kalman_filter/time_delay_kalman_filter.cpp
+++ /dev/null
@@ -1,95 +0,0 @@
-/*
- * Copyright 2018-2019 Autoware Foundation. All rights reserved.
- *
- * 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"
-
-TimeDelayKalmanFilter::TimeDelayKalmanFilter() {}
-
-void TimeDelayKalmanFilter::init(
- const Eigen::MatrixXd & x, const Eigen::MatrixXd & P0, const int max_delay_step)
-{
- max_delay_step_ = max_delay_step;
- dim_x_ = x.rows();
- dim_x_ex_ = dim_x_ * max_delay_step;
-
- x_ = Eigen::MatrixXd::Zero(dim_x_ex_, 1);
- P_ = Eigen::MatrixXd::Zero(dim_x_ex_, dim_x_ex_);
-
- for (int i = 0; i < max_delay_step_; ++i) {
- x_.block(i * dim_x_, 0, dim_x_, 1) = x;
- P_.block(i * dim_x_, i * dim_x_, dim_x_, dim_x_) = P0;
- }
-};
-
-void TimeDelayKalmanFilter::getLatestX(Eigen::MatrixXd & x) { x = x_.block(0, 0, dim_x_, 1); };
-void TimeDelayKalmanFilter::getLatestP(Eigen::MatrixXd & P) { P = P_.block(0, 0, dim_x_, dim_x_); };
-
-bool TimeDelayKalmanFilter::predictWithDelay(
- const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q)
-{
- /*
- * time delay model:
- *
- * [A 0 0] [P11 P12 P13] [Q 0 0]
- * A = [I 0 0], P = [P21 P22 P23], Q = [0 0 0]
- * [0 I 0] [P31 P32 P33] [0 0 0]
- *
- * covariance calculation in prediction : P = A * P * A' + Q
- *
- * [A*P11*A'*+Q A*P11 A*P12]
- * P = [ P11*A' P11 P12]
- * [ P21*A' P21 P22]
- */
-
- const int d_dim_x = dim_x_ex_ - dim_x_;
-
- /* slide states in the time direction */
- Eigen::MatrixXd x_tmp = Eigen::MatrixXd::Zero(dim_x_ex_, 1);
- x_tmp.block(0, 0, dim_x_, 1) = x_next;
- x_tmp.block(dim_x_, 0, d_dim_x, 1) = x_.block(0, 0, d_dim_x, 1);
- x_ = x_tmp;
-
- /* update P with delayed measurement A matrix structure */
- Eigen::MatrixXd P_tmp = Eigen::MatrixXd::Zero(dim_x_ex_, dim_x_ex_);
- P_tmp.block(0, 0, dim_x_, dim_x_) = A * P_.block(0, 0, dim_x_, dim_x_) * A.transpose() + Q;
- P_tmp.block(0, dim_x_, dim_x_, d_dim_x) = A * P_.block(0, 0, dim_x_, d_dim_x);
- P_tmp.block(dim_x_, 0, d_dim_x, dim_x_) = P_.block(0, 0, d_dim_x, dim_x_) * A.transpose();
- P_tmp.block(dim_x_, dim_x_, d_dim_x, d_dim_x) = P_.block(0, 0, d_dim_x, d_dim_x);
- P_ = P_tmp;
-
- return true;
-};
-
-bool TimeDelayKalmanFilter::updateWithDelay(
- const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R,
- const int delay_step)
-{
- if (delay_step >= max_delay_step_) {
- std::cerr << "delay step is larger than max_delay_step. ignore update." << std::endl;
- return false;
- }
-
- const int dim_y = y.rows();
-
- /* set measurement matrix */
- Eigen::MatrixXd C_ex = Eigen::MatrixXd::Zero(dim_y, dim_x_ex_);
- C_ex.block(0, dim_x_ * delay_step, dim_y, dim_x_) = C;
-
- /* update */
- if (!update(y, C_ex, R)) return false;
-
- return true;
-};
\ No newline at end of file
diff --git a/localization/ekf_localizer/test/src/test_ekf_localizer.cpp b/localization/ekf_localizer/test/src/test_ekf_localizer.cpp
deleted file mode 100644
index 3288a5c85e612..0000000000000
--- a/localization/ekf_localizer/test/src/test_ekf_localizer.cpp
+++ /dev/null
@@ -1,289 +0,0 @@
-/*
- * Copyright 2018-2019 Autoware Foundation. All rights reserved.
- *
- * 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
-
-#include
-#include
-#include
-#include
-#include
-
-#include "ekf_localizer/ekf_localizer.h"
-
-class EKFLocalizerTestSuite : public ::testing::Test
-{
-public:
- EKFLocalizerTestSuite() : nh_(""), pnh_("~")
- {
- sub_twist = nh_.subscribe("/ekf_twist", 1, &EKFLocalizerTestSuite::callbackTwist, this);
- sub_pose = nh_.subscribe("/ekf_pose", 1, &EKFLocalizerTestSuite::callbackPose, this);
- tiemr_ = nh_.createTimer(ros::Duration(0.1), &EKFLocalizerTestSuite::timerCallback, this);
- }
- ~EKFLocalizerTestSuite() {}
-
- ros::NodeHandle nh_, pnh_;
-
- std::string frame_id_a_ = "world";
- std::string frame_id_b_ = "base_link";
-
- ros::Subscriber sub_twist;
- ros::Subscriber sub_pose;
-
- ros::Timer tiemr_;
-
- std::shared_ptr current_pose_ptr_;
- std::shared_ptr current_twist_ptr_;
-
- void timerCallback(const ros::TimerEvent & e)
- {
- /* !!! this should be defined before sendTransform() !!! */
- static tf2_ros::TransformBroadcaster br;
- geometry_msgs::TransformStamped sended;
-
- ros::Time current_time = ros::Time::now();
-
- sended.header.stamp = current_time;
- sended.header.frame_id = frame_id_a_;
- sended.child_frame_id = frame_id_b_;
- sended.transform.translation.x = -7.11;
- sended.transform.translation.y = 0.0;
- sended.transform.translation.z = 0.0;
- tf2::Quaternion q;
- q.setRPY(0, 0, 0.5);
- sended.transform.rotation.x = q.x();
- sended.transform.rotation.y = q.y();
- sended.transform.rotation.z = q.z();
- sended.transform.rotation.w = q.w();
-
- br.sendTransform(sended);
- };
-
- void callbackPose(const geometry_msgs::PoseStamped::ConstPtr & pose)
- {
- current_pose_ptr_ = std::make_shared(*pose);
- }
-
- void callbackTwist(const geometry_msgs::TwistStamped::ConstPtr & twist)
- {
- current_twist_ptr_ = std::make_shared(*twist);
- }
-
- void resetCurrentPoseAndTwist()
- {
- current_pose_ptr_ = nullptr;
- current_twist_ptr_ = nullptr;
- }
-};
-
-TEST_F(EKFLocalizerTestSuite, measurementUpdatePose)
-{
- EKFLocalizer ekf_;
-
- ros::Publisher pub_pose = nh_.advertise("/in_pose", 1);
- geometry_msgs::PoseStamped in_pose;
- in_pose.header.frame_id = "world";
- in_pose.pose.position.x = 1.0;
- in_pose.pose.position.y = 2.0;
- in_pose.pose.position.z = 3.0;
- in_pose.pose.orientation.x = 0.0;
- in_pose.pose.orientation.y = 0.0;
- in_pose.pose.orientation.z = 0.0;
- in_pose.pose.orientation.w = 1.0;
-
- /* test for valid value */
- const double pos_x = 12.3;
- in_pose.pose.position.x = pos_x; // for vaild value
-
- for (int i = 0; i < 20; ++i) {
- in_pose.header.stamp = ros::Time::now();
- pub_pose.publish(in_pose);
- ros::spinOnce();
- ros::Duration(0.1).sleep();
- }
-
- ASSERT_FALSE(current_pose_ptr_ == nullptr);
- ASSERT_FALSE(current_twist_ptr_ == nullptr);
-
- double ekf_x = current_pose_ptr_->pose.position.x;
- bool is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x));
- ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value.";
- ASSERT_TRUE(std::fabs(ekf_x - pos_x) < 0.1)
- << "ekf pos x: " << ekf_x << " should be close to " << pos_x;
-
- /* test for invalid value */
- in_pose.pose.position.x = NAN; // check for invalid values
- for (int i = 0; i < 10; ++i) {
- in_pose.header.stamp = ros::Time::now();
- pub_pose.publish(in_pose);
- ros::spinOnce();
- ros::Duration(0.1).sleep();
- }
- is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x));
- ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value.";
-
- resetCurrentPoseAndTwist();
-}
-
-TEST_F(EKFLocalizerTestSuite, measurementUpdateTwist)
-{
- EKFLocalizer ekf_;
-
- ros::Publisher pub_twist = nh_.advertise("/in_twist", 1);
- geometry_msgs::TwistStamped in_twist;
- in_twist.header.frame_id = "base_link";
-
- /* test for valid value */
- const double vx = 12.3;
- in_twist.twist.linear.x = vx; // for vaild value
- for (int i = 0; i < 20; ++i) {
- in_twist.header.stamp = ros::Time::now();
- pub_twist.publish(in_twist);
- ros::spinOnce();
- ros::Duration(0.1).sleep();
- }
-
- ASSERT_FALSE(current_pose_ptr_ == nullptr);
- ASSERT_FALSE(current_twist_ptr_ == nullptr);
-
- double ekf_vx = current_twist_ptr_->twist.linear.x;
- bool is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx));
- ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value.";
- ASSERT_TRUE(std::fabs(ekf_vx - vx) < 0.1)
- << "ekf vel x: " << ekf_vx << ", should be close to " << vx;
-
- /* test for invalid value */
- in_twist.twist.linear.x = NAN; // check for invalid values
- for (int i = 0; i < 10; ++i) {
- in_twist.header.stamp = ros::Time::now();
- pub_twist.publish(in_twist);
- ros::spinOnce();
- ros::Duration(0.1).sleep();
- }
-
- ekf_vx = current_twist_ptr_->twist.linear.x;
- is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx));
- ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value.";
-
- resetCurrentPoseAndTwist();
-}
-
-TEST_F(EKFLocalizerTestSuite, measurementUpdatePoseWithCovariance)
-{
- pnh_.setParam("use_pose_with_covariance", true);
- ros::Duration(0.2).sleep();
- EKFLocalizer ekf_;
-
- ros::Publisher pub_pose =
- nh_.advertise("/in_pose_with_covariance", 1);
- geometry_msgs::PoseWithCovarianceStamped in_pose;
- in_pose.header.frame_id = "world";
- in_pose.pose.pose.position.x = 1.0;
- in_pose.pose.pose.position.y = 2.0;
- in_pose.pose.pose.position.z = 3.0;
- in_pose.pose.pose.orientation.x = 0.0;
- in_pose.pose.pose.orientation.y = 0.0;
- in_pose.pose.pose.orientation.z = 0.0;
- in_pose.pose.pose.orientation.w = 1.0;
- for (int i = 0; i < 36; ++i) {
- in_pose.pose.covariance[i] = 0.1;
- }
-
- /* test for valid value */
- const double pos_x = 99.3;
- in_pose.pose.pose.position.x = pos_x; // for vaild value
-
- for (int i = 0; i < 20; ++i) {
- in_pose.header.stamp = ros::Time::now();
- pub_pose.publish(in_pose);
- ros::spinOnce();
- ros::Duration(0.1).sleep();
- }
-
- ASSERT_FALSE(current_pose_ptr_ == nullptr);
- ASSERT_FALSE(current_twist_ptr_ == nullptr);
-
- double ekf_x = current_pose_ptr_->pose.position.x;
- bool is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x));
- ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value.";
- ASSERT_TRUE(std::fabs(ekf_x - pos_x) < 0.1)
- << "ekf pos x: " << ekf_x << " should be close to " << pos_x;
-
- /* test for invalid value */
- in_pose.pose.pose.position.x = NAN; // check for invalid values
- for (int i = 0; i < 10; ++i) {
- in_pose.header.stamp = ros::Time::now();
- pub_pose.publish(in_pose);
- ros::spinOnce();
- ros::Duration(0.1).sleep();
- }
- is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x));
- ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value.";
-
- resetCurrentPoseAndTwist();
-}
-
-TEST_F(EKFLocalizerTestSuite, measurementUpdateTwistWithCovariance)
-{
- EKFLocalizer ekf_;
-
- ros::Publisher pub_twist =
- nh_.advertise("/in_twist_with_covariance", 1);
- geometry_msgs::TwistWithCovarianceStamped in_twist;
- in_twist.header.frame_id = "base_link";
-
- /* test for valid value */
- const double vx = 12.3;
- in_twist.twist.twist.linear.x = vx; // for vaild value
- for (int i = 0; i < 36; ++i) {
- in_twist.twist.covariance[i] = 0.1;
- }
- for (int i = 0; i < 10; ++i) {
- in_twist.header.stamp = ros::Time::now();
- pub_twist.publish(in_twist);
- ros::spinOnce();
- ros::Duration(0.1).sleep();
- }
-
- ASSERT_FALSE(current_pose_ptr_ == nullptr);
- ASSERT_FALSE(current_twist_ptr_ == nullptr);
-
- double ekf_vx = current_twist_ptr_->twist.linear.x;
- bool is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx));
- ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value.";
- ASSERT_TRUE((ekf_vx - vx) < 0.1) << "vel x should be close to " << vx;
-
- /* test for invalid value */
- in_twist.twist.twist.linear.x = NAN; // check for invalid values
- for (int i = 0; i < 10; ++i) {
- in_twist.header.stamp = ros::Time::now();
- pub_twist.publish(in_twist);
- ros::spinOnce();
- ros::Duration(0.1).sleep();
- }
-
- ekf_vx = current_twist_ptr_->twist.linear.x;
- is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx));
- ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value.";
-}
-
-int main(int argc, char ** argv)
-{
- testing::InitGoogleTest(&argc, argv);
- ros::init(argc, argv, "EKFLocalizerTestSuite");
-
- return RUN_ALL_TESTS();
-}
\ No newline at end of file
diff --git a/localization/ekf_localizer/test/test_ekf_localizer.test b/localization/ekf_localizer/test/test_ekf_localizer.test
deleted file mode 100644
index 1878685aff756..0000000000000
--- a/localization/ekf_localizer/test/test_ekf_localizer.test
+++ /dev/null
@@ -1,5 +0,0 @@
-
-
-
-
-
\ No newline at end of file