kalfilt = TrackerEKF(s0, q0, points, w0, v0, p=0.02, q=0.0002, r=0.000002)
for i in range(1, 180):
# kalfilt.x[14][0] = 0
# 更新卡尔曼滤波器，获取预测值
estimate = kalfilt.step(measured_points[i])
# 四元数在update之后要归一化！！！！
num = math.sqrt(kalfilt.x[5][0]**2 + kalfilt.x[6][0]**2 + kalfilt.x[7][0]**2 + kalfilt.x[8][0]**2)
kalfilt.x[5][0] /= num
kalfilt.x[6][0] /= num
kalfilt.x[7][0] /= num
kalfilt.x[8][0] /= num
# kalfilt.x[14][0] = 0
np.savetxt("result/kalman-"+str(i), kalfilt.x)
kalman_points.append(kalfilt.x)
draw(kalman_points, [0, 1], ['X', 'Y'])
draw(kalman_points, [2, 3, 4], ['Vx', 'Vy', 'Vz'])
draw(kalman_points, [5, 6, 7, 8], ['q1', 'q2', 'q3', 'q4'])
draw(kalman_points, [9, 10, 11], ['Wx', 'Wy', 'Wz'])
for i in range(M):
draw(kalman_points, [12+i*3, 13+i*3, 14+i*3], ['X-'+str(i+1), 'Y-'+str(i+1), 'Z-'+str(i+1)])
drawQuaternions(kalman_points, [5, 6, 7, 8], ['q1', 'q2', 'q3', 'q4'], quaternions)
# kalfilt = TrackerEKF(s0, q0, points, w0, v0, p=0.2, q=0.002, r=0.000002)
kalfilt = TrackerEKF(s0, q0, points, w0, v0, p=0.02, q=0.0002, r=0.000002)
for i in range(1, 180):
# 更新卡尔曼滤波器，获取预测值
estimate = kalfilt.step(measured_points[i])
# 四元数在update之后要归一化！！！！
num = math.sqrt(kalfilt.x[6][0]**2 + kalfilt.x[7][0]**2 + kalfilt.x[8][0]**2 + kalfilt.x[9][0]**2)
kalfilt.x[6][0] /= num
kalfilt.x[7][0] /= num
kalfilt.x[8][0] /= num
kalfilt.x[9][0] /= num
np.savetxt("result/kalman-"+str(i), kalfilt.x)
kalman_points.append(kalfilt.x)
draw(kalman_points, [0, 1, 2], ['X', 'Y', 'Z'])
draw(kalman_points, [3, 4, 5], ['Vx', 'Vy', 'Vz'])
draw(kalman_points, [6, 7, 8, 9], ['q0', 'q1', 'q2', 'q3'])
draw(kalman_points, [10, 11, 12], ['Wx', 'Wy', 'Wz'])
for i in range(M):
draw(kalman_points, [13+i*3, 14+i*3, 15+i*3], ['X-'+str(i+1), 'Y-'+str(i+1), 'Z-'+str(i+1)])
drawQuaternions2(kalman_points, [6, 7, 8, 9], ['q0', 'q1', 'q2', 'q3'], quaternions)
