Make ekf a ROS-independent

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 <Eigen/Core>
+#include <Eigen/Geometry>
+#include <iostream>
+
+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<double, num_state_, 1> x_;
+  Eigen::Matrix<double, num_error_state_, num_error_state_> 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_

include/kalman_filter_localization/ekf_localization_component.hpp

@@ -70,6 +70,8 @@ extern "C" {
 }  // extern "C"
 #endif
 
+#include <kalman_filter_localization/ekf.hpp>
+
 #include <rclcpp/rclcpp.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/transform.hpp>
@@ -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<geometry_msgs::msg::PoseStamped>::SharedPtr sub_initial_pose_;
   rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub_imu_;
   rclcpp::Subscription<nav_msgs::msg::Odometry>::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