%matplotlib inline
import matplotlib.pyplot as plt
import numpy as np
from IPython.display import HTML
from numpy import linalg as LA
import matplotlib.animation as animation
def kalman(n,Nx):
eta = np.random.normal(0, 0.1, n+1)
t = np.linspace(0,n,n+1)
yn = np.sin(0.1*t + 4*np.sin(0.01*t)) + eta
yground = np.sin(0.1*t + 4*np.sin(0.01*t))
err = np.vstack((eta,np.roll(eta,-1)))
Ny = np.dot(err,err.T)/n
M = np.array([(1,0),(1,-1)])
A = np.eye(2)
x = np.zeros([1,n])
x[0,0:2] = np.ones(2)
xest = np.array([(1),(1)])
#xhist = np.array([(1,1),(1,1)])
xhist = np.ones(n)
B = np.eye(2)
E = np.eye(2)
for i in range(2,n):
### Estimate new observable
yest = np.sin(np.dot(B,xest))
### Get true y
ytrue = yn[i-1:i+1]
### Update Kalman matrix
temp = LA.inv(np.dot(B,np.dot(E,B.T))+Ny)
K = np.dot(np.dot(E,B.T),temp)
### estimate new state
xestnew = xest + np.dot(K,(ytrue - yest))
###
Eestnew = np.dot((np.eye(2) - np.dot(K,B)),E)
#xtemp[:,0] = xest
#xtemp[:,1:3] = xhist[:,0:2]
#xhist = xtemp
###
xest = np.dot(A,xestnew)
#xest = xest + (xhist[:,1]-xhist[:,2])
xhist[i] = xest[0]
###
E = Eestnew + Nx
return xhist,yn,yground
g = 10**-1
Nx = np.array([(g**2,0),(0,g**2)])
n = 700
xhist,yn,yground = kalman(n,Nx)
fig=plt.figure(figsize=(8, 5), dpi= 180, facecolor='w', edgecolor='k')
ax = fig.gca()
ax.plot(yground,linewidth = 2,color = 'k',label ='ground truth')
ax.plot(yn,linewidth = 2,color = 'blue',alpha = 0.5,label ='noisy data')
ax.plot(xhist,linewidth = 2,color = 'red',alpha = 0.5,label ='kalman')
handles, labels = ax.get_legend_handles_labels()
plt.legend(handles, labels)
plt.legend(loc=0)
plt.ylabel('Signal')
plt.xlabel('Time')
plt.title(' Noise = 10e-1')
g = 10**-3
Nx = np.array([(g**2,0),(0,g**2)])
n = 700
xhist,yn,yground = kalman(n,Nx)
fig=plt.figure(figsize=(8, 5), dpi= 180, facecolor='w', edgecolor='k')
ax = fig.gca()
ax.plot(yground,linewidth = 2,color = 'k',label ='ground truth')
ax.plot(yn,linewidth = 2,color = 'blue',alpha = 0.5,label ='noisy data')
ax.plot(xhist,linewidth = 2,color = 'red',alpha = 0.5,label ='kalman')
handles, labels = ax.get_legend_handles_labels()
plt.legend(handles, labels)
plt.legend(loc=0)
plt.ylabel('Signal')
plt.xlabel('Time')
plt.title(' Noise = 10e-3')
g = 10**-5
Nx = np.array([(g**2,0),(0,g**2)])
n = 700
xhist,yn,yground = kalman(n,Nx)
fig=plt.figure(figsize=(8, 5), dpi= 180, facecolor='w', edgecolor='k')
ax = fig.gca()
ax.plot(yground,linewidth = 2,color = 'k',label ='ground truth')
ax.plot(yn,linewidth = 2,color = 'blue',alpha = 0.5,label ='noisy data')
ax.plot(xhist,linewidth = 2,color = 'red',alpha = 0.5,label ='kalman')
handles, labels = ax.get_legend_handles_labels()
plt.legend(handles, labels)
plt.legend(loc=0)
plt.ylabel('Signal')
plt.xlabel('Time')
plt.title(' Noise = 10e-5')