1.Update readme;

2.Fix some code error.

3rd/GeographicLib/CMakeLists.txt

@@ -1,36 +1,37 @@
-project (GeographicLib)
+project(GeographicLib)
 
 # Version information
-set (PROJECT_VERSION_MAJOR 1)
-set (PROJECT_VERSION_MINOR 49)
-set (PROJECT_VERSION_PATCH 0)
-set (PROJECT_VERSION "${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}")
+set(PROJECT_VERSION_MAJOR 1)
+set(PROJECT_VERSION_MINOR 49)
+set(PROJECT_VERSION_PATCH 0)
+set(PROJECT_VERSION "${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}")
 if (PROJECT_VERSION_PATCH GREATER 0)
-  set (PROJECT_VERSION "${PROJECT_VERSION}.${PROJECT_VERSION_PATCH}")
+    set(PROJECT_VERSION "${PROJECT_VERSION}.${PROJECT_VERSION_PATCH}")
 endif ()
 
 # The library version tracks the numbering given by libtool in the
 # autoconf set up.
-set (LIBVERSION_API 17)
-set (LIBVERSION_BUILD 17.1.2)
-string (TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER)
-string (TOUPPER ${PROJECT_NAME} PROJECT_NAME_UPPER)
+set(LIBVERSION_API 17)
+set(LIBVERSION_BUILD 17.1.2)
+string(TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER)
+string(TOUPPER ${PROJECT_NAME} PROJECT_NAME_UPPER)
 
-cmake_minimum_required (VERSION 2.8.4) # This version was released 2011-02-16
+cmake_minimum_required(VERSION 2.8.4) # This version was released 2011-02-16
 
 # (7) Set the default "real" precision.  This should probably be left
 # at 2 (double).
-set (GEOGRAPHICLIB_PRECISION 2 CACHE STRING
-  "Precision: 1 = float, 2 = double, 3 = extended, 4 = quadruple, 5 = variable")
-set_property (CACHE GEOGRAPHICLIB_PRECISION PROPERTY STRINGS 1 2 3 4 5)
+set(GEOGRAPHICLIB_PRECISION 2 CACHE STRING
+    "Precision: 1 = float, 2 = double, 3 = extended, 4 = quadruple, 5 = variable")
+set_property(CACHE GEOGRAPHICLIB_PRECISION PROPERTY STRINGS 1 2 3 4 5)
 
 
-set (LIBNAME Geographic)
+set(LIBNAME Geographic)
 
 include_directories(
-  ./include/
+    ./include/
 )
 
-add_library(libGeographiccc src/LocalCartesian.cpp
-                          src/Geocentric.cpp
-                          src/Math.cpp)
+add_library(libGeographiccc SHARED
+    src/LocalCartesian.cpp
+    src/Geocentric.cpp
+    src/Math.cpp)

CMakeLists.txt

@@ -12,6 +12,8 @@ include(cmake/YAML.cmake)
 include(cmake/glog.cmake)
 include_directories(${GLOG_INCLUDE_DIRS})
 
+add_subdirectory(3rd/GeographicLib GeographicLib)
+
 include_directories(${PROJECT_SOURCE_DIR}/3rd/GeographicLib/include/)
 include_directories(${PROJECT_SOURCE_DIR}/include)
 
@@ -21,14 +23,12 @@ add_library(DEPS SHARED
     src/imu_tool.cpp
     src/eskf_flow.cpp
     src/observability_analysis.cpp
-    3rd/GeographicLib/src/LocalCartesian.cpp
-    3rd/GeographicLib/src/Geocentric.cpp
-    3rd/GeographicLib/src/Math.cpp
 )
 
 target_link_libraries(DEPS
     ${ALL_TARGET_LIBRARIES}
     ${GLOG_LIBRARIES}
+    libGeographiccc
 )
 
 add_executable(gps_imu_fusion app/gps_imu_fusion.cpp)

config/config.yaml

@@ -18,4 +18,12 @@ gyro_noise_std: 1.0e-2
 accelerometer_noise_std: 1.0e-1
 
 # kalman measurement process std (uint: m)
-gps_position_std: 5
+gps_position_x_std: 5
+gps_position_y_std: 5
+gps_position_z_std: 8
+
+# only use prediction, equivalent to only using IMU to integration
+only_prediction: false
+
+# use earth model
+use_earth_model: true

include/config_parameters.h

@@ -27,7 +27,11 @@ class ConfigParameters {
             gyro_bias_error_prior_std_ = config_node["gyro_bias_error_prior_std"].as<double>();
             gyro_noise_std_ = config_node["gyro_noise_std"].as<double>();
             accelerometer_noise_std_ = config_node["accelerometer_noise_std"].as<double>();
-            gps_position_std_ = config_node["gps_position_std"].as<double>();
+            gps_position_x_std_ = config_node["gps_position_x_std"].as<double>();
+            gps_position_y_std_ = config_node["gps_position_y_std"].as<double>();
+            gps_position_z_std_ = config_node["gps_position_z_std"].as<double>();
+            only_prediction_ = config_node["only_prediction"].as<bool>();
+            use_earth_model_ = config_node["use_earth_model"].as<bool>();
 
             LOG(INFO) << "####### Config Parameters #######";
             LOG(INFO) << "earth_rotation_speed: " << earth_rotation_speed_;
@@ -41,7 +45,11 @@ class ConfigParameters {
             LOG(INFO) << "gyro_bias_error_prior_std: " << gyro_bias_error_prior_std_;
             LOG(INFO) << "gyro_noise_std: " << gyro_noise_std_;
             LOG(INFO) << "accelerometer_noise_std: " << accelerometer_noise_std_;
-            LOG(INFO) << "gps_position_std: " << gps_position_std_;
+            LOG(INFO) << "gps_position_x_std: " << gps_position_x_std_;
+            LOG(INFO) << "gps_position_y_std: " << gps_position_y_std_;
+            LOG(INFO) << "gps_position_z_std: " << gps_position_z_std_;
+            LOG(INFO) << "only_prediction: " << only_prediction_;
+            LOG(INFO) << "use_earth_model: " << use_earth_model_;
             LOG(INFO) << std::endl;
         } catch (...) {
             LOG(FATAL) << "Load config parameters failure, path: " << config_file_path;
@@ -69,7 +77,14 @@ class ConfigParameters {
     double accelerometer_noise_std_{};
 
     // kalman measurement process std
-    double gps_position_std_{};
+    double gps_position_x_std_{};
+    double gps_position_y_std_{};
+    double gps_position_z_std_{};
+
+    // only using IMU to integration
+    bool only_prediction_{};
+
+    bool use_earth_model_{};
 };
 
 #endif //GPS_IMU_FUSION_CONFIG_PARAMETERS_H

include/eskf.h

@@ -49,7 +49,11 @@ class ErrorStateKalmanFilter {
 private:
     void SetCovarianceQ(double gyro_noise_cov, double accel_noise_cov);
 
-    void SetCovarianceR(double posi_noise_cov);
+    /*!
+     *
+     * @param position_x_std
+     */
+    void SetCovarianceR(double position_x_std, double position_y_std, double position_z_std);
 
     void SetCovarianceP(double posi_noise, double velocity_noise, double ori_noise,
                         double gyro_noise, double accel_noise);

include/gps_data.h

@@ -5,7 +5,7 @@
 #ifndef GPS_IMU_FUSION_GPS_DATA_H
 #define GPS_IMU_FUSION_GPS_DATA_H
 
-#include "GeographicLib/LocalCartesian.hpp"
+#include "Geocentric/LocalCartesian.hpp"
 #include <eigen3/Eigen/Core>
 
 class GPSData {

include/gps_tool.h

@@ -6,7 +6,7 @@
 #define GPS_IMU_FUSION_GPS_TOOL_H
 
 #include "gps_data.h"
-#include "GeographicLib/LocalCartesian.hpp"
+#include "Geocentric/LocalCartesian.hpp"
 
 #include <deque>
 #include <vector>

readme.md

@@ -1,26 +1,9 @@
-### 2021年9月17日更新：
-有同学反应代码在编译和运行的过程中有一些bug，由于我最近工作有些忙，我先给出一个简单的办法补救一下，后续闲下来我再好好测试和修复，实在不好意思！
+# 误差状态卡尔曼滤波器(ESKF)融合IMU与GPS数据
 
-a. **编译报错：GeographicLib/LocalCartesian.hpp：没有那个文件或目录**
-
-原因：我的cmakelists.txt中的文件路径设置错误了。
-
-解决办法：最简单的解决办法就是安装Geographic库`sudo apt-get install libgeographic-dev`，然后重新编译问题就解决了。
-
-b. **在Ubuntu20.04系统下可以成功编译，但是运行有问题**
-
-解决办法：该问题并不是每一个Ubuntu20.04系统都会出现，看起来像是个别现象。我的开发环境是Ubuntu 18.04！
-
-======================= 我只是一个分割线 ==============================
-
-# ESKF融合IMU与GPS数据
-
-
-![融合IMU数据之后的GPS轨迹效果](https://img-blog.csdnimg.cn/20210304150232490.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3UwMTEzNDE4NTY=,size_16,color_FFFFFF,t_70#pic_center)
-
-绿色轨迹：ground truth
-蓝色轨迹：fuse IMU and GPS
-红色轨迹：GPS
+|                   只使用IMU进行积分的结果                    |                     使用ESKF融合IMU和GPS                     |
+| :----------------------------------------------------------: | :----------------------------------------------------------: |
+| <img src="./data/images/0.png" alt="0" style="zoom: 33%;" /> | <img src="./data/images/1.png" alt="0" style="zoom: 33%;" /> |
+| <img src="./data/images/3.png" alt="0" style="zoom: 33%;" /> | <img src="./data/images/2.png" alt="0" style="zoom: 33%;" /> |
 
 实现方法请参考我的博客[《【附源码+代码注释】误差状态卡尔曼滤波(error-state Kalman Filter)实现GPS+IMU融合，EKF ErrorStateKalmanFilter GPS+IMU》](https://blog.csdn.net/u011341856/article/details/114262451)
 

src/eskf.cpp

@@ -17,7 +17,9 @@ ErrorStateKalmanFilter::ErrorStateKalmanFilter(const ConfigParameters &config_pa
                    config_parameters_.gyro_bias_error_prior_std_,
                    config_parameters_.accelerometer_bias_error_prior_std_);
 
-    SetCovarianceR(config_parameters_.gps_position_std_);
+    SetCovarianceR(config_parameters_.gps_position_x_std_,
+                   config_parameters_.gps_position_y_std_,
+                   config_parameters_.gps_position_z_std_);
 
     SetCovarianceQ(config_parameters_.gyro_noise_std_, config_parameters_.accelerometer_noise_std_);
 
@@ -33,9 +35,11 @@ void ErrorStateKalmanFilter::SetCovarianceQ(double gyro_noise, double accel_nois
     Q_.block<3, 3>(3, 3) = Eigen::Matrix3d::Identity() * accel_noise * accel_noise;
 }
 
-void ErrorStateKalmanFilter::SetCovarianceR(double posi_noise) {
+void ErrorStateKalmanFilter::SetCovarianceR(double position_x_std, double position_y_std, double position_z_std) {
     R_.setZero();
-    R_ = Eigen::Matrix3d::Identity() * posi_noise * posi_noise;
+    R_(0, 0) = position_x_std * position_x_std;
+    R_(1, 1) = position_y_std * position_y_std;
+    R_(2, 2) = position_z_std * position_z_std;
 }
 
 void ErrorStateKalmanFilter::SetCovarianceP(double posi_noise, double velocity_noise, double ori_noise,
@@ -58,7 +62,7 @@ bool ErrorStateKalmanFilter::Init(const GPSData &curr_gps_data, const IMUData &c
                                 Eigen::AngleAxisd(0 * kDegree2Radian, Eigen::Vector3d::UnitX());
 
     pose_.block<3, 3>(0, 0) = Q_init.toRotationMatrix();
-    pose_.block<3, 1>(0, 3) = GPSTool::LLAToLocalNED(curr_gps_data.position_lla);
+    pose_.block<3, 1>(0, 3) = GPSTool::LLAToLocalNED(curr_gps_data.true_position_lla);
 
     imu_data_buff_.clear();
     imu_data_buff_.push_back(curr_imu_data);
@@ -94,7 +98,11 @@ bool ErrorStateKalmanFilter::Correct(const GPSData &curr_gps_data) {
 bool ErrorStateKalmanFilter::Predict(const IMUData &curr_imu_data) {
     imu_data_buff_.push_back(curr_imu_data);
 
-    const Eigen::Vector3d w_in = ComputeNavigationFrameAngularVelocity(); // 时刻 m-1 -> m 地球转动引起的导航系转动角速度
+    Eigen::Vector3d w_in = Eigen::Vector3d::Zero();
+
+    if (config_parameters_.use_earth_model_) {
+        w_in = ComputeNavigationFrameAngularVelocity(); // 时刻 m-1 -> m 地球转动引起的导航系转动角速度
+    }
     UpdateOdomEstimation(w_in);
 
     double delta_t = curr_imu_data.time - imu_data_buff_.front().time;

src/eskf_flow.cpp

@@ -61,7 +61,7 @@ bool ESKFFlow::Run() {
     std::ofstream fused_file(data_file_path_ + "/fused.txt", std::ios::trunc);
     std::ofstream measured_file(data_file_path_ + "/gps_measurement.txt", std::ios::trunc);
 
-    LOG(INFO) << "Start Fusion IMU and GPS ...";
+    LOG(INFO) << "Start fuse IMU and GPS ...";
     while (!imu_data_buff_.empty() && !gps_data_buff_.empty()) {
         curr_imu_data_ = imu_data_buff_.front();
         curr_gps_data_ = gps_data_buff_.front();
@@ -72,7 +72,9 @@ bool ESKFFlow::Run() {
             eskf_ptr_->Predict(curr_imu_data_);
             imu_data_buff_.pop_front();
 
-            eskf_ptr_->Correct(curr_gps_data_);
+            if (!config_parameters_.only_prediction_){
+                eskf_ptr_->Correct(curr_gps_data_);
+            }
 
             SaveTUMPose(fused_file, Eigen::Quaterniond(eskf_ptr_->GetPose().topLeftCorner<3, 3>()),
                         eskf_ptr_->GetPose().topRightCorner<3, 1>(), curr_imu_data_.time);
@@ -100,7 +102,7 @@ bool ESKFFlow::Run() {
         observability_analysis.ComputeObservability();
     }
 
-    LOG(INFO) << "End Fusion IMU and GPS";
+    LOG(INFO) << "End fuse IMU and GPS";
     LOG(INFO) << "Ground Truth data in: " << data_file_path_ + "/gt.txt";
     LOG(INFO) << "Fusion data in: " << data_file_path_ + "/fused.txt";
     LOG(INFO) << "GPS data in: " << data_file_path_ + "/gps_measurement.txt";