diff --git a/include/kalman_filter_localization/ekf.hpp b/include/kalman_filter_localization/ekf.hpp new file mode 100644 index 0000000..d7b8f4b --- /dev/null +++ b/include/kalman_filter_localization/ekf.hpp @@ -0,0 +1,237 @@ +// Copyright (c) 2020, Ryohei Sasaki +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +#ifndef KALMAN_FILTER_LOCALIZATION__EKF_HPP_ +#define KALMAN_FILTER_LOCALIZATION__EKF_HPP_ + +#include +#include +#include + +class EKFEstimator +{ +public: + EKFEstimator() + : P_(Eigen::MatrixXd::Identity(num_error_state_, num_error_state_) * 100), + var_imu_w_{0.33}, + var_imu_acc_{0.33}, + tau_gyro_bias_{1.0} + { + /* x = [p v q] = [x y z vx vy vz qx qy qz qw] */ + x_ << 0, 0, 0, 0, 0, 0, 0, 0, 0, 1; + } + +/* state +* x = [p v q] = [x y z vx vy vz qx qy qz qw] +* dx = [dp dv dth] = [dx dy dz dvx dvy dvz dthx dthy dthz] +* +* pos_k = pos_{k-1} + vel_k * dt + (1/2) * (Rot(q_{k-1}) acc_{k-1}^{imu} - g) *dt^2 +* vel_k = vel_{k-1} + (Rot(quat_{k-1})) acc_{k-1}^{imu} - g) *dt +* quat_k = Rot(w_{k-1}^{imu}*dt)*quat_{k-1} +* +* covariance +* P_{k} = F_k P_{k-1} F_k^T + L Q_k L^T +*/ + void predictionUpdate( + const double current_time_imu, + const Eigen::Vector3d & gyro, + const Eigen::Vector3d & linear_acceleration + ) + { + double dt_imu = current_time_imu - previous_time_imu_; + previous_time_imu_ = current_time_imu; + if (dt_imu > 0.5 /* [sec] */) { + std::cout << "imu time interval is too large" << std::endl; + return; + } + + Eigen::Quaterniond quat_wdt = Eigen::Quaterniond( + Eigen::AngleAxisd(gyro.x() * dt_imu, Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(gyro.y() * dt_imu, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(gyro.z() * dt_imu, Eigen::Vector3d::UnitZ())); + Eigen::Vector3d acc = Eigen::Vector3d( + linear_acceleration.x(), + linear_acceleration.y(), + linear_acceleration.z()); + + // state + Eigen::Quaterniond previous_quat = + Eigen::Quaterniond(x_(STATE::QW), x_(STATE::QX), x_(STATE::QY), x_(STATE::QZ)); + Eigen::MatrixXd rot_mat = previous_quat.toRotationMatrix(); + + // pos + x_.segment(STATE::X, 3) = x_.segment(STATE::X, 3) + dt_imu * x_.segment(STATE::VX, 3) + + 0.5 * dt_imu * dt_imu * (rot_mat * acc - gravity_); + // vel + x_.segment(STATE::VX, 3) = x_.segment(STATE::VX, 3) + dt_imu * (rot_mat * acc - gravity_); + // quat + Eigen::Quaterniond predicted_quat = quat_wdt * previous_quat; + x_.segment(STATE::QX, 4) = Eigen::Vector4d( + predicted_quat.x(), predicted_quat.y(), predicted_quat.z(), predicted_quat.w()); + + // F + Eigen::MatrixXd F = Eigen::MatrixXd::Identity(num_error_state_, num_error_state_); + F.block<3, 3>(0, 3) = dt_imu * Eigen::MatrixXd::Identity(3, 3); + Eigen::Matrix3d acc_skew; + acc_skew << + 0, -acc(2), acc(1), + acc(2), 0, -acc(0), + -acc(1), acc(0), 0; + F.block<3, 3>(3, 6) = rot_mat * (-acc_skew) * dt_imu; + + // Q + Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(6, 6); + Q.block<3, 3>(0, 0) = var_imu_acc_ * Q.block<3, 3>(0, 0); + Q.block<3, 3>(3, 3) = var_imu_w_ * Q.block<3, 3>(3, 3); + Q = Q * (dt_imu * dt_imu); + + // L + Eigen::MatrixXd L = Eigen::MatrixXd::Zero(9, 6); + L.block<3, 3>(3, 0) = Eigen::MatrixXd::Identity(3, 3); + L.block<3, 3>(6, 3) = Eigen::MatrixXd::Identity(3, 3); + + P_ = F * P_ * F.transpose() + L * Q * L.transpose(); + } + +/* +* y = pobs = [xobs yobs zobs] +* +* K = P_k H^T (H P_k H^T + R)^{-1} +* +* dx = K (y_k - p_k ) +* +* p_x = p_{k-1} + dp_k +* v_k = v_{k-1} + dv_k +* q_k = Rot(dth) q_{k-1} +* +* P_k = (I - KH)*P_{k-1} +*/ + void observationUpdate( + const Eigen::Vector3d & y, + const Eigen::Vector3d & variance + ) + { + // error state + Eigen::Matrix3d R; + R << + variance.x(), 0, 0, + 0, variance.y(), 0, + 0, 0, variance.z(); + Eigen::MatrixXd H = Eigen::MatrixXd::Zero(3, num_error_state_); + H.block<3, 3>(0, 0) = Eigen::MatrixXd::Identity(3, 3); + Eigen::MatrixXd K = P_ * H.transpose() * (H * P_ * H.transpose() + R).inverse(); + Eigen::VectorXd dx = K * (y - x_.segment(STATE::X, 3)); + + // state + x_.segment(STATE::X, 3) = x_.segment(STATE::X, 3) + dx.segment(ERROR_STATE::DX, 3); + x_.segment(STATE::VX, 3) = x_.segment(STATE::VX, 3) + dx.segment(ERROR_STATE::DVX, 3); + double norm_quat = sqrt( + pow(dx(ERROR_STATE::DTHX), 2) + + pow(dx(ERROR_STATE::DTHY), 2) + + pow(dx(ERROR_STATE::DTHZ), 2)); + if (norm_quat < 1e-10) { + x_.segment(STATE::QX, 4) = Eigen::Vector4d(0, 0, 0, cos(norm_quat / 2)); + } else { + x_.segment(STATE::QX, 4) = Eigen::Vector4d( + sin(norm_quat / 2) * dx(ERROR_STATE::DTHX) / norm_quat, + sin(norm_quat / 2) * dx(ERROR_STATE::DTHY) / norm_quat, + sin(norm_quat / 2) * dx(ERROR_STATE::DTHZ) / norm_quat, + cos(norm_quat / 2)); + } + + P_ = (Eigen::MatrixXd::Identity(num_error_state_, num_error_state_) - K * H) * P_; + } + + void setTauGyroBias(const double tau_gyro_bias) + { + tau_gyro_bias_ = tau_gyro_bias; + } + + void setVarImuGyro(const double var_imu_w) + { + var_imu_w_ = var_imu_w; + } + + void setVarImuAcc(const double var_imu_acc) + { + var_imu_acc_ = var_imu_acc; + } + + void setInitialX(Eigen::VectorXd x) + { + x_ = x; + } + + Eigen::VectorXd getX() + { + return x_; + } + + Eigen::MatrixXd getCoveriance() + { + return P_; + } + + int getNumState() + { + return num_state_; + } + +private: + double previous_time_imu_; + double var_imu_w_; + double var_imu_acc_; + + static const int num_state_{10}; + static const int num_error_state_{9}; + + Eigen::Matrix x_; + Eigen::Matrix P_; + + const Eigen::Vector3d gravity_{0, 0, 9.80665}; + + double tau_gyro_bias_; + + enum STATE + { + X = 0, Y = 1, Z = 2, + VX = 3, VY = 4, VZ = 5, + QX = 6, QY = 7, QZ = 8, QW = 9, + }; + enum ERROR_STATE + { + DX = 0, DY = 1, DZ = 2, + DVX = 3, DVY = 4, DVZ = 5, + DTHX = 6, DTHY = 7, DTHZ = 8, + }; +}; + +#endif // KALMAN_FILTER_LOCALIZATION__EKF_HPP_ diff --git a/include/kalman_filter_localization/ekf_localization_component.hpp b/include/kalman_filter_localization/ekf_localization_component.hpp index e8dbf4b..856e2c2 100644 --- a/include/kalman_filter_localization/ekf_localization_component.hpp +++ b/include/kalman_filter_localization/ekf_localization_component.hpp @@ -70,6 +70,8 @@ extern "C" { } // extern "C" #endif +#include + #include #include #include @@ -108,28 +110,24 @@ class EkfLocalizationComponent : public rclcpp::Node std::string odom_topic_; std::string gnss_pose_topic_; int pub_period_; - int num_state_{10}; - int num_error_state_{9}; - int num_obs_; + double var_imu_w_; double var_imu_acc_; double var_gnss_xy_; double var_gnss_z_; - double var_gnss_[3]; + Eigen::Vector3d var_gnss_; double var_odom_xyz_; - double var_odom_[3]; + Eigen::Vector3d var_odom_; bool use_gnss_; bool use_odom_; - bool initial_pose_recieved_; + bool initial_pose_recieved_{false}; - double previous_time_imu_{-1}; geometry_msgs::msg::PoseStamped current_pose_; rclcpp::Time current_stamp_; - Eigen::VectorXd x_; - Eigen::MatrixXd P_; - Eigen::Vector3d gravity_{0, 0, 9.80665}; + EKFEstimator ekf_; + rclcpp::Subscription::SharedPtr sub_initial_pose_; rclcpp::Subscription::SharedPtr sub_imu_; rclcpp::Subscription::SharedPtr sub_odom_; @@ -139,10 +137,10 @@ class EkfLocalizationComponent : public rclcpp::Node rclcpp::Clock clock_; tf2_ros::Buffer tfbuffer_; tf2_ros::TransformListener listener_; - void predictUpdate(const sensor_msgs::msg::Imu input_imu_msg); + void predictUpdate(const sensor_msgs::msg::Imu imu_msg); void measurementUpdate( - const geometry_msgs::msg::PoseStamped input_pose_msg, - const double variance[]); + const geometry_msgs::msg::PoseStamped pose_msg, + const Eigen::Vector3d variance); void broadcastPose(); enum STATE @@ -151,12 +149,6 @@ class EkfLocalizationComponent : public rclcpp::Node VX = 3, VY = 4, VZ = 5, QX = 6, QY = 7, QZ = 8, QW = 9, }; - enum ERROR_STATE - { - DX = 0, DY = 1, DZ = 2, - DVX = 3, DVY = 4, DVZ = 5, - DTHX = 6, DTHY = 7, DTHZ = 8, - }; }; } // namespace kalman_filter_localization diff --git a/src/ekf_localization_component.cpp b/src/ekf_localization_component.cpp index 5b7863b..dfee8c8 100644 --- a/src/ekf_localization_component.cpp +++ b/src/ekf_localization_component.cpp @@ -129,20 +129,10 @@ EkfLocalizationComponent::EkfLocalizationComponent(const rclcpp::NodeOptions & o } ); - // Init - x_ = Eigen::VectorXd::Zero(num_state_); - x_(STATE::QW) = 1; - // todo:set initial value properly - P_ = Eigen::MatrixXd::Identity(num_error_state_, num_error_state_) * 100; - - var_gnss_[0] = var_gnss_xy_; - var_gnss_[1] = var_gnss_xy_; - var_gnss_[2] = var_gnss_z_; - var_odom_[0] = var_odom_xyz_; - var_odom_[1] = var_odom_xyz_; - var_odom_[2] = var_odom_xyz_; - - initial_pose_recieved_ = false; + ekf_.setVarImuGyro(var_imu_w_); + ekf_.setVarImuAcc(var_imu_acc_); + var_gnss_ << var_gnss_xy_, var_gnss_xy_, var_gnss_z_; + var_odom_ << var_odom_xyz_, var_odom_xyz_, var_odom_xyz_; // Setup Publisher std::string output_pose_name = get_name() + std::string("/current_pose"); @@ -156,16 +146,16 @@ EkfLocalizationComponent::EkfLocalizationComponent(const rclcpp::NodeOptions & o std::cout << "initial pose callback" << std::endl; initial_pose_recieved_ = true; current_pose_ = *msg; - x_(STATE::X) = current_pose_.pose.position.x; - x_(STATE::Y) = current_pose_.pose.position.y; - x_(STATE::Z) = current_pose_.pose.position.z; - x_(STATE::QX) = current_pose_.pose.orientation.x; - x_(STATE::QY) = current_pose_.pose.orientation.y; - x_(STATE::QZ) = current_pose_.pose.orientation.z; - x_(STATE::QW) = current_pose_.pose.orientation.w; - std::cout << "initial_x" << std::endl; - std::cout << x_ << std::endl; - std::cout << "----------------------" << std::endl; + + Eigen::VectorXd x = Eigen::VectorXd::Zero(ekf_.getNumState()); + x(STATE::X) = current_pose_.pose.position.x; + x(STATE::Y) = current_pose_.pose.position.y; + x(STATE::Z) = current_pose_.pose.position.z; + x(STATE::QX) = current_pose_.pose.orientation.x; + x(STATE::QY) = current_pose_.pose.orientation.y; + x(STATE::QZ) = current_pose_.pose.orientation.z; + x(STATE::QW) = current_pose_.pose.orientation.w; + ekf_.setInitialX(x); }; auto imu_callback = @@ -245,141 +235,50 @@ EkfLocalizationComponent::EkfLocalizationComponent(const rclcpp::NodeOptions & o std::bind(&EkfLocalizationComponent::broadcastPose, this)); } -/* state -* x = [p v q] = [x y z vx vy vz qx qy qz qw] -* dx = [dp dv dth] = [dx dy dz dvx dvy dvz dthx dthy dthz] -* -* pos_k = pos_{k-1} + vel_k * dt + (1/2) * (Rot(q_{k-1}) acc_{k-1}^{imu} - g) *dt^2 -* vel_k = vel_{k-1} + (Rot(quat_{k-1})) acc_{k-1}^{imu} - g) *dt -* quat_k = Rot(w_{k-1}^{imu}*dt)*quat_{k-1} -* -* covariance -* P_{k} = F_k P_{k-1} F_k^T + L Q_k L^T -*/ void EkfLocalizationComponent::predictUpdate(const sensor_msgs::msg::Imu imu_msg) { current_stamp_ = imu_msg.header.stamp; - // dt_imu double current_time_imu = imu_msg.header.stamp.sec + imu_msg.header.stamp.nanosec * 1e-9; - double dt_imu = current_time_imu - previous_time_imu_; - previous_time_imu_ = current_time_imu; - if (dt_imu > 0.5 /* [sec] */) { - RCLCPP_WARN(this->get_logger(), "imu time interval is too large"); - return; - } - - // state - Eigen::Quaterniond previous_quat = - Eigen::Quaterniond(x_(STATE::QW), x_(STATE::QX), x_(STATE::QY), x_(STATE::QZ)); - Eigen::MatrixXd rot_mat = previous_quat.toRotationMatrix(); - Eigen::Vector3d acc = Eigen::Vector3d( + Eigen::Vector3d gyro = Eigen::Vector3d( + imu_msg.angular_velocity.x, + imu_msg.angular_velocity.y, + imu_msg.angular_velocity.z); + Eigen::Vector3d linear_acceleration = Eigen::Vector3d( imu_msg.linear_acceleration.x, imu_msg.linear_acceleration.y, imu_msg.linear_acceleration.z); - // pos - x_.segment(STATE::X, 3) = x_.segment(STATE::X, 3) + dt_imu * x_.segment(STATE::VX, 3) + - 0.5 * dt_imu * dt_imu * (rot_mat * acc - gravity_); - // vel - x_.segment(STATE::VX, 3) = x_.segment(STATE::VX, 3) + dt_imu * (rot_mat * acc - gravity_); - // quat - Eigen::Quaterniond quat_wdt = Eigen::Quaterniond( - Eigen::AngleAxisd(imu_msg.angular_velocity.x * dt_imu, Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(imu_msg.angular_velocity.y * dt_imu, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(imu_msg.angular_velocity.z * dt_imu, Eigen::Vector3d::UnitZ())); - Eigen::Quaterniond predicted_quat = quat_wdt * previous_quat; - x_.segment(STATE::QX, 4) = Eigen::Vector4d( - predicted_quat.x(), predicted_quat.y(), predicted_quat.z(), predicted_quat.w()); - - // F - Eigen::MatrixXd F = Eigen::MatrixXd::Identity(num_error_state_, num_error_state_); - F.block<3, 3>(0, 3) = dt_imu * Eigen::MatrixXd::Identity(3, 3); - Eigen::Matrix3d acc_skew; - acc_skew << - 0, -acc(2), acc(1), - acc(2), 0, -acc(0), - -acc(1), acc(0), 0; - F.block<3, 3>(3, 6) = rot_mat * (-acc_skew) * dt_imu; - // Q - Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(6, 6); - Q.block<3, 3>(0, 0) = var_imu_acc_ * Q.block<3, 3>(0, 0); - Q.block<3, 3>(3, 3) = var_imu_w_ * Q.block<3, 3>(3, 3); - Q = Q * (dt_imu * dt_imu); - - // L - Eigen::MatrixXd L = Eigen::MatrixXd::Zero(9, 6); - L.block<3, 3>(3, 0) = Eigen::MatrixXd::Identity(3, 3); - L.block<3, 3>(6, 3) = Eigen::MatrixXd::Identity(3, 3); - - P_ = F * P_ * F.transpose() + L * Q * L.transpose(); + ekf_.predictionUpdate(current_time_imu, gyro, linear_acceleration); } -/* -* y = pobs = [xobs yobs zobs] -* -* K = P_k H^T (H P_k H^T + R)^{-1} -* -* dx = K (y_k - p_k ) -* -* p_x = p_{k-1} + dp_k -* v_k = v_{k-1} + dv_k -* q_k = Rot(dth) q_{k-1} -* -* P_k = (I - KH)*P_{k-1} -*/ + void EkfLocalizationComponent::measurementUpdate( - const geometry_msgs::msg::PoseStamped input_pose_msg, - const double variance[]) + const geometry_msgs::msg::PoseStamped pose_msg, + const Eigen::Vector3d variance) { - // error state - current_stamp_ = input_pose_msg.header.stamp; - Eigen::Matrix3d R; - R << - variance[0], 0, 0, - 0, variance[1], 0, - 0, 0, variance[2]; - Eigen::MatrixXd H = Eigen::MatrixXd::Zero(3, num_error_state_); - H.block<3, 3>(0, 0) = Eigen::MatrixXd::Identity(3, 3); - Eigen::MatrixXd K = P_ * H.transpose() * (H * P_ * H.transpose() + R).inverse(); - Eigen::Vector3d y = Eigen::Vector3d(input_pose_msg.pose.position.x, - input_pose_msg.pose.position.y, - input_pose_msg.pose.position.z); - Eigen::VectorXd dx = K * (y - x_.segment(STATE::X, 3)); - - // state - x_.segment(STATE::X, 3) = x_.segment(STATE::X, 3) + dx.segment(ERROR_STATE::DX, 3); - x_.segment(STATE::VX, 3) = x_.segment(STATE::VX, 3) + dx.segment(ERROR_STATE::DVX, 3); - double norm_quat = sqrt( - pow(dx(ERROR_STATE::DTHX), 2) + - pow(dx(ERROR_STATE::DTHY), 2) + - pow(dx(ERROR_STATE::DTHZ), 2)); - if (norm_quat < 1e-10) { - x_.segment(STATE::QX, 4) = Eigen::Vector4d(0, 0, 0, cos(norm_quat / 2)); - } else { - x_.segment(STATE::QX, 4) = Eigen::Vector4d( - sin(norm_quat / 2) * dx(ERROR_STATE::DTHX) / norm_quat, - sin(norm_quat / 2) * dx(ERROR_STATE::DTHY) / norm_quat, - sin(norm_quat / 2) * dx(ERROR_STATE::DTHZ) / norm_quat, - cos(norm_quat / 2)); - } + current_stamp_ = pose_msg.header.stamp; + Eigen::Vector3d y = Eigen::Vector3d(pose_msg.pose.position.x, + pose_msg.pose.position.y, + pose_msg.pose.position.z); - P_ = (Eigen::MatrixXd::Identity(num_error_state_, num_error_state_) - K * H) * P_; + ekf_.observationUpdate(y, variance); } void EkfLocalizationComponent::broadcastPose() { if (initial_pose_recieved_) { + auto x = ekf_.getX(); current_pose_.header.stamp = current_stamp_; current_pose_.header.frame_id = reference_frame_id_; - current_pose_.pose.position.x = x_(STATE::X); - current_pose_.pose.position.y = x_(STATE::Y); - current_pose_.pose.position.z = x_(STATE::Z); - current_pose_.pose.orientation.x = x_(STATE::QX); - current_pose_.pose.orientation.y = x_(STATE::QY); - current_pose_.pose.orientation.z = x_(STATE::QZ); - current_pose_.pose.orientation.w = x_(STATE::QW); + current_pose_.pose.position.x = x(STATE::X); + current_pose_.pose.position.y = x(STATE::Y); + current_pose_.pose.position.z = x(STATE::Z); + current_pose_.pose.orientation.x = x(STATE::QX); + current_pose_.pose.orientation.y = x(STATE::QY); + current_pose_.pose.orientation.z = x(STATE::QZ); + current_pose_.pose.orientation.w = x(STATE::QW); current_pose_pub_->publish(current_pose_); } }