image_output_1.png
import numpy as np

m = 100

vx = 10
vy = 10

mx = np.array(vx + np.random.randn(m))
my = np.array(vy + np.random.randn(m))
measurements = np.vstack((mx, my))

dt = 0.1
I = np.eye(4)
x = np.matrix([[0.0, 0.0, 0.0, 0.0]]).T
P = np.diag([1000.0, 1000.0, 1000.0, 1000.0])
A = np.matrix([[1.0, 0.0, dt, 0.0], [0.0, 1.0, 0.0, dt], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]])
H = np.matrix([[0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]])
r = 100.0
R = np.matrix([[r, 0.0], [0.0, r]])
s = 8.8
G = np.matrix([[0.5 * dt ** 2], [0.5 * dt ** 2], [dt], [dt]])
Q = G * G.T * s ** 2

xt = []
yt = []
dxt = []
dyt = []
Zx = []
Zy = []
Px = []
Py = []
Pdx = []
Pdy = []
Rdx = []
Rdy = []
Kx = []
Ky = []
Kdx = []
Kdy = []

for n in range(len(measurements[0])):
    x = A * x
    P = A * P * A.T + Q
    S = H * P * H.T + R
    K = P * H.T * np.linalg.pinv(S)
    Z = measurements[:, n].reshape(2, 1)
    y = Z - H * x
    x = x + K * y
    P = (I - K * H) * P
    xt.append(float(x[0]))
    yt.append(float(x[1]))
    dxt.append(float(x[2]))
    dyt.append(float(x[3]))
    Zx.append(float(Z[0]))
    Zy.append(float(Z[1]))
    Px.append(float(P[0, 0]))
    Py.append(float(P[1, 1]))
    Pdx.append(float(P[2, 2]))
    Pdy.append(float(P[3, 3]))
    Rdx.append(float(R[0, 0]))
    Rdy.append(float(R[1, 1]))
    Kx.append(float(K[0, 0]))
    Ky.append(float(K[1, 0]))
    Kdx.append(float(K[2, 0]))
    Kdy.append(float(K[3, 0]))

def plot_K():
    fig = plt.figure(figsize=(16,9))
    plt.plot(range(len(measurements[0])),Kx, label='Kalman Gain for $x$')
    plt.plot(range(len(measurements[0])),Ky, label='Kalman Gain for $y$')
    plt.plot(range(len(measurements[0])),Kdx, label='Kalman Gain for $\dot x$')
    plt.plot(range(len(measurements[0])),Kdy, label='Kalman Gain for $\dot y$')

    plt.xlabel('Filter Step')
    plt.ylabel('')
    plt.title('Kalman Gain (the lower, the more the measurement fullfill the prediction)')
    plt.legend(loc='best',prop={'size':22})

plot_K()