3d kalmar

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

T = 1

p_x = []
p_y = []
s = []
P = []
z = []

z_p = [0]
e = [0]
S = [0]
K = [0]

P_p = []
s_p = []

w = 1

P.append(np.identity(4) * 5)
Q = np.identity(2) * 0.25
R = np.identity(2) * 2.0
I = np.identity(4)

F = np.identity(4)
F[0][2] = T
F[1][3] = T

G = np.ndarray((4,2))
G.fill(0)
G[2][0] = 1.0
G[3][1] = 1.0

H = np.ndarray((2,4))
H.fill(0)
H[0][0] = 1.0
H[1][1] = 1.0

with open("measurements17.txt", "r") as f:
    lines = f.read().splitlines()

for line in lines:
    first, second = line.split()
    p_x.append(np.float(first))
    p_y.append(np.float(second))

s.append([p_x[0],p_y[0],0,0])
z.append([p_x[0],p_y[0]])

for i in range(len(lines)-1):

    s.append(F@s[i])
    P.append(F@P[i]@F.T + G@Q@G.T)
    z_p.append(H@s[i+1])

    z.append([p_x[i+1],p_y[i+1]])
    e.append(z[i+1] - z_p[i+1])
    S.append(H@P[i+1]@H.T + R)
    K.append(P[i+1]@H.T @ np.linalg.inv(S[i+1]))
    s[i+1] = s[i+1] + K[i+1]@e[i+1]
    P[i+1] = (I - K[i+1]@H)@P[i+1]

num = 5
s_p.append(s[-1])
P_p.append(P[-1])

new_z = [[s[-1][0],s[-1][1]]]

for k in range(num):
    s_p.append(F@s_p[k])
    P_p.append(F@P_p[k]@F.T + G@Q@G.T)
    new_z.append(H@s_p[k+1])

draw_x = []
draw_y = []

for pred_x,pred_y in new_z:
    draw_x.append(pred_x)
    draw_y.append(pred_y)

t = np.arange(0,len(s),1)
t_2 = np.arange(len(s)-1,len(s)+num,1)

label = "Przewidziana trajektoria za " + np.str(num) + "s"
fig, axs = plt.subplots(2,2)
gs = axs[0, 0].get_gridspec()

for ax in axs[:, 0]:
    ax.remove()
axbig = fig.add_subplot(gs[:, 0], projection='3d')
fig.tight_layout()

axbig.plot3D([i[0] for i in s], [i[1] for i in s],t)

axbig.plot3D([i[0] for i in s],[i[1] for i in s],t, 'b', label='Trajektoria samolotu')
axbig.plot3D(p_x,p_y,t, 'rx', label='Dane pomiarowe z pliku')
axbig.plot3D([i[0] for i in new_z],[i[1] for i in new_z],t_2, 'g', label=label)
axbig.plot3D([new_z[-1][0]],new_z[-1][1],[t_2[-1]], 'go')
axbig.set_xlabel("Położenie X")
axbig.set_ylabel("Położenie Y")
axbig.set_zlabel("Czas")
axbig.legend()

axs[0][1].plot(t,[i[2] for i in s], 'b', label='Prędkość w osi X')
axs[1][1].plot(t,[i[3] for i in s], 'b', label='Prędkość w osi Y')

axs[0][1].plot(t_2,[i[2] for i in s_p], 'g', label='Prędkość w osi X przewidziana jest stała')
axs[1][1].plot(t_2,[i[3] for i in s_p], 'g', label='Prędkość w osi Y przewidziana jest stała)')

for ax in axs:
     ax[1].grid(True, which='both')
     ax[1].axhline(y=0, color='k')
     ax[1].axvline(x=0, color='k')
     ax[1].set_ylabel("V [m/s]")
     ax[1].set_xlabel("Czas [s]")
     ax[1].legend()

fig.set_size_inches(14,5)
plt.show()