diff --git a/scripts/plot_navresult.py b/scripts/plot_navresult.py
new file mode 100644
index 0000000..c7a88d2
--- /dev/null
+++ b/scripts/plot_navresult.py
@@ -0,0 +1,318 @@
+#!/usr/bin/python3
+# -*- coding: utf-8 -*-
+
+import numpy as np
+import math as m
+import matplotlib.pyplot as plt
+
+# WGS84参数
+WGS84_RA = 6378137.0
+WGS84_E1 = 0.00669437999013
+WGS84_WIE = 7.2921151467e-5
+
+D2R = np.pi / 180.0
+R2D = 180.0 / np.pi
+
+
+# 计算子午圈半径和卯酉圈半径
+# 输入参数: lat(纬度)[rad]
+def radiusmn(lat):
+    tmp = np.square(m.sin(lat))
+    tmp = 1 - WGS84_E1 * tmp
+    sqrttmp = np.sqrt(tmp)
+
+    radm = WGS84_RA * (1 - WGS84_E1) / (sqrttmp * tmp)
+    radn = WGS84_RA / sqrttmp
+    return radm, radn
+
+
+# 地理坐标系增量转成n系下坐标增量
+# 参数: rm, rn 子午圈半径和卯酉圈半径; pos当前位置地理位置[rad, rad, m], drad(地理坐标系相对增量)[rad, rad, m]
+# @param: return: dm, n系下增量
+def drad2dm(rm, rn, pos, drad):
+    dm = np.zeros([3, 1])
+    dm[0] = drad[0] * (rm + pos[2])
+    dm[1] = drad[1] * (rn + pos[2]) * m.cos(pos[0])
+    dm[2] = -drad[2]
+    return dm
+
+
+def plotNavresult(navresult_filepath):
+
+    navresult = np.loadtxt(navresult_filepath)
+
+    # 小范围内将位置转到第一个位置确定的n系
+    pos = np.zeros([len(navresult), 4])
+    navresult[:, 2:4] = navresult[:, 2:4] * D2R
+    pos[:, 0] = navresult[:, 1]
+
+    blh_station = navresult[0, 2:5]
+    rm, rn = radiusmn(blh_station[0])
+
+    for i in range(len(pos)):
+        delta_blh = navresult[i, 2:5] - navresult[0, 2:5]
+        pos[i, 1:4] = drad2dm(rm, rn, blh_station, delta_blh).reshape(1, 3)
+
+    print('plotting estimated navigation result!')
+
+    # 绘图
+    plt.figure('horizontal position')
+    plt.plot(pos[:, 2], pos[:, 1])
+    plt.axis('equal')
+    plt.xlabel('East [m]')
+    plt.ylabel('North [m]')
+    plt.title('Horizontal Position')
+    plt.grid()
+    plt.tight_layout()
+
+    plt.figure('height')
+    plt.plot(navresult[:, 1], navresult[:, 4])
+    plt.xlabel('Time [s]')
+    plt.ylabel('Height [m]')
+    plt.title('Height')
+    plt.grid()
+    plt.tight_layout()
+
+    plt.figure()
+    plt.plot(navresult[:, 1], navresult[:, 5:8])
+    plt.legend(['North', 'East', 'Down'])
+    plt.xlabel('Time [s]')
+    plt.ylabel('Velocity [m/s]')
+    plt.title('Velocity')
+    plt.grid()
+    plt.tight_layout()
+
+    plt.figure()
+    plt.plot(navresult[:, 1], navresult[:, 8:11])
+    plt.legend(['Roll', 'Pitch', 'Yaw'])
+    plt.xlabel('Time [s]')
+    plt.ylabel('Angle [deg]')
+    plt.title('Attitude')
+    plt.grid()
+    plt.tight_layout()
+
+    plt.show()
+
+
+def plotIMUerror(imuerr_filepath):
+
+    imuerr = np.loadtxt(imuerr_filepath)
+    print('plotting estimated IMU error!')
+
+    plt.figure('gyro bias')
+    plt.plot(imuerr[:, 0], imuerr[:, 1:4])
+    plt.legend(['X', 'Y', 'Z'])
+    plt.xlabel('Time [s]')
+    plt.ylabel('Bias [deg/h]')
+    plt.title('Gyroscope Bias')
+    plt.grid()
+    plt.tight_layout()
+
+    plt.figure('accel bias')
+    plt.plot(imuerr[:, 0], imuerr[:, 4:7])
+    plt.legend(['X', 'Y', 'Z'])
+    plt.xlabel('Time [s]')
+    plt.ylabel('Bias [mGal]')
+    plt.title('Accelerometer Bias')
+    plt.grid()
+    plt.tight_layout()
+
+    plt.figure('gyro scale')
+    plt.plot(imuerr[:, 0], imuerr[:, 7:10])
+    plt.legend(['X', 'Y', 'Z'])
+    plt.xlabel('Time [s]')
+    plt.ylabel('Scale [ppm]')
+    plt.title('Gyroscope Scale')
+    plt.grid()
+    plt.tight_layout()
+
+    plt.figure('accel scale')
+    plt.plot(imuerr[:, 0], imuerr[:, 10:13])
+    plt.legend(['X', 'Y', 'Z'])
+    plt.xlabel('Time [s]')
+    plt.ylabel('Scale [ppm]')
+    plt.title('Accelerometer Scale')
+    plt.grid()
+    plt.tight_layout()
+
+    plt.show()
+
+
+def plotNavError(navresult_filepath, refresult_filepath):
+
+    naverror = calcNavresultError(navresult_filepath, refresult_filepath)
+    print('calculate mavigtion result error finished!')
+    print('plotting navigation error!')
+
+    # 绘制误差曲线
+    plt.figure('position error')
+    plt.plot(naverror[:, 1], naverror[:, 2:5])
+    plt.legend(['North', 'East', 'Down'])
+    plt.xlabel('Time [s]')
+    plt.ylabel('Error [m]')
+    plt.title('Position Error')
+    plt.grid()
+    plt.tight_layout()
+
+    plt.figure('velocity error')
+    plt.plot(naverror[:, 1], naverror[:, 5:8])
+    plt.legend(['North', 'East', 'Down'])
+    plt.xlabel('Time [s]')
+    plt.ylabel('Error [m/s]')
+    plt.title('Velocity Error')
+    plt.grid()
+    plt.tight_layout()
+
+    plt.figure('attitude error')
+    plt.plot(naverror[:, 1], naverror[:, 8:11])
+    plt.legend(['Roll', 'Pitch', 'Yaw'])
+    plt.xlabel('Time [s]')
+    plt.ylabel('Error [deg]')
+    plt.title('Attitude Error')
+    plt.grid()
+    plt.tight_layout()
+
+    plt.show()
+
+
+def plotSTD(std_filepath):
+
+    std = np.loadtxt(std_filepath)
+    print('plotting estimated STD!')
+
+    plt.figure('position std')
+    plt.plot(std[:, 0], std[:, 1:4])
+    plt.legend(['North', 'East', 'Down'])
+    plt.xlabel('Time [s]')
+    plt.ylabel('STD [m]')
+    plt.title('Position STD')
+    plt.grid()
+    plt.tight_layout()
+
+    plt.figure('velocity std')
+    plt.plot(std[:, 0], std[:, 4:7])
+    plt.legend(['North', 'East', 'Down'])
+    plt.xlabel('Time [s]')
+    plt.ylabel('STD [m/s]')
+    plt.title('Velocity STD')
+    plt.grid()
+    plt.tight_layout()
+
+    plt.figure('attitude std')
+    plt.plot(std[:, 0], std[:, 7:10])
+    plt.legend(['Roll', 'Pitch', 'Yaw'])
+    plt.xlabel('Time [s]')
+    plt.ylabel('STD [deg]')
+    plt.title('Attitude STD')
+    plt.grid()
+    plt.tight_layout()
+
+    plt.figure('gyrobias std')
+    plt.plot(std[:, 0], std[:, 10:13])
+    plt.legend(['X', 'Y', 'Z'])
+    plt.xlabel('Time [s]')
+    plt.ylabel('STD [deg/h]')
+    plt.title('Gyroscope Bias STD')
+    plt.grid()
+    plt.tight_layout()
+
+    plt.figure('accelbias std')
+    plt.plot(std[:, 0], std[:, 13:16])
+    plt.legend(['X', 'Y', 'Z'])
+    plt.xlabel('Time [s]')
+    plt.ylabel('STD [mGal]')
+    plt.title('Accelerometer Bias STD')
+    plt.grid()
+    plt.tight_layout()
+
+    plt.figure('gyroscale std')
+    plt.plot(std[:, 0], std[:, 16:19])
+    plt.legend(['X', 'Y', 'Z'])
+    plt.xlabel('Time [s]')
+    plt.ylabel('STD [ppm]')
+    plt.title('Gyroscope Scale STD')
+    plt.grid()
+    plt.tight_layout()
+
+    plt.figure('accelscale std')
+    plt.plot(std[:, 0], std[:, 19:22])
+    plt.legend(['X', 'Y', 'Z'])
+    plt.xlabel('Time [s]')
+    plt.ylabel('STD [ppm]')
+    plt.title('Accelerometer Scale STD')
+    plt.grid()
+    plt.tight_layout()
+
+    plt.show()
+
+
+def calcNavresultError(navresult_filepath, refresult_filepath):
+
+    navresult = np.loadtxt(navresult_filepath)
+    refresult = np.loadtxt(refresult_filepath)
+
+    # 航向角平滑
+    for i in range(1, len(navresult)):
+        if navresult[i, 10] - navresult[i - 1, 10] < -180:
+            navresult[i:, 10] = navresult[i:, 10] + 360
+        if navresult[i, 10] - navresult[i - 1, 10] > 180:
+            navresult[i:, 10] = navresult[i:, 10] - 360
+
+    for i in range(1, len(refresult)):
+        if refresult[i, 10] - refresult[i - 1, 10] < -180:
+            refresult[i:, 10] = refresult[i:, 10] + 360
+        if refresult[i, 10] - refresult[i - 1, 10] > 180:
+            refresult[i:, 10] = refresult[i:, 10] - 360
+
+    # 找到数据重合部分，参考结果内插到测试结果
+    start_time = refresult[0, 1] if refresult[0, 1] >= navresult [0, 1] else navresult [0, 1]
+    end_time = refresult[-1, 1] if refresult[-1, 1] <= navresult [-1, 1] else navresult [-1, 1]
+    start_index = np.argwhere(navresult[:, 1] >= start_time)[0, 0]
+    end_index = np.argwhere(navresult[:, 1] <= end_time)[-1, 0]
+    navresult = navresult[start_index:end_index, :]
+    navresult[:, 2:4] = navresult[:, 2:4] * D2R
+    refresult[:, 2:4] = refresult[:, 2:4] * D2R
+
+    refinter = np.zeros_like(navresult)
+    refinter[:, 1] = navresult[:, 1]
+    for col in range(2, 11):
+        refinter[:, col] = np.interp(navresult[:, 1], refresult[:, 1], refresult[:, col])
+
+    # 计算误差
+    naverror = np.zeros_like(navresult)
+    naverror[:, 1] = navresult[:, 1]
+    naverror[:, 2:11] = navresult[:, 2:11] - refinter[:, 2:11]
+
+    # 航向角误差处理
+    for i in range(len(naverror)):
+        if naverror[i, 10] > 180:
+            naverror[i, 10] -= 360
+        if naverror[i, 10] < -180:
+            naverror[i, 10] += 360
+
+    # 位置误差转到第一个位置确定的n系
+    blh_station = navresult[0, 2:5]
+    rm, rn = radiusmn(blh_station[0])
+    for i in range(len(naverror)):
+        naverror[i, 2:5] = drad2dm(rm, rn, blh_station, naverror[i, 2:5]).reshape(1, 3)
+
+    return naverror
+
+
+if __name__ == '__main__':
+
+    # 导航结果和导航误差
+    navresult_filepath = '../dataset/KF_GINS_Navresult.nav'
+    refresult_filepath = '../dataset/truth.nav'
+    # 导航结果
+    plotNavresult(navresult_filepath)
+    # 计算并绘制导航误差
+    # plotNavError(navresult_filepath, refresult_filepath)
+
+    # 估计的IMU误差
+    imuerr_filepath = '../dataset/KF_GINS_IMU_ERR.txt'
+    # plotIMUerror(imuerr_filepath)
+
+    # 估计的导航状态标准差和IMU误差标准差
+    std_filepath = '../dataset/KF_GINS_STD.txt'
+    # plotSTD(std_filepath)