it works. add plot function.

CMakeLists.txt

@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.10)
 project(Wheel-INS)
 
 set(CMAKE_CXX_STANDARD 14)
-set(CMAKE_BUILD_TYPE DEBUG)
+set(CMAKE_BUILD_TYPE RELEASE)
 
 set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
 set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)

config/car.yaml

@@ -0,0 +1,39 @@
+# Pioneer Robot 
+
+imupath: "/home/yibin/code/KF-GINS/dataset/car/Wheel-IMU/C1_imu.bin"
+
+outputpath: "/home/yibin/code/KF-GINS/output/car"
+
+imudatalen: 7
+
+imudatarate: 200
+
+starttime: 353650
+endtime: 356255
+
+initposstd: [ 0.1, 0.1, 0.2 ] # m
+initvelstd: [ 0.05, 0.05, 0.05 ] # m/s
+initattstd: [ 0.5, 0.5, 1.0 ] # deg
+
+imunoise:
+  arw: 1.2          # [deg/sqrt(hr)]
+  vrw: 6          # [m/s/sqrt(hr)]
+  gbstd: 50        # [deg/hr]
+  abstd: 2000     # [mGal]
+  gsstd: 50  # [ppm]
+  asstd: 500  # [ppm]
+  corrtime: 1.0                    # [hr]
+
+# Misalignment angle between the up IMU and the vehicle. (unit: degree)
+MisalignAngle: [0, 1.32, 0] 
+# Leverarm, wheel center in the imu frame
+WheelLA: [-0.015, 0.005, 0.005]
+
+# Velocity measurement standard deviation
+ODO_std: [0.035, 0.02, 0.02]
+# Velocity update time interval
+ODO_dt: 0.5
+Wheel_Radius: 0.35
+
+# if use gyro bias to compensate the wheel velocity
+ifCompVel: false

config/wheel-ins.yaml → config/robot.yaml

@@ -1,8 +1,8 @@
 # Pioneer Robot 
 
-imupath: "/home/yibin/code/KF-GINS/dataset/C1_imu.bin"
+imupath: "/home/yibin/code/KF-GINS/dataset/robot/Wheel-IMU/C1_imu.bin"
 
-outputpath: "/home/yibin/code/KF-GINS/output"
+outputpath: "/home/yibin/code/KF-GINS/output/robot"
 
 imudatalen: 7
 
@@ -11,9 +11,9 @@ imudatarate: 200
 starttime: 132030
 endtime: 132945
 
-initposstd: [ 0.1, 0.1, 0.2 ] # m
+initposstd: [ 0.05, 0.05, 0.05 ] # m
 initvelstd: [ 0.05, 0.05, 0.05 ] # m/s
-initattstd: [ 0.5, 0.5, 1.0 ] # deg
+initattstd: [ 1.0, 1.0, 0.1 ] # deg
 
 imunoise:
   arw: 1.2          # [deg/sqrt(hr)]

src/fileio/imufileloader.h

@@ -45,8 +45,8 @@ class ImuFileLoader : public FileLoader {
 
         imu_.timestamp = data_[0];
         //if the original imu dta is not increment
-        for (int i = 1; i<=6; i++)
-            data_[i] = data_[i]*dt_;
+        // for (int i = 1; i<=6; i++)
+        //     data_[i] = data_[i]*dt_;
 
         memcpy(imu_.angular_velocity.data(), &data_[1], 3 * sizeof(double));
         memcpy(imu_.acceleration.data(), &data_[4], 3 * sizeof(double));

src/main.cpp

@@ -25,6 +25,8 @@
 #include <iomanip>
 #include <iostream>
 #include <yaml-cpp/yaml.h>
+#include <sys/stat.h>
+#include <unistd.h>
 
 #include "common/angle.h"
 #include "fileio/filesaver.h"
@@ -80,6 +82,8 @@ int main(int argc, char *argv[]) {
         std::cout << "Failed when loading configuration. Please check the file path and output path!" << std::endl;
         return -1;
     }
+    if (access(outputpath.c_str(), F_OK))
+    mkdir(outputpath.c_str(), S_IRWXU);
 
     // imu数据配置，数据处理区间
     // imudata configuration， data processing interval
@@ -113,7 +117,7 @@ int main(int argc, char *argv[]) {
     const int nav_columns = 11, imuerr_columns = 13, std_columns = 22;
     FileSaver navfile(outputpath + "/wheelins_Navresult.nav", nav_columns, FileSaver::TEXT);
     FileSaver imuerrfile(outputpath + "/wheelins_IMU_ERR.txt", imuerr_columns, FileSaver::TEXT);
-    FileSaver stdfile(outputpath + "wheelins_STD.txt", std_columns, FileSaver::TEXT);
+    FileSaver stdfile(outputpath + "/wheelins_STD.txt", std_columns, FileSaver::TEXT);
 
     std::string debugoutputpath = outputpath + "/debug.txt";
     debug_.open(debugoutputpath.c_str());
@@ -183,12 +187,14 @@ int main(int argc, char *argv[]) {
         navstate  = wheelins.getNavState();
         cov       = wheelins.getCovariance();
 
+        // std::cout << timestamp << " " << navstate.pos.transpose() << " " << navstate.vel.transpose() << " " << navstate.euler.transpose()<<std::endl;
+
         // 保存处理结果
         // save processing results
         writeNavResult(timestamp, navstate, navfile, imuerrfile);
         writeSTD(timestamp, cov, stdfile);
 
-        std::cout<<std::setprecision(15)<<timestamp<<" "<<navstate.pos.transpose()<<" "<<navstate.euler.transpose()<<std::endl;
+
         // 显示处理进展
         // display processing progress
         percent = int((imu_cur.timestamp - starttime) / interval * 100);
@@ -304,7 +310,6 @@ void writeNavResult(double time, NavState &navstate, FileSaver &navfile, FileSav
     // 保存导航结果
     // save navigation result
     result.clear();
-    result.push_back(0);
     result.push_back(time);
     result.push_back(navstate.pos[0]);
     result.push_back(navstate.pos[1]);

src/wheel_ins/wheelins.cpp

@@ -1,26 +1,3 @@
-/*
- * KF-GINS: An EKF-Based GNSS/INS Integrated Navigation System
- *
- * Copyright (C) 2022 i2Nav Group, Wuhan University
- *
- *     Author : Liqiang Wang
- *    Contact : wlq@whu.edu.cn
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program.  If not, see <https://www.gnu.org/licenses/>.
- */
-
-
 #include "common/rotation.h"
 
 #include "wheelins.h"
@@ -35,17 +12,14 @@ WheelINS::WheelINS(Paras &options) {
 
     timestamp_ = 0;
 
-    // 设置协方差矩阵，系统噪声阵和系统误差状态矩阵大小
-    // resize covariance matrix, system noise matrix, and system error state matrix
+
     Cov_.resize(RANK, RANK);
     Qc_.resize(NOISERANK, NOISERANK);
     dx_.resize(RANK, 1);
     Cov_.setZero();
     Qc_.setZero();
     dx_.setZero();
 
-    // 初始化系统噪声阵
-    // initialize noise matrix
     auto imunoise                   = options_.imunoise;
     Qc_.block(ARW_ID, ARW_ID, 3, 3) = imunoise.gyr_arw.cwiseProduct(imunoise.gyr_arw).asDiagonal();
     Qc_.block(VRW_ID, VRW_ID, 3, 3) = imunoise.acc_vrw.cwiseProduct(imunoise.acc_vrw).asDiagonal();
@@ -138,8 +112,7 @@ void WheelINS::insPropagation(IMU &imupre, IMU &imucur) {
     // IMU状态更新(机械编排算法)
 
     INSMech::insMech(pvapre_, pvacur_, imupre, imucur);
-    std::cout<<"pvapre_: "<<pvapre_.pos.transpose()<<" "<<pvapre_.att.euler.transpose()<<std::endl;
-    std::cout<<"pvacur_: "<<pvacur_.pos.transpose()<<" "<<pvacur_.att.euler.transpose()<<std::endl;
+
     // 系统噪声传播，姿态误差采用phi角误差模型
     // system noise propagate, phi-angle error model for attitude error
     Eigen::MatrixXd Phi, F, Qd, G;
@@ -288,11 +261,10 @@ void WheelINS::ODOUpdate() {
 
 void WheelINS::getWheelVelocity() {
 
-
     double gyro_x_mean = 0.0;
 
     int i = 0;
-    for(auto it = imuBuff.rend(); i < imuBuffsize; it++,i++){
+    for(auto it = imuBuff.begin(); it != imuBuff.end(); ++it){
         gyro_x_mean += it->angular_velocity[0];
     }
     gyro_x_mean /= imuBuffsize;

utils/plot.py

@@ -0,0 +1,49 @@
+#!/usr/bin/env python3
+import numpy as np
+import matplotlib.pyplot as plt
+
+path = "/home/yibin/code/KF-GINS/output/robot/wheelins_Navresult.nav"
+imupath = "/home/yibin/code/KF-GINS/dataset/robot/Wheel-IMU/C1_imu.bin"
+
+num_columns = 7
+dtype = np.float64
+
+# Read the binary file using NumPy
+try:
+    imu = np.fromfile(imupath, dtype=dtype)
+
+    # Reshape the data into a 2D array with 7 columns
+    imu = imu.reshape(-1, num_columns)
+
+    print("Successfully read the binary file.")
+    print("Shape of the loaded data:", imu.shape)
+except FileNotFoundError:
+    print(f"File not found at path: {imupath}")
+except Exception as e:
+    print(f"An error occurred: {str(e)}")
+
+fig, axs = plt.subplots(2, 1,figsize=(12, 5))
+axs[0].plot(imu[:, 0] - imu[0, 0], imu[:, 1])
+axs[0].plot(imu[:, 0] - imu[0, 0], imu[:, 2])
+axs[0].plot(imu[:, 0] - imu[0, 0], imu[:, 3])
+axs[0].set_xlabel('Time (s)')
+axs[0].set_ylabel('Gyro (rad/s)')
+axs[0].legend(['X', 'Y', 'Z'])
+axs[1].plot(imu[:, 0] - imu[0, 0], imu[:, 4])
+axs[1].plot(imu[:, 0] - imu[0, 0], imu[:, 5])
+axs[1].plot(imu[:, 0] - imu[0, 0], imu[:, 6])
+axs[1].set_xlabel('Time (s)')
+axs[1].set_ylabel('Acc (m/s2) ' + str(imu[0, 0]))
+axs[1].legend(['X', 'Y', 'Z'])
+
+traj =  np.loadtxt(path)
+fig, ax = plt.subplots()
+ax.plot(traj[:, 1], traj[:, 2])
+ax.set_xlabel('X (m)')
+ax.set_ylabel('Y (m)')
+ax.axis('equal')
+ax.legend(['Wheel-INS'])
+
+
+
+plt.show()