q = Quaternion(1,0,0,0)
for i in range(imu_val.shape[1]):
# Time increment
if i == 0:
dt = 0.01
else:
dt = ts[0, i] - ts[0, i - 1]
#find angles and axis
v = imu_val[3:6, i]
v_norm = (v[0]**2 + v[1]**2 + v[2]**2)**0.5
angle = v_norm*dt
axis = v / v_norm
qt = qu.angle_axis2quat(angle,axis)
q *= qt
R = q.q_rot()
#Gyro_rot[:,:,i] = R
r,p,y = qu.rot2rpy(R)
Gyro_rpy[0, i] = r
Gyro_rpy[1, i] = p
Gyro_rpy[2, i] = y
#Plot
print('Begin Plot \n')
plt.figure(1)
plt.subplot(311)
#plt.plot(vicon_raw['ts'].flatten(), r_vicon, 'y-', label='vicon')
plt.plot(imu_raw['ts'].flatten(), r_UKF,'r-', label='UKF')
plt.plot(imu_raw['ts'].flatten(), Acc_rpy[0,:],'g-', label='Acc')
plt.plot(imu_raw['ts'].flatten(), Gyro_rpy[0,:],'b-', label='Gyro')
plt.xlabel('time')
