add Python scripts to plot the result
This commit is contained in:
liqiang
parent
bb15456bdb
commit
7685d69ea2
318
scripts/plot_navresult.py
Normal file
318
scripts/plot_navresult.py
Normal file
@ -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)
|
||||||
Loading…
x
Reference in New Issue
Block a user