diff --git a/CMakeLists.txt b/CMakeLists.txt
index 45b9164..67e081a 100644
--- a/CMakeLists.txt
+++ b/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)
diff --git a/config/car.yaml b/config/car.yaml
new file mode 100644
index 0000000..db1a703
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/config/wheel-ins.yaml b/config/robot.yaml
similarity index 78%
rename from config/wheel-ins.yaml
rename to config/robot.yaml
index bfb74d2..3bc47fb 100644
--- a/config/wheel-ins.yaml
+++ b/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)]
diff --git a/src/common/logging.h b/src/common/logging.h
deleted file mode 100644
index 07664e4..0000000
--- a/src/common/logging.h
+++ /dev/null
@@ -1,85 +0,0 @@
-/*
- * OB_GINS: An Optimization-Based GNSS/INS Integrated Navigation System
- *
- * Copyright (C) 2022 i2Nav Group, Wuhan University
- *
- *     Author : Hailiang Tang
- *    Contact : thl@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/>.
- */
-
-#ifndef LOGGING_H
-#define LOGGING_H
-
-#include <Eigen/Geometry>
-#include <absl/strings/str_format.h>
-#include <glog/logging.h>
-#include <glog/stl_logging.h>
-#include <iostream>
-
-using std::string;
-
-#define LOGI (LOG(INFO))
-#define LOGW (LOG(WARNING))
-#define LOGE (LOG(ERROR))
-#define LOGF (LOG(FATAL))
-
-#if !DCHECK_IS_ON()
-#define DLOGI (static_cast<void>(0), true ? (void) 0 : google::LogMessageVoidify() & LOG(INFO))
-#define DLOGW (static_cast<void>(0), true ? (void) 0 : google::LogMessageVoidify() & LOG(WARNING))
-#define DLOGE (static_cast<void>(0), true ? (void) 0 : google::LogMessageVoidify() & LOG(ERROR))
-#define DLOGF (static_cast<void>(0), true ? (void) 0 : google::LogMessageVoidify() & LOG(FATAL))
-#else
-#define DLOGI LOGI
-#define DLOGW LOGW
-#define DLOGE LOGE
-#define DLOGF LOGF
-#endif
-
-class Logging {
-
-public:
-    static void initialization(char **argv, bool logtostderr = true, bool logtofile = false) {
-        if (logtostderr & logtofile) {
-            FLAGS_alsologtostderr = true;
-        } else if (logtostderr) {
-            FLAGS_logtostderr = true;
-        }
-
-        if (logtostderr) {
-            // 输出颜色
-            FLAGS_colorlogtostderr = true;
-        }
-
-        // glog初始化
-        google::InitGoogleLogging(argv[0]);
-    }
-
-    template <typename T, int Rows, int Cols>
-    static void printMatrix(const Eigen::Matrix<T, Rows, Cols> &matrix, const string &prefix = "Matrix: ") {
-        std::cout << prefix << matrix.rows() << "x" << matrix.cols() << std::endl;
-        if (matrix.cols() == 1) {
-            std::cout << matrix.transpose() << std::endl;
-        } else {
-            std::cout << matrix << std::endl;
-        }
-    }
-
-    static string doubleData(double data) {
-        return absl::StrFormat("%0.6lf", data);
-    }
-};
-
-#endif // LOGGING_H
diff --git a/src/fileio/imufileloader.h b/src/fileio/imufileloader.h
index 8c76dbb..958ef1f 100644
--- a/src/fileio/imufileloader.h
+++ b/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));
diff --git a/src/main.cpp b/src/main.cpp
index 5789dc8..d5fdf6d 100644
--- a/src/main.cpp
+++ b/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]);
diff --git a/src/wheel_ins/wheelins.cpp b/src/wheel_ins/wheelins.cpp
index 228c4bf..3594599 100644
--- a/src/wheel_ins/wheelins.cpp
+++ b/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,8 +12,7 @@ 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);
@@ -44,8 +20,6 @@ WheelINS::WheelINS(Paras &options) {
     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;
diff --git a/utils/plot.py b/utils/plot.py
new file mode 100644
index 0000000..42ee17b
--- /dev/null
+++ b/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()
\ No newline at end of file