KALMAN PYTHON

`
import numpy as np
import matplotlib.pyplot as plt

by ajust noise H and Q to see what happen

z = [i for i in range(100)]
z_watch = np.mat(z)

guasi noise with cov 1 , .2 point

noise = np.round(np.random.normal(0, 10, 100), 2) ### noise H
noise_mat = np.mat(noise)
z_mat = z_watch + noise_mat

print(z_watch)

define x state

x_mat = np.mat([[0,], [0,]])

initial

p_mat = np.mat([[1, 0], [0, 1]])

transform matrix

f_mat = np.mat([[1, 1], [0, 1]])

define conv

q_mat = np.mat([[0.0001, 0], [0.0001, 0.0]]) #### noise Q

h

h_mat = np.mat([1, 0])

noise

r_mat = np.mat([1])

for i in range(100):

x_predict = f_mat * x_mat  #  predict function: X'_t = F*X_t-1  
p_predict = f_mat * p_mat * f_mat.T + q_mat  #  transform cov p : P'_t =  F*P_t-1*F + Q 
kalman = p_predict * h_mat.T / (h_mat * p_predict * h_mat.T + r_mat) # K = P'_t*H^/(H*P'_t*H^ + R)
x_mat = x_predict + kalman *(z_mat[0, i] - h_mat * x_predict) #  X_t= X'_t = K(Z_t - H*X'_t)
p_mat = (np.eye(2) - kalman * h_mat) * p_predict # P_t = (I-K*H_t)*P'_t 

#plt.plot(x_mat[0, 0], x_mat[1, 0], 'ro', markersize = 1)
plt.plot(x_mat[0, 0], i, 'ro', markersize = 1)
plt.plot(z_mat[0, i], i, 'ro', markersize = 0.5)

plt.show()

`