从数据集中收集到一些IMU传感器输出的测量值和参考位姿，现需要对他们进行积分得到位姿，并与位姿真值进行对比
数据读取
准备的数据以txt格式保存，每行表示一组测量，其存放格式为
# 时间，真实位移x，真实位移x，真实位移x，真实四元数qx，真实四元数qx，真实四元数qx，真实四元数qx，测量加速度x，测量加速度y，测量加速度z，测量角速度x，测量角速度y，测量角速度z
t,x,y,z,qx,qy,qz,qw,accx,accy,accz,gyrox,gyroy,gyroz
数据示例：
0.000000000 -0.982430570 0.462775503 1.440096452 0.017048921 0.017442947 0.999681640 0.006457347 9.128356708 0.106238708 -2.606934458 0.094945911 0.020245819 0.058643063
0.010116100 -0.982516322 0.462718628 1.440084893 0.016769881 0.016830918 0.999696135 0.006567209 9.398039583 1.618097250 -2.705000958 -0.011170107 0.027925268 0.069813170
0.020016193 -0.982455216 0.462720566 1.440119365 0.017149872 0.017305289 0.999682528 0.006422704 8.989429167 -0.130755333 -2.713173167 -0.092153385 0.036302848 0.078888882
0.030037880 -0.982471853 0.462759382 1.440067216 0.016636406 0.016787754 0.999699626 0.006485327 9.299973083 -1.234003458 -2.476179125 -0.005585054 0.009075712 0.113795467
0.039942741 -0.982428366 0.462761779 1.440089186 0.017071389 0.017441812 0.999681696 0.006392007 9.275456458 0.073549875 -2.582417833 0.061435590 0.023736478 0.091455253
0.050014973 -0.982485600 0.462756273 1.440062406 0.016627783 0.016859991 0.999698348 0.006516953 9.471589458 0.400438208 -2.606934458 0.009773844 0.028623400 0.060039326
0.060015678 -0.982469652 0.462743111 1.440121145 0.017145077 0.017273791 0.999682861 0.006468433 9.332661917 0.122583125 -2.705000958 -0.032114058 0.042586034 0.086568331
0.069996595 -0.982430145 0.462763755 1.440059984 0.017014370 0.017147056 0.999688199 0.006324076 9.659550250 -0.375921583 -2.917478375 -0.030717795 0.041887902 0.094945911
0.080028534 -0.982400129 0.462789534 1.440090802 0.017226939 0.017420019 0.999680099 0.006282920 9.291800875 -0.490332500 -2.598762250 -0.000698132 0.024434610 0.077492619
0.090289354 -0.982467627 0.462647409 1.440103708 0.016840361 0.017015106 0.999695012 0.006064004 9.447072833 -0.098066500 -2.655967708 0.032812190 0.008377580 0.075398224
数据读取程序：
raw_data = []
with open("MyData.txt", "r") as f:
for line in f.readlines():
line = line.strip('\n') # 删除行尾的换行符号
data = line.split('\n\t')
for str in data:
sub_str = str.split(' ')
if sub_str:
raw_data.append(sub_str)
解释：
- .readlines() 方法用于读取所有行(直到结束符 EOF)并返回列表
- .strip(‘x’) 方法 删除s字符串中开头、结尾处的字符x，当rm为空时，默认删除空白符（包括’\n’, ‘\r’, ‘\t’, ’ ')
- .split(‘x’) 方法 拆分字符串。通过指定分隔符’x’对字符串进行切片，并返回分割后的字符串列表
list 转 numpy
由于直接读取的是 list 格式，为方便运算，需要将其转为 numpy 格式
raw_data = np.array(raw_data)
print(type(raw_data))
print(raw_data.shape)
截取参考位姿和IMU测量值
PoseTrue = raw_data[:,0:8]
IMURaw = raw_data[:,8:14]
注意：在使用任何数据集时，都需要重新梳理各个坐标系的关系
以Euroc数据集为例，IMU的X-Y-Z与VICON的X-Y-Z是不一致的。
为了方便对齐坐标系，可以将IMU测量的重力加速度乘以参考姿态角构成的矩阵，然后看与[0,0,g]是否大致相符，如果不相符，则适当调整IMU各轴的顺序
定义积分函数
积分函数的输入为 初始位姿，和 所有的 IMU 测量值
def Integration(PoseTrue, VelInit, IMU):
print("I am in Integration")
# Position
InitPos = PoseTrue[0][1:4]
# quaternion
InitQuat = PoseTrue[0][4:8]
# quat -> euler
InitEuler = R.from_quat(InitQuat).as_euler('zyx')
# attitude angles
phi = np.zeros((len(IMU),1))
theta = np.zeros((len(IMU),1))
psi = np.zeros((len(IMU),1))
C_ned2b = np.zeros((len(IMU), 3, 3))
C_b2ned = np.zeros((len(IMU), 3, 3))
Pos = np.zeros((len(IMU),3))
Vel = np.zeros((len(IMU),3))
time_s = PoseTrue[:,0];
#
phi[0] = InitEuler[2] # x-axis
theta[0] = InitEuler[1] # y-axis
psi[0] = InitEuler[0] # z-axis
Pos[0] = InitPos
Vel[0] = VelInit
# return value
Rtn = np.zeros((len(IMU), 6))
for i in range(len(IMURaw)-1):
dt = time_s[i+1] - time_s[i]
# preparation
C11 = math.cos(theta[i])*math.cos(psi[i])
C12 = math.cos(theta[i])*math.sin(psi[i])
C13 = -math.sin(theta[i])
C21 = math.sin(phi[i])*math.sin(theta[i])*math.cos(psi[i])-math.cos(phi[i])*math.sin(psi[i])
C22 = math.sin(phi[i])*math.sin(theta[i])*math.sin(psi[i])+math.cos(phi[i])*math.cos(psi[i])
C23 = math.sin(phi[i])*math.cos(theta[i])
C31 = math.cos(phi[i])*math.sin(theta[i])*math.cos(psi[i])+math.sin(phi[i])*math.sin(psi[i])
C32 = math.cos(phi[i])*math.sin(theta[i])*math.sin(psi[i])-math.sin(phi[i])*math.cos(psi[i])
C33 = math.cos(phi[i])*math.cos(theta[i])
C_ned2b[i] = np.matrix([
[C11, C12, C13],
[C21, C22, C23],
[C31, C32, C33]
])
C_b2ned[i] = C_ned2b[i].transpose()
C_bodyrate2eulerdot = np.matrix([
[1, math.sin(phi[i])*math.tan(theta[i]), math.cos(phi[i])*math.tan(theta[i])],
[0, math.cos(phi[i]), -math.sin(phi[i])],
[0, math.sin(phi[i])/math.cos(theta[i]), math.cos(phi[i])/math.cos(theta[i])]
])
# IMU
wx = IMU[i][3]
wy = IMU[i][4]
wz = IMU[i][5]
fx = IMU[i][0]
fy = IMU[i][1]
fz = IMU[i][2]
vGyro = np.matrix([
[wz],
[-wy],
[wx]
])
vAcc = np.matrix([
[fz],
[-fy],
[fx]
])
vG = np.matrix([
[0],
[0],
[9.82]
])
dot_atti = np.dot(C_bodyrate2eulerdot, vGyro)
acc_n = np.dot(C_b2ned[i], vAcc).astype(float) - vG.astype(float)
Pos[i+1] = Pos[i] + Vel[i]*dt + 0.5*acc_n.A.squeeze()*dt*dt;
Vel[i+1] = Vel[i] + acc_n.A.squeeze()*dt;
atti_last = np.array([phi[i], theta[i], psi[i]])
atti_next = atti_last + dot_atti*dt
phi[i+1] = atti_next[0]
theta[i+1] = atti_next[1]
psi[i+1] = atti_next[2]
Rtn[:,0:3] = Pos
Rtn[:,3] = phi.squeeze()
Rtn[:,4] = theta.squeeze()
Rtn[:,5] = psi.squeeze()
return Rtn
（与matlab相比，python的编程还是比较繁琐，需要时时关注数据的维度）
使用 matplotlib 绘图
使用 pip install matplotlib 安装matplotlib 库，然后引入
import matplotlib
matplotlib.use('Qt5Agg')#必须显式指明matplotlib的后端
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
