In [27]:

    

import numpy as np
import matplotlib.pyplot as plt
from scipy.io import loadmat
from numpy.linalg import inv
%matplotlib inline


    

In [35]:

    

data = loadmat('data_files/Tut7_file1.mat')
locals().update(data)
data.keys()


    

    
Out[35]:





dict_keys(['__header__', '__version__', '__globals__', 'A', 'B', 'C', 'Gamma', 'L0', 'Sigma', 'mu0', 'u', 'x', 'z'])

In [22]:

    

p, T = z.shape


    

In [127]:

    

mu = np.zeros(z.shape)
K = np.zeros((4, 4, T))
V = np.zeros((4, 4, T))
L = np.zeros((4, 4, T))

K[...,0] = L0.dot(B.T.dot(inv(B.dot(L0.dot(B.T)) + Gamma)))
mu[..., [0]] = A.dot(mu0) + K[..., 0].dot(x[:, [0]] - B.dot(A.dot(mu0))) + C.dot(u[..., [0]])
V[..., 0] = (np.eye(4) - K[..., 0].dot(B)).dot(L0)
L[..., 0] = A.dot(V[..., 0].dot(A.T)) + Sigma


    

In [128]:

    

for t in range(1, T):
    K[...,t] = L[..., t - 1].dot(B.T.dot(inv(B.dot(L[..., t - 1].dot(B.T)) + Gamma)))
    mu[..., [t]] = A.dot(mu[..., [t-1]]) + K[..., t].dot(x[:, [t]] - B.dot(A.dot(mu[..., [t-1]]))) + C.dot(u[..., [t]])
    V[..., t] = (np.eye(4) - K[..., t].dot(B)).dot(L[..., t-1])
    L[..., t] = A.dot(V[..., t].dot(A.T)) + Sigma


    

In [129]:

    

plt.plot(mu.T)
plt.plot(z.T, color='red')


    

    
Out[129]:





[<matplotlib.lines.Line2D at 0x7fc3cad14e80>,
 <matplotlib.lines.Line2D at 0x7fc3cac96908>,
 <matplotlib.lines.Line2D at 0x7fc3cac96ac8>,
 <matplotlib.lines.Line2D at 0x7fc3cac96c88>]






    

In [130]:

    

V_tilde = np.zeros(V.shape)
mu_tilde = np.zeros(mu.shape)
V_tilde[..., -1] = V[..., -1]
mu_tilde[..., [-1]] = mu_tilde[..., [-1]]

for t in range(T - 2, -1, -1):
    #print(t)
    W = V[..., t].dot(A.T.dot(inv(L[..., t])))
    V_tilde[..., t] = V[..., t] + W.dot(V_tilde[..., t+1] - L[..., t]).dot(W.T)
    mu_tilde[..., [t]] = mu[..., [t]] + V[..., t].dot(A.T.dot(inv(L[..., t]).dot(mu_tilde[..., [t+1]] - A.dot(mu[..., [t]])) ))


    

In [131]:

    

plt.plot(mu_tilde.T)
plt.plot(z.T, color='red')


    

    
Out[131]:





[<matplotlib.lines.Line2D at 0x7fc3cad7e080>,
 <matplotlib.lines.Line2D at 0x7fc3cabd0ba8>,
 <matplotlib.lines.Line2D at 0x7fc3cabd0d68>,
 <matplotlib.lines.Line2D at 0x7fc3cabd0f28>]






    

In [132]:

    

plt.plot((mu - z).T ** 2)


    

    
Out[132]:





[<matplotlib.lines.Line2D at 0x7fc3caafd898>,
 <matplotlib.lines.Line2D at 0x7fc3caafda58>,
 <matplotlib.lines.Line2D at 0x7fc3caafdc50>,
 <matplotlib.lines.Line2D at 0x7fc3caafde48>]






    

In [133]:

    

np.mean((mu_tilde - z).T ** 2, axis=0) / np.mean((mu - z).T ** 2, axis=0)


    

    
Out[133]:





array([   0.96831871,    1.15059285,  126.33713054,  507.65067727])

In [134]:

    

plt.plot((mu_tilde - z).T ** 2)


    

    
Out[134]:





[<matplotlib.lines.Line2D at 0x7fc3caa94f98>,
 <matplotlib.lines.Line2D at 0x7fc3caa9e198>,
 <matplotlib.lines.Line2D at 0x7fc3caa9e390>,
 <matplotlib.lines.Line2D at 0x7fc3caa9e588>]






    

In [ ]: