Contact
CoCalc Logo Icon
StoreFeaturesDocsShareSupport News AboutSign UpSign In
| Download
Project: EELE 577
Views: 43
Kernel: Python 2 (SageMath)
import matplotlib as mpl import numpy as np import matplotlib.pyplot as plt
dt = 1 t = np.arange(0,101,dt)
# System Update Matrix A = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]])
# System Noise Matrix varu = 0.0001 Q = np.array([[0, 0, 0, 0], [0, 0, 0, 0], [0, 0, varu, 0], [0, 0, 0, varu]])
# Measurement Noise varR = 0.1 varB = 0.01 C = np.array([[varR, 0], [0, varB]])
# State Vector x = np.zeros((4,t.size)) # initial Conditon x[:,0] = [10, -5, -0.2, 0.2]
# Measurment Vector y = np.zeros((2,t.size)) y[:,0] = np.array([np.sqrt(x[0,0]**2 + x[1,0]**2), np.arctan2(x[1,0],x[0,0])]) + \ np.random.multivariate_normal(np.zeros(2),C)
# Extended Kalman Filter M = 100 * np.identity(4) #estimate Vector xhat = np.zeros((4,t.size)) #inital estimate xhat[:,0] = [5, 5, 0, 0] def extended_kalman(y, xhat_k1, M_k1, A, Q, C): """ Extended Kalman Filter Args: A - State Transition Matrix xhat_k1 previous state estimate M_K1 - Previous min MSE matrix y - Observations Q - Model Noise C - Observation Noise Returns: xhatk - updated estimate M - updated min MSE matrix """ def H_jacobian(x): """ Compute Jacobian of Observation Function Args: x Returns: y """ rx = x[0] ry = x[1] R = np.sqrt(rx**2 + ry**2) H = np.array([[rx/R, ry/R, 0, 0], [-ry/(R**2), rx/(R**2), 0, 0]]) return H xhat_pred = A.dot(xhat_k1) M_pred = A.dot(M_k1).dot(A.T) + Q H = H_jacobian(xhat_pred) K = M_pred.dot(H.T).dot(np.linalg.inv(C + H.dot(M_pred).dot(H.T))) rx = xhat_pred[0] ry = xhat_pred[1] h = np.array([np.sqrt(rx**2 + ry**2), np.arctan2(ry,rx)]) xhat = xhat_pred + K.dot(y - h) M = (np.identity(4)- K.dot(H)).dot(M_pred) return xhat, M;
# Simulate System for n in range(1,t.size): x[:,n] = A.dot(x[:,n-1]) + np.random.multivariate_normal(np.zeros(4),Q) y[:,n] = np.array([np.sqrt(x[0,n]**2 + x[1,n]**2), np.arctan2(x[1,n],x[0,n])]) + \ np.random.multivariate_normal(np.zeros(2),C) xhat[:,n], M = extended_kalman(y[:,n],xhat[:,n-1], M, A, Q, C)
# Plots
mpl.rcParams['legend.fontsize'] = 12 plt.figure() rxobs = y[0,:] * np.cos(y[1,:]) ryobs = y[0,:] * np.sin(y[1,:]) plt.plot(xhat[0,:], xhat[1,:],rxobs, ryobs, x[0,:], x[1,:],linewidth=3.0) #plt.plot(s[0,:], s[1,:]) plt.xlabel('rx (Distance)') plt.ylabel('ry (Distance)') plt.title('Position Observation and Kalman') plt.legend(['Kalman','Observation', 'True State'])
<matplotlib.legend.Legend at 0x7f062873e2d0>
Image in a Jupyter notebook
plt.figure() mpl.rcParams['legend.fontsize'] = 12 rxobs = y[0,:] * np.cos(y[1,:]) ryobs = y[0,:] * np.sin(y[1,:]) plt.subplot(1,2,1) plt.plot(t,np.abs(xhat[0,:]- x[0,:]), t,np.abs(rxobs - x[0,:])) plt.xlabel('time (s)') plt.ylabel('error (Distance)') plt.title('Error X') plt.legend(['Kalman','Observation']) plt.subplot(1,2,2) plt.plot(t,np.abs(xhat[1,:]- x[1,:]), t,np.abs(ryobs - x[1,:])) plt.xlabel('time (s)') plt.ylabel('error (Distance)') plt.title('Error Y') plt.legend(['Kalman','Observation'])
<matplotlib.legend.Legend at 0x7f062111e950>
Image in a Jupyter notebook