松组合 release 1.0.0

.gitignore

@@ -40,4 +40,6 @@ log
 launch.json
 *.gz
 *.zip
-*.rar
+*.rar
+sim_data
+result

.vscode/settings.json

@@ -48,6 +48,12 @@
         "geometry": "cpp",
         "core": "cpp",
         "mutex": "cpp",
-        "algorithm": "cpp"
+        "algorithm": "cpp",
+        "bitset": "cpp",
+        "complex": "cpp",
+        "list": "cpp",
+        "cfenv": "cpp",
+        "regex": "cpp",
+        "string": "cpp"
     }
 }

CMakeLists.txt

@@ -25,3 +25,4 @@ add_subdirectory(${PROJECT_SOURCE_DIR}/src/imu)
 add_subdirectory(${PROJECT_SOURCE_DIR}/src/data)
 add_subdirectory(${PROJECT_SOURCE_DIR}/src/process)
 add_subdirectory(${PROJECT_SOURCE_DIR}/test)
+add_subdirectory(${PROJECT_SOURCE_DIR}/gnss_ins)

README.md

@@ -1,33 +1,69 @@
 # mscnav
-### 初衷
-- 希望学习组合导航和VIO相关内容.
-- 了解到组合导航方面开源代码较少,正好自己在学习相关内容,希望可以和有兴趣的小伙伴们一起学习讨论.
+## loose_couple gnss/ins
+### 1.简介
+GNSS/INS松组合导航功能目前针对实测数据和仿真数据做过一些测试，暂时认为可以使用，所以想发一版针对松组合导航定位的程序。欢迎大家一起讨论学习！
+由于我测试的实测数据的所有权并不属于我个人，因此本程序仅仅附带一组仿真数据，该仿真数据来自程序 https://github.com/Aceinna/gnss-ins-sim  在此十分感谢作者。有需要的小伙伴可以前去学习。还是很容易上手的。
+### 2.程序相关说明
+#### 2.1 依赖与克隆
+- glog
+- Eigen3
 
-### 程序依赖（目前）
-- glog 
-- Eigen
+上述的两个依赖库的安装十分简单，大家可以自行百度，在此不做赘述
 
-### 使用说明
-最新版本对应为dev分支
-mscnav使用了submodules形式挂载了tools,因此clone完本程序需要更新tools
+由于程序采用了submodules形式挂载了tools,因此clone完本程序需要更新tools
 
 ```shell
-git checkout -b dev origin/dev
 git submodule init
 git submodule update
 ```
-已经安装完glog和Eigen条件下,可以直接编译程序
+
+执行上述命令才能完整地clone全部程序，否则submodules文件夹为空，编译会有问题
+#### 2.2 文件架构
+
+|文件夹|说明|
+|-----|-----|
+|config|存储配置文件|
+|data|数据目录，目前存有一组仿真数据，其中result为程序计算结果目录，sim_data为仿真数据|
+|gnss_ins|主程序对应的cc文件目录| 
+|include|全部的头文件，包括 1.data-数据流 2.filter-滤波器 3.imu-惯导处理 4.process-量测更新处理|
+|src|程序的源码文件，和include对应|
+|submodules|子模块，包含tools，基本工具类，在整个程序中都用到|
+|test|本来用于存放测试部分的代码，现在主要存了由仿真数据转到我程序可读取文件的小工具和比较结果的小工具|
+#### 2.3 程序的编译与运行
+- **编译**
+完成程序的全部clone后，同时安装好相关依赖，可直接编译程序
 ```shell
 mkdir build && cd build 
 cmake .. && make
 ```
+或者直接执行(注意权限)
+```shell
+./rebuild.sh
+```
+- **运行**
+程序运行也非常简单，给定配置文件和log输出目录即可
+```shell
+./mscnav_loose_couple ../config/configture.ini ./log
+```
+
+#### 2.4 数据说明
+- 本程序导航坐标系采用**地心地固坐标系**（ecef）与常用的当地水平坐标系不一致，需要注意
+- 本程序imu坐标采用**前-右-下**坐标,同时数据格式为**增量**格式,如果需要跑自己的数据这一点需要小心，注意转换。
+- GNSS数据格式较为简单，每一个历元数据一行，每行格式为“GPS周 周内秒 X
+ Y Z X的标准差 Y的标准差 Z的标准差”，其中"data/sim_data/gps.txt" 中可以具体参考 
+- IMU数据则是二进制的bin数据，直接为程序中ImuData的结构体，具体可以参考程序中 “navstruct.hpp" 文件。
+- 当前程序配置的是针对 data/sim_data/下的数据，编译完成后可直接运行
+### 3. 程序结果
+利用仿真程序生成一组较为简单数据，数据时长约700s,数据轨迹和实际速度如下图所示：
+<img src="./picture/ref_pos.png" width="400" height="400" alt="轨迹图" align=center><img src="./picture/ref_vel.png" width="300" height="300" alt="实际速度图" align=center>
+惯性器件的输出如下：
+<img src="./picture/accel_0.png" width="300" height="300" alt="加计输出" align=center><img src="./picture/gyro_0.png" width="300" height="300" alt="陀螺输出" align=center>
 
-### 功能
-- 支持纯惯导推算
-- 初始化功能完成,初步测试认为该功能可满足目前使用.支持给定姿态／动对准／加计调平后对准航向三种方式.（面向低成本IMU器件，不考虑高等级设备可以进行静止航向对准）
-- 初步完成松组合定位功能，需要进一步测试，准备整理一些开源数据进行测试，核实程序中公式，符号，单位等易错点
-- 目前程序还在持续开发,后续希望能做好松组合,基于msckf的vio+GPS
+程序结果输出：
+<img src="./picture/result.png" width="500" height="380" alt="加计输出" align=center>
+由于仿真给定的GPS位置误差std为0.2m，因此程序中存在噪声，全序列统计XYZ三个方向标准差分别为 0.20，0.169，0.170。符合仿真设定值
 
-### 学习交流
+### 4.学习交流
+希望和大家一起学习交流讨论
 - QQ: 1280269817
 - e-mail: [email protected]   [email protected]

config/configture.ini

@@ -1,28 +1,26 @@
 #---------------------------------------------------#
-process_date:                       2018, 01, 19
-start_time:                         457034
-end_time:                           459034
+process_date:                       2018, 05, 06
+start_time:                         0
+end_time:                           800
 #----------------------------------------------------#
 gnss_enable:                        1
 imu_enable:                         1
 nhc_enable:                         0
 odo_enable:                         0
 camera_enable:                      0
 #----------------------------------------------------#
-gnss_velocity_enable:               1
+gnss_velocity_enable:               0
 gnss_position_enable:               1
 #----------------------------------------------------#
-imu_data_path:                      /media/fwt/Data/program/mscnav/data/alignmentdata/imu_file.bin
-gnss_data_path:                     /media/fwt/Data/program/mscnav/data/alignmentdata/pppforins.txt
-result_output_path:                 /media/fwt/Data/program/mscnav/data/alignmentdata/
-data_rate:                          200 
-result_output_rate:                 10
+imu_data_path:                      ../data/sim_data/imu.bin
+gnss_data_path:                     ../data/sim_data/gps.txt
+result_output_path:                 ../data/result
+data_rate:                          100 
+result_output_rate:                 100
 #log----------------------------------------------------#
 gnsslog_enable:                     0
-gnsslog_out_path:                   ./log/
 filter_debug_log_enable:            0
-filter_debug_output:                0
-filter_debug_cov_file:              ./debug.info
+filter_debug_cov_file:              filter.info
 #----------------------------------------------------#
 alignnment_velocity_threshold:      5
 alignnment_attitude_mode:           0
@@ -34,27 +32,27 @@ initial_vel:                        0,0,0
 initial_att:                        0,0,0
 initial_gyro_bias:                  0,0,0
 initial_acce_bias:                  0,0,0
-initial_gyro_scale:                 100,100,100
-initial_acce_scale:                 100,100,100
+initial_gyro_scale:                 0,0,0   
+initial_acce_scale:                 0,0,0
 evaluate_imu_scale:                 0
 evaluate_imu_vehicle_rotation:      0
-leverarm:                           -0.123, 0.525, -0.254
+leverarm:                           0,0,0
 #----------------------------------------------------#
 use_define_variance_pos_vel:        0
 use_define_variance_att:            0
 initial_vel_std:                    1,1,1
 initial_pos_std:                    1,1,1
 initial_att_std:                    1,1,1
 position_random_walk:               0,0,0
-velocity_random_walk:               0.03,0.03,0.03
-attitude_random_walk:               0.003,0.003,0.003
-gyro_bias_std:                      0.03,0.03,0.03
-acce_bias_std:                      10,10,10
+velocity_random_walk:               0.03119,0.03009,0.04779
+attitude_random_walk:               0.25,0.25,0.25
+gyro_bias_std:                      3.5, 3.5, 3.5
+acce_bias_std:                      4.29, 5.72, 8.02
 gyro_scale_std:                     100,100,100
 acce_scale_std:                     100,100,100
 #scale_std_comment:                  the unit is ppm
-corr_time_of_gyro_bias:             4
-corr_time_of_acce_bias:             4
+corr_time_of_gyro_bias:             0.15
+corr_time_of_acce_bias:             0.15
 corr_time_of_gyro_scale:            4
 corr_time_of_acce_scale:            4
 #corr_time_comment:                  the unit is hour

gnss_ins/CMakeLists.txt

@@ -0,0 +1,5 @@
+cmake_minimum_required( VERSION 2.8 )
+PROJECT(mscnav_loose_couple)
+
+add_executable(${PROJECT_NAME} ./NavProcessing.cc)
+target_link_libraries( ${PROJECT_NAME} process filter data imu tools)

rebuild.sh

@@ -1,3 +1,3 @@
 rm -r build
 mkdir build && cd build
-cmake .. && make -j7
+cmake .. && make -j3

src/data/navgnss.cc

@@ -40,7 +40,7 @@ bool FileGnssData::StartReadGnssData()
 	}
 	if (logout_)
 	{
-		std::string log_path = config->get<std::string>("gnsslog_out_path");
+		std::string log_path = config->get<std::string>("result_output_path");
 		NavTime time = NavTime::NowTime();
 		log_path += "/gnss-data-" + time.Time2String() + ".log";
 		ofs_log_.open(log_path);

src/filter/navfilter.cc

@@ -32,8 +32,10 @@ bool KalmanFilter::InitialStateCov(const Eigen::VectorXd &init_state_cov)
   if (debug_log_)
   {
     ConfigInfo::Ptr getconfig = ConfigInfo::GetInstance();
-    debug_log_file_.open(getconfig->get<std::string>("filter_debug_cov_file"));
-    LOG(INFO) << "filter_debug_cov_file: " << getconfig->get<std::string>("filter_debug_cov_file") << std::endl;
+    std::string debug_info_path = getconfig->get<std::string>("result_output_path") + "/" +
+                                  getconfig->get<std::string>("filter_debug_cov_file");
+    debug_log_file_.open(debug_info_path);
+    LOG(INFO) << "filter_debug_cov_file: " << debug_info_path << std::endl;
     if (!debug_log_file_.good())
     {
       LOG(INFO) << ("Open filter debug file failed! ") << std::endl;

src/imu/navinitialized.cc

@@ -201,7 +201,8 @@ Eigen::VectorXd &InitializedNav::SetInitialVariance(Eigen::VectorXd &PVariance,
     else
     {
         PVariance.segment<3>(index.pos_index_) = nav_info.pos_std_;
-        PVariance.segment<3>(index.vel_index_) = nav_info.vel_std_;
+        PVariance.segment<3>(index.vel_index_) = nav_info.pos_std_;
+        // PVariance.segment<3>(index.vel_index_) = nav_info.vel_std_;
     }
     bool user_define_std_att = config->get<int>("use_define_variance_att") == 0 ? false : true;
     if (user_define_std_att || aligned_mode_ == SETTING)

src/imu/navmech.cc

@@ -117,13 +117,13 @@ Eigen::MatrixXd MechTransferMat(const ImuData &pre_imu_data, const ImuData &curr
     static int scale_of_acce = config->get<int>("evaluate_imu_scale") == 0 ? 0 : 3;
     static int scale_of_gyro = scale_of_acce;
     static int rows = 15 + scale_of_acce + scale_of_gyro, cols = rows;
-    static int corr_time_of_gyro_bias = config->get<int>("corr_time_of_gyro_bias") * constant_hour;
-    static int corr_time_of_acce_bias = config->get<int>("corr_time_of_acce_bias") * constant_hour;
-    static int corr_time_of_gyro_scale = 0, corr_time_of_acce_scale = 0;
+    static double corr_time_of_gyro_bias = config->get<double>("corr_time_of_gyro_bias") * constant_hour;
+    static double corr_time_of_acce_bias = config->get<double>("corr_time_of_acce_bias") * constant_hour;
+    static double corr_time_of_gyro_scale = 0, corr_time_of_acce_scale = 0;
     if (scale_of_gyro == 3)
-        corr_time_of_gyro_scale = config->get<int>("corr_time_of_gyro_scale") * constant_hour;
+        corr_time_of_gyro_scale = config->get<double>("corr_time_of_gyro_scale") * constant_hour;
     if (scale_of_acce == 3)
-        corr_time_of_acce_scale = config->get<int>("corr_time_of_acce_scale") * constant_hour;
+        corr_time_of_acce_scale = config->get<double>("corr_time_of_acce_scale") * constant_hour;
 
     auto &pos = nav_info.pos_;
     auto &vel = nav_info.vel_;

src/process/navstate.cc

@@ -105,16 +105,16 @@ bool State::InitializeState()
     }
     state_q_tmp = state_q_tmp.array().pow(2);
 
-    int gbtime = config_->get<int>("corr_time_of_gyro_bias") * constant_hour;
-    int abtime = config_->get<int>("corr_time_of_acce_bias") * constant_hour;
+    double gbtime = config_->get<double>("corr_time_of_gyro_bias") * constant_hour;
+    double abtime = config_->get<double>("corr_time_of_acce_bias") * constant_hour;
 
     state_q_tmp.segment<3>(index.gyro_bias_index_) *= (2.0 / gbtime);
     state_q_tmp.segment<3>(index.acce_bias_index_) *= (2.0 / abtime);
 
     if (config_->get<int>("evaluate_imu_scale") != 0)
     {
-        int gstime = config_->get<int>("corr_time_of_gyro_scale") * constant_hour;
-        int astime = config_->get<int>("corr_time_of_acce_scale") * constant_hour;
+        double gstime = config_->get<double>("corr_time_of_gyro_scale") * constant_hour;
+        double astime = config_->get<double>("corr_time_of_acce_scale") * constant_hour;
         state_q_tmp.segment<3>(index.gyro_scale_index_) *= (2.0 / gstime);
         state_q_tmp.segment<3>(index.acce_scale_index_) *= (2.0 / astime);
     }
@@ -211,7 +211,7 @@ void State::StartProcessing()
             ptr_pre_imu_data = ptr_curr_imu_data;
             ptr_curr_imu_data = nullptr;
         }
-        int idt = int(nav_info_.time_.Second() * output_rate) - int(nav_info_bak_.time_.Second() * output_rate);
+        int idt = int((nav_info_.time_.Second()+1e-6) * output_rate) - int(nav_info_bak_.time_.Second() * output_rate);
         if (idt > 0)
         {
             double second_of_week = nav_info_.time_.SecondOfWeek();

test/CMakeLists.txt

@@ -1,17 +1,8 @@
 cmake_minimum_required( VERSION 2.8 )
 PROJECT(test_mscnav)
 
-add_executable(test_filter ./NavFilter.cc)
-target_link_libraries( test_filter filter tools)
+add_executable(SimDataProcess ./SimDataProcess.cc)
+target_link_libraries( SimDataProcess tools)
 
-add_executable(test_mech ./NavPureMech.cc)
-target_link_libraries( test_mech imu tools)
-
-add_executable(test_data_queue ./NavdataQueue.cc)
-target_link_libraries( test_data_queue data imu tools)
-
-add_executable(test_data_align ./NavAlign.cc)
-target_link_libraries( test_data_align data imu tools)
-
-add_executable(test_processing ./NavProcessing.cc)
-target_link_libraries( test_processing process filter data imu tools)
+add_executable(RefProcess ./RefProcess.cc)
+target_link_libraries( RefProcess tools)