kalman_filter.ipynb

Kernel: Python 2 (SageMath)

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

# time
dt = 0.1
t = np.arange(0,100,dt)

# Transition  Matrix
A = np.array([[1, 0, 0, dt, 0, 0, dt**2/2, 0, 0],
             [0, 1, 0, 0, dt, 0, 0, dt**2/2, 0],
             [0, 0, 1, 0, 0, dt, 0, 0, dt**2/2],
             [0, 0, 0, 1, 0, 0, dt, 0, 0],
             [0, 0, 0, 0, 1, 0, 0, dt, 0],
             [0, 0, 0, 0, 0, 1, 0, 0, dt],
             [0, 0, 0, 0, 0, 0, 1, 0, 0],
             [0, 0, 0, 0, 0, 0, 0, 1, 0],
             [0, 0, 0, 0, 0, 0, 0, 0, 1]])

# Measurement Matrix
H = np.array([[1, 0, 0, 0, 0, 0, 0, 0, 0],
               [0, 1, 0, 0, 0, 0, 0, 0, 0],
               [0, 0, 1, 0, 0, 0, 0, 0, 0]])

# process noise
x = 0.0028
y = 0.0059
z = 0.068
# process noise covariance
Q = np.array([[0, 0, 0, 0, 0, 0, 0, 0, 0],
             [0, 0, 0, 0, 0, 0, 0, 0, 0],
             [0, 0, 0, 0, 0, 0, 0, 0, 0],
             [0, 0, 0, x, 0, 0, 0, 0, 0],
             [0, 0, 0, 0, y, 0, 0, 0, 0],
             [0, 0, 0, 0, 0, z, 0, 0, 0],
             [0, 0, 0, 0, 0, 0, x, 0, 0],
             [0, 0, 0, 0, 0, 0, 0, y, 0],
             [0, 0, 0, 0, 0, 0, 0, 0, z]])

# Measurement noise covariance vector
C = np.array([[1, 0, 0],
              [0, 1, 0],
              [0, 0, 1]])
C = 100 * C

# Initial Conditions
# [px, vx, ax, py, vy, ay, pz, vz, az]
# starting at zero with an inital velocity of  45 m/s in the x direction
# and initial position of (0,0,0)
xk = np.zeros((9, t.size))
xk[:,0] = np.array([[0,0,0, 
  15 * np.sqrt(3), 15*  np.sqrt(3), 15* np.sqrt(3),
  0.01,0.01,0]])

# Observations
yk = np.zeros((3,t.size))
yk[:,0] = [0, 0, 0]

# Kalman Filter Parameters
M = 0.1 * np.identity(9)
xhat = np.zeros((9,t.size))

def kalman_filter_1(A,xhat_k1,M_k1,y,Q,C,H):
    """ Standard Kalman Filter
    Args:
        A - State Transition Matrix
        xhat_k-1 - previous state estimate
        M_K1 - Previous min MSE matrix
        y - Observations
        Q - Model Noise
        C - Observation Noise
        H - Measurement Matrix
    Returns:
        xhatk - updated estimat
        M - updated min MSE matrix
    """
    xhat_prediction = A.dot(xhat_k1)
    
    M_prediction = (A.dot(M_k1)).dot(A.T) + Q
    
    K  = M_prediction.dot(H.T).dot(np.linalg.inv(C + 
                         H.dot(M_prediction).dot(H.T)))
    
    xhatk = xhat_prediction + K.dot(y - H.dot(xhat_prediction))
    
    M = (np.identity(9) - K.dot(H)).dot(M_prediction)
    return xhatk, M;

# Kalman Filter 2 Parameters
M2 = 1 * np.identity(9)
xhat2 = np.zeros((9,t.size))

def kalman_filter_2(A,xhat_k1,M_k1,y,Q,C,H):
    """ Kalman Filter treating observations as independent sclar measurements
    Args:
        A - State Transition Matrix
        xhat_k1 - Ahat_k-1 previous state estimate
        M_K1 - Previous min MSE matrix
        y - Observations
        Q - Model Noise
        C - Observation Noise
        H - Measurement Matrix
    Returns:
        xhatk - updated estimat
        M - updated min MSE matrix
    """
    M = A.dot(M_k1).dot(A.T) + Q
    xhat = A.dot(xhat_k1)
    
    for i in range(y.size):

        Kbar = (1/(H[i,:].dot(M).dot(H[i,:].T)+C[i,i])) * M.dot(H[i,:].T) 
        
        M = M - np.outer(Kbar,H[i,:]).dot(M)        
        
        xhat = xhat + Kbar*(y[i] - H[i,:].dot(xhat))
        
    return xhat, M;

# Simulate the System
for i in range(t.size-1):
    xk[:,i+1] = A.dot(xk[:,i]) + np.random.multivariate_normal(np.zeros(9),Q)

    yk[:,i+1] = H.dot(xk[:,i+1]) + \
    np.random.multivariate_normal(np.zeros(3),C)
    
    (xhat[:,i+1], M) = \
    kalman_filter_1(A, xhat[:,i],M,yk[:,i+1],Q,C,H)
    
    (xhat2[:,i+1], M2) = \
    kalman_filter_2(A, xhat2[:,i],M2,yk[:,i+1],Q,C,H)

# Plots

# Sample paths
mpl.rcParams['legend.fontsize'] = 10
fig = plt.figure()
ax = fig.gca(projection='3d')
ax.plot(xk[0,:],xk[1,:],xk[2,:],label='Position')
ax.set_title('Sample Path')
ax.set_xlabel('x (m)')
ax.set_ylabel('y (m)')
ax.set_zlabel('z (m)')
ax.plot(yk[0,:],yk[1,:],yk[2,:],label='Measured Position')
ax.legend()
plt.show()

# Kalman Errors
plt.figure()
plt.rc('text', usetex=True)
plt.subplot(2,2,1)
plt.plot(t,np.abs(xk[0,:]-yk[0,:]),t,np.abs(xk[0,:]-xhat[0,:]))
plt.xlabel('time (s)')
plt.ylabel('Error (m)')
plt.title('Error X')

plt.subplot(2,2,3)
plt.plot(t,np.abs(xk[1,:]-yk[1,:]),t,np.abs(xk[1,:]-xhat[1,:]))
plt.xlabel('time (s)')
plt.ylabel('Error (m)')
plt.title('Error Y')

plt.subplot(2,2,4)
plt.plot(t,np.abs(xk[2,:]-yk[2,:]),t,np.abs(xk[2,:]-xhat[2,:]))
plt.xlabel('time (s)')
plt.ylabel('Error (m)')
plt.title('Error Z')
plt.suptitle('$|x - \hat{x}|$ Kalman Filter')
plt.legend(['$|x- y|$', '$|x - \hat x|$'])

<matplotlib.legend.Legend at 0x7efef6533e90>

# Scar updates error
plt.figure()
plt.rc('text', usetex=True)
plt.subplot(2,2,1)
plt.plot(t,np.abs(xk[0,:]-yk[0,:]),t,np.abs(xk[0,:]-xhat2[0,:]))
plt.xlabel('time (s)')
plt.ylabel('Error (m)')
plt.title('Error X')

plt.subplot(2,2,3)
plt.plot(t,np.abs(xk[1,:]-yk[1,:]),t,np.abs(xk[1,:]-xhat2[1,:]))
plt.xlabel('time (s)')
plt.ylabel('Error (m)')
plt.title('Error Y')

plt.subplot(2,2,4)
plt.plot(t,np.abs(xk[2,:]-yk[2,:]),t,np.abs(xk[2,:]-xhat2[2,:]))
plt.xlabel('time (s)')
plt.ylabel('Error (m)')
plt.title('Error Z')
plt.suptitle('$|x - \hat{x}|$ Kalman Filter Scalar updates')
plt.legend(['$|x- y|$', '$|x - \hat x|$'])

<matplotlib.legend.Legend at 0x7efef5311190>

# Observations
plt.figure()
plt.plot(t,xk[0,:],t,yk[0,:], '-x',t,xhat[0,:])
plt.xlabel('time (s)')
plt.ylabel('Position X (m)')
plt.title('State, Observation, Kalman')
plt.legend(['X','Y','Kalman'])
plt.xlim(40, 43)
plt.ylim(xk[0,int(42/dt)] -80, xk[0,int(42/dt)] +80)

(649.18363106938239, 809.18363106938239)