Add files via upload

CMakeLists.txt

@@ -0,0 +1,48 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(tightly_coupled)
+
+find_package(catkin REQUIRED COMPONENTS
+  geometry_msgs
+  mavros_msgs
+  roscpp
+  rospy
+  std_msgs
+  sensor_msgs
+  nlink_parser
+)
+
+find_package(Eigen3 REQUIRED)
+
+generate_messages(
+   DEPENDENCIES
+   std_msgs 
+   geometry_msgs
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIR}
+)
+
+catkin_package(
+  LIBRARIES tightly_coupled_ekf tightly_coupled_eskf tightly_coupled_liekf
+  CATKIN_DEPENDS geometry_msgs sensor_msgs roscpp rospy std_msgs message_runtime nlink_parser
+  INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}
+)
+
+add_library(tightly_coupled_ekf src/tightlyCoupledFusionEKF.cpp)
+add_executable(ekfNode src/ekfNode.cpp)
+target_link_libraries(ekfNode ${catkin_LIBRARIES} tightly_coupled_ekf)
+
+add_library(tightly_coupled_eskf src/tightlyCoupledFusionESKF.cpp)
+add_executable(eskfNode src/eskfNode.cpp)
+target_link_libraries(eskfNode ${catkin_LIBRARIES} tightly_coupled_eskf)
+
+add_library(tightly_coupled_liekf src/tightlyCoupledFusionLIEKF.cpp)
+add_executable(liekfNode src/liekfNode.cpp)
+target_link_libraries(liekfNode ${catkin_LIBRARIES} tightly_coupled_liekf)
+
+add_library(tightly_coupled_ukf src/tightlyCoupledFusionUKF.cpp)
+add_executable(ukfNode src/ukfNode.cpp)
+target_link_libraries(ukfNode ${catkin_LIBRARIES} tightly_coupled_ukf)

README.md

@@ -1,16 +1,6 @@
-# Experimental Comparison of Tightly and Loosely Coupled Kalman Filters for 6D Pose Estimation Using UWB and IMU Sensors
-
-we present a novel approach for robot pose estimation in SE(3) using a tightly coupled integration
-of Ultra-Wideband (UWB) and Inertial Measurement Unit (IMU) data. Our method leverages Extended Kalman
-Filter (EKF) and Error-State Kalman Filter (ESKF) to accurately estimate the pose, including position and orientation,
-of a mobile robot in three-dimensional space. The tightly coupled approach ensures robust performance even in environments
-where UWB data quality is poor or in NLOS(Non-Line-of-Sight) conditions by utilizing raw UWB data and
-effectively fusing with IMU. The proposed method is validated through extensive simulations and real-world experiments,
-demonstrating significant improvements in accuracy and robustness compared to the loosely coupled method. Our results
-show that the tightly coupled approach, with its enhanced data fusion capability, provides superior z-axis estimation
-performance. This makes it particularly advantageous in applications requiring accurate SE(3) pose estimation.
-
-##  6D pose estimation using tightly coupled UWB/IMU fusion using filtering method(EKF/ESKF/UKF/LIEKF)
+# Tightly Coupled UWB/IMU Fusion
+
+ 6D pose estimation using tightly coupled UWB/IMU fusion using filtering method(EKF/ESKF/UKF/LIEKF)
  - Extended Kalman filter
  - Error-state Kalman filter
  - Unscented  Kalman filter
@@ -53,5 +43,5 @@ Experimental results
   </tr>
 </table>
 
-
-
+## Experimental Comparison of Tightly and Loosely Coupled Kalman Filters for 6D Pose Estimation Using UWB and IMU Sensors
+ Paper Will be uploaded..

config/params.yaml

@@ -0,0 +1,10 @@
+stdV: 0.0015387262937311438  # standard deviation of linear accelerometer
+stdW: 1.0966208586777776e-06 # standard deviation of angular Velocity
+
+anchorPositions: [0, 0, 8.86, 8.86, 0, 0, 8.86, 8.86,
+                 0, 8.00, 8.00, 0, 0, 8.00, 8.00, 0,
+                 0.2, 0.2, 0.2, 0.2, 2.40, 2.40, 2.40, 2.40]
+
+# anchorPositions: [x1,  x2,  x3,  x4,  x5,  x6,  x7,  x8,
+#                  y1,  y2,  y3,  y4,  y5,  y6,  y7,  y8,
+#                  z1,  z2,  z3,  z4,  z5,  z6,  z7,  z8] 

include/tightlyCoupledFusionEKF.h

@@ -0,0 +1,37 @@
+#pragma once
+#include "types.h"
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+#include <iostream>
+#include <vector>
+
+class TightlyCoupled {
+public:
+    TightlyCoupled();
+    ~TightlyCoupled();
+    void setImuVar(const double stdV, const double stdW);
+    void setDt(const double delta_t);
+    void setAnchorPositions(const Eigen::Matrix<double, 3, 8> &anchorPositions);
+    Eigen::Matrix3d Exp(const Eigen::Vector3d &omega);
+    void prediction(const ImuData<double> &imu_data);
+    void setZ(const UwbData<double> currUwbData);    
+    void motionModel(const ImuData<double> &imu_data);
+    void motionModelJacobian(const ImuData<double> &imu_data);
+    Eigen::Matrix3d vectorToSkewSymmetric(const Eigen::Vector3d &vector);
+    void measurementModel();
+    void measurementModelJacobian ();
+    StateforEKF<double> correction();
+    void setState(StateforEKF<double> &State);
+    StateforEKF<double> getState();
+
+private:
+    Eigen::Matrix<double, 15, 15> covP, covQ, jacobianMatF;
+    Eigen::Matrix<double, 8, 8> covR;
+    Eigen::Matrix<double, 8, 1> vecZ, vecH;
+    Eigen::Matrix<double, 3, 8> anchorPositions;
+    Eigen::Matrix<double, 8, 15> jacobianMatH;
+    double dt, TOL;
+    Eigen::Matrix<double, 3, 1> _g;
+    StateforEKF<double> STATE;
+
+};

include/tightlyCoupledFusionESKF.h

@@ -0,0 +1,40 @@
+#pragma once
+#include "types.h"
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+#include <iostream>
+#include <vector>
+
+class TightlyCoupled {
+public:
+    TightlyCoupled();
+    ~TightlyCoupled();
+    void setImuVar(const double stdV, const double stdW);
+    void setDt(const double delta_t);
+    void setAnchorPositions(const Eigen::Matrix<double, 3, 8> &anchorPositions);
+    Eigen::Matrix3d Exp(const Eigen::Vector3d &omega);
+    Eigen::Quaterniond getQuaFromAA(Eigen::Matrix<double, 3, 1> vec);
+    void prediction(const ImuData<double> &imu_data);
+    void setZ(const UwbData<double> currUwbData);    
+    void motionModel(const ImuData<double> &imu_data);
+    void motionModelJacobian(const ImuData<double> &imu_data);
+    Eigen::Matrix3d vectorToSkewSymmetric(const Eigen::Vector3d &vector);
+    void measurementModel();
+    void updateH();
+    void measurementModelJacobian ();
+    StateforESKF<double> correction();
+    void setState(StateforESKF<double> &State);
+    StateforESKF<double> getState();
+
+private:
+    Eigen::Matrix<double, 15, 15> covP, covQ, jacobianMatF;
+    Eigen::Matrix<double, 8, 8> covR;
+    Eigen::Matrix<double, 8, 1> vecZ, vecH;
+    Eigen::Matrix<double, 3, 8> anchorPositions;
+    Eigen::Matrix<double, 8, 15> jacobianMatH;
+    Eigen::Matrix<double, 16, 15> XdX;
+    double dt, TOL;
+    Eigen::Matrix<double, 3, 1> _g;
+    StateforESKF<double> STATE;
+
+};

include/tightlyCoupledFusionUKF.h

@@ -0,0 +1,104 @@
+#pragma once
+#include "types.h"
+#include <Eigen/Dense>
+#include <Eigen/Geometry> 
+#include <iostream>
+#include <vector>
+
+class TightlyCoupled {
+public:
+    TightlyCoupled();
+    ~TightlyCoupled();
+
+    void setImuVar(const double stdV, const double stdW);
+    void setDt(const double delta_t);
+    void setAnchorPositions(const Eigen::Matrix<double, 3, 8> &anchorPositions);
+    void setZ(const UwbData<double>& currUwbData);    
+
+    void prediction(const ImuData<double> &imu_data);
+    void update(const UwbData<double>& uwb_data);
+
+    StateforEKF<double> getState();
+    void setState(const StateforEKF<double> &State);
+
+private:
+    Eigen::Matrix3d vectorToSkewSymmetric(const Eigen::Vector3d &vector);
+    Eigen::Matrix3d Exp(const Eigen::Vector3d &omega);
+    Eigen::Vector3d Log(const Eigen::Matrix3d &Rot);
+    Eigen::Vector3d vee(const Eigen::Matrix3d &phi);
+
+    Eigen::VectorXd phiInv(const StateforEKF<double> &state, const StateforEKF<double> &hatState);
+    StateforEKF<double> phi(const StateforEKF<double> &state, const Eigen::VectorXd &xi);
+    Eigen::VectorXd h(const StateforEKF<double> &state);
+
+    void generateSigmaPoints();
+    void predictSigmaPoints(const ImuData<double> &imu_data);
+    void predictMeanAndCovariance();
+    void predictMeasurement();
+    void updateState(const UwbData<double>& uwb_data);
+
+    Eigen::Matrix<double, 15, 15> covP;
+    Eigen::Matrix<double, 15, 15> covQ;
+    Eigen::Matrix<double, 8, 8> covR;
+
+    Eigen::Matrix<double, 3, 8> anchorPositions;
+    Eigen::VectorXd weightsMean;
+    Eigen::VectorXd weightsCov;
+    double lambda;
+
+    double dt;
+    double TOL;
+    Eigen::Vector3d _g;
+    StateforEKF<double> STATE;
+    std::vector<StateforEKF<double>> sigmaPoints;
+    std::vector<StateforEKF<double>> predictedSigmaPoints;
+    std::vector<Eigen::VectorXd> predictedMeasurements;
+
+    Eigen::VectorXd z_pred;
+    Eigen::MatrixXd S;
+    Eigen::MatrixXd Tc;
+};
+
+
+// #pragma once
+// #include "types.h"
+// #include <Eigen/Dense>
+// #include <Eigen/Geometry> 
+// #include <iostream>
+// #include <vector>
+
+// class TightlyCoupled {
+// public:
+//     TightlyCoupled();
+//     ~TightlyCoupled();
+//     void setImuVar(const double stdV, const double stdW);
+//     void setDt(const double delta_t);
+//     void setAnchorPositions(const Eigen::Matrix<double, 3, 8> &anchorPositions);
+//     Eigen::Matrix3d Exp(const Eigen::Vector3d &omega);
+//     void prediction(const ImuData<double> &imu_data);
+//     void setZ(const UwbData<double> currUwbData);    
+//     void motionModel(const ImuData<double> &imu_data);
+//     void motionModelJacobian(const ImuData<double> &imu_data);
+//     Eigen::Matrix3d vectorToSkewSymmetric(const Eigen::Vector3d &vector);
+//     void measurementModel();
+//     void measurementModelJacobian ();
+//     StateforEKF<double> correction();
+//     void setState(StateforEKF<double> &State);
+//     StateforEKF<double> getState();
+
+// private:
+//     Eigen::Matrix<double, 15, 15> covP, jacobianMatF;
+//     Eigen::Matrix<double, 12, 12> covQ;
+//     Eigen::Matrix<double, 8, 8> covR;
+//     Eigen::Vector<double, 3> upIdx;
+//     Eigen::Matrix<double, 12, 12> cholQ;
+//     Eigen::Vector<double,15> redIdxs;
+
+//     Eigen::Matrix<double, 8, 1> vecZ, vecH;
+//     Eigen::Matrix<double, 3, 8> anchorPositions;
+//     Eigen::Matrix<double, 8, 15> jacobianMatH;
+//     double dt, TOL;
+//     Eigen::Matrix<double, 3, 1> _g;
+//     StateforEKF<double> STATE;
+
+// };

include/types.h

@@ -0,0 +1,66 @@
+#pragma once
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
+template <typename scalar>
+struct ImuData {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    double timestamp;
+    Eigen::Matrix<scalar, 3, 1> acc;
+    Eigen::Matrix<scalar, 3, 1> gyr;
+};
+
+template <typename scalar>
+struct UwbData {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    double timestamp;
+    Eigen::Matrix<scalar, 8, 1> distance;
+};
+
+template <typename scalar>
+struct StateforEKF{
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    Eigen::Matrix<scalar, 3, 1> p;
+    Eigen::Matrix<scalar, 3, 3> R;
+    Eigen::Matrix<scalar, 3, 1> v;
+    Eigen::Matrix<scalar, 3, 1> a_b;
+    Eigen::Matrix<scalar, 3, 1> w_b;
+};
+
+template <typename scalar>
+struct StateforESKF{
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    Eigen::Matrix<scalar, 3, 1> p;
+    Eigen::Quaterniond q;  
+    Eigen::Matrix<scalar, 3, 1> v;
+    Eigen::Matrix<scalar, 3, 1> a_b;
+    Eigen::Matrix<scalar, 3, 1> w_b;
+};
+
+struct WEIGHTS {
+    struct W {
+        double sqrtDlambda;
+        double wj;
+        double wm;
+        double w0;
+
+        W(double l, double alpha) {
+            double m = (std::pow(alpha, 2) - 1) * l;
+            sqrtDlambda = std::sqrt(l + m);
+            wj = 1 / (2 * (l + m));
+            wm = m / (l + m);
+            w0 = m / (l + m) + 3 - std::pow(alpha, 2);
+        }
+    };
+
+    W redD;
+    W q;
+    W upD;
+
+    WEIGHTS(double red_d_val, double q_val, double up_d_val, double* alpha) :
+    //             15                12            3        diag[0.001, 0.001, 0.001]   
+    redD(red_d_val, alpha[0]),
+    q(q_val, alpha[1]),
+    upD(up_d_val, alpha[2])
+ {}
+};

launch/ekf.launch

@@ -0,0 +1,5 @@
+<launch>
+  <rosparam file="$(find tightly_coupled)/config/params.yaml" command="load" ns="tightly_coupled_ekf"/>
+  <node name="tightly_coupled_ekf" pkg="tightly_coupled" type="ekfNode" output="screen"/>
+
+</launch>

launch/eskf.launch

@@ -0,0 +1,5 @@
+<launch>
+  <rosparam file="$(find tightly_coupled)/config/params.yaml" command="load" ns="tightly_coupled_eskf"/>
+  <node name="tightly_coupled_eskf" pkg="tightly_coupled" type="eskfNode" output="screen"/>
+
+</launch>

launch/liekf.launch

@@ -0,0 +1,5 @@
+<launch>
+  <rosparam file="$(find tightly_coupled)/config/params.yaml" command="load" ns="tightly_coupled_liekf"/>
+  <node name="tightly_coupled_liekf" pkg="tightly_coupled" type="liekfNode" output="screen"/>
+
+</launch>

launch/ukf.launch

@@ -0,0 +1,5 @@
+<launch>
+  <rosparam file="$(find tightly_coupled)/config/params.yaml" command="load" ns="tightly_coupled_ukf"/>
+  <node name="tightly_coupled_ukf" pkg="tightly_coupled" type="ukfNode" output="screen"/>
+
+</launch>