kalmar

import numpy as np
import matplotlib.pyplot as plt

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)

label = "Przewidziana trajektoria za " + np.str(num) + "s"
fig, axs = plt.subplots()
axs.plot(p_x,p_y, 'rx', label='Dane pomiarowe z pliku')
axs.plot([i[0] for i in s],[i[1] for i in s], 'b', label='Trajektoria samolotu')
axs.plot([i[0] for i in new_z],[i[1] for i in new_z], 'g', label=label)
axs.plot(new_z[-1][0],new_z[-1][1], 'go')
axs.axhline(y=0, color='k')
axs.axvline(x=0, color='k')
axs.legend()
plt.show()