sim_kalman.py

import matplotlib.pyplot as plt
import numpy as np

def gaussian(x, mu, var):
    return 1.0/np.sqrt(2.0*np.pi*var) * np.exp(-0.5*(mu-x)*(1.0/var)*(mu-x))


X = np.array([[0],[0]])
X_sigma = np.identity(2)*0.1


P_noise = np.identity(2)*0.1
Q_noise = 1
U = np.array([1])
A = np.array([[1,1],[0,1]])
B = np.array([[0],[1]])
C = np.array([[1,0]])

X_gt = []
x_pred_sigma = [X_sigma]


z_gt = []
z_gt_pred = []

epochs = 5

 
x_gt = np.array([[0],[0]])
for j in range (0,epochs):
    x_gt = A@x_gt + B*U + np.random.multivariate_normal([0,0], P_noise, 1).T
    X_gt.append(x_gt)
    z_gt.append(C@x_gt)
    x_pred_sigma.append(A@x_pred_sigma[-1]@A.T + P_noise)

    # x_pred_sigma.append(x_pred_sigma)

for j in range (0,epochs):
    z = z_gt[j] + np.random.normal(0,np.sqrt(Q_noise), 1)
    # # Kalman
    X_et = A@X + B*U # \bar{\bm{x}}_{t+1}
    X_Sigma_et = A @ X_sigma @ A.transpose() + P_noise # \bar{\bm{\Sigma}}_{t+1} 
    S = (C@[email protected](C)+Q_noise)
    #K_t = X_Sigma_et@C @ np.linalg.inv(S)
    K_t = [email protected](C) * (1.0/S)
    X = X_et +  K_t*(z-C@X_et)
    X_Sigma = (np.identity(2) - K_t@C)*X_Sigma_et


    #ploting
    y=[]
    x_measurment=[]
    x_ekf=[]
    x_prediction=[]
    for i in np.arange(-5,13,0.1):
     #,X_sigma[0,0]+Q_noise)
      x_measurment.append(gaussian(i,float(z),float(Q_noise)))
      x_ekf.append(gaussian(i,float(C@X),float(X_Sigma[0][0])))
      x_prediction.append(gaussian(i,float(C@(X_gt[j])),float(x_pred_sigma[j][0][0]) ))
      y.append(i)

    plt.subplot(epochs,1,j+1)
    plt.title("Probability distributions of position at time moment %d"%j)
    plt.ylabel("probability")  
    plt.plot(y,x_measurment,'-.')
    plt.plot(y,x_prediction,'-.')
    plt.plot(y,x_ekf,'b')
    plt.legend(['state estimation from measurment only','state estimation from previous state only', 'state estimation from EKF'])


plt.tight_layout(True)  
plt.show()