In [110]:
import numpy as np
import matplotlib.pyplot as plt
from scipy.io import loadmat
from pandas import DataFrame
from numpy.random import multivariate_normal
from numpy.linalg import inv
from pykalman import KalmanFilter
%matplotlib inline
In [111]:
def Relu(z):
return np.max((z, np.zeros(z.shape)), axis=0)
def dRelu(z):
return (z > 0) * 1
p = 3
q = 10
T = 500
data = loadmat('Tut11_file1.mat')
locals().update(data)
data.keys()
Out[111]:
In [144]:
z = np.zeros((p, T + 1))
x = np.zeros((q, T + 1))
np.random.seed(10)
z[:,[0]] = mu0 #+ multivariate_normal(np.zeros(p), Sigma).reshape(mu0.shape)
In [145]:
for t in range(T):
x[:, [t + 1]] = B.dot(z[:, [t]]) #+ multivariate_normal(np.zeros(q), Gamma).reshape((q, 1))
z[:, [t + 1]] = A.dot(z[:, [t]]) #+ multivariate_normal(np.zeros(p), Sigma).reshape(mu0.shape)
#z = z[:, 1:]
#x = x[:, 1:]
z.shape
Out[145]:
In [9]:
x[:, 0], mu0
Out[9]:
In [112]:
z = np.zeros((p, T + 1))
x = np.zeros((q, T + 1))
np.random.seed(10)
z[:,[0]] = mu0 #+ multivariate_normal(np.zeros(p), Sigma).reshape(mu0.shape)
def transition_f(z):
return A.dot(z) + W.dot(np.maximum(z, 0.))
def obs_g(x):
return B.dot(x)
for t in range(T):
x[:, t + 1] = obs_g(z[:, t]) + multivariate_normal(np.zeros(q), Gamma)
z[:, t + 1] = transition_f(z[:, t]) + multivariate_normal(np.zeros(p), Sigma)
x[:, 0] = np.ma.masked
In [96]:
kf = KalmanFilter()
kf.initial_state_mean = mu0.flatten()
kf.initial_state_covariance = np.zeros(Sigma.shape)
kf.transition_matrices = A
kf.transition_covariance = Sigma
kf.observation_matrices = B
kf.observation_covariance = Gamma
filtered, _= kf.filter(x[:, 1:].T)
#(smoothed_state_means, smoothed_state_covariances) = kf.smooth(x.T)
print ('Non smoothed result:', np.sum((filtered - z[:, :-1].T) ** 2))
#print('Smoothed result:', np.sum((smoothed_state_means - z.T) ** 2))
z.shape, filtered.shape
Out[96]:
In [86]:
Sigma
Out[86]:
In [94]:
#plt.plot(smoothed_state_means)
plt.plot((filtered - z[:, :-1].T))
Out[94]:
In [92]:
def transition_f(z):
return A.dot(z) + W.dot(np.maximum(z, 0.))
def obs_g(x):
return B.dot(x)
In [63]:
kf = AdditiveUnscentedKalmanFilter()
kf.initial_state_mean = mu0.flatten()
kf.initial_state_covariance = Sigma
kf.transition_functions = transition_f
kf.transition_covariance = Sigma
kf.observation_functions = obs_g
kf.observation_covariance = Gamma
filtered, _= kf.filter(x[:, 1:].T)
#(smoothed_state_means, smoothed_state_covariances) = kf.smooth(x.T)
print ('Non smoothed result:', np.sum((filtered - z[:, :-1].T) ** 2))
In [64]:
plt.plot(smoothed_state_means)
plt.plot(z.T)
print ('Non smoothed result:', np.sum((filtered - z.T) ** 2))
print('Smoothed result:', np.sum((smoothed_state_means - z.T) ** 2))
In [15]:
plt.plot(x.T[1:, :], ':');
plt.figure()
plt.plot(z.T, ':');
In [169]:
# Filter
mu = np.zeros(z.shape)
K = np.zeros((p, q, T))
V = np.zeros((p, p, T))
L = np.zeros((p, p, T))
L[..., 0] = Sigma
# mu[..., [0]] = mu0
K[...,0] = L[..., 0].dot(B.T.dot(inv(B.dot(L[..., 0].dot(B.T)) + Gamma)))
mu[..., [0]] = A.dot(mu0) + K[..., 0].dot(x[:, [0]] - B.dot(A.dot(mu0)))
V[..., 0] = (np.eye(p) - K[..., 0].dot(B)).dot(Sigma)
#L[..., 0] = A.dot(V[..., 0].dot(A.T)) + Sigma
for t in range(1, T):
L[..., t] = A.dot(V[..., t - 1].dot(A.T)) + Sigma
K[...,t] = L[..., t].dot(B.T.dot(inv(B.dot(L[..., t].dot(B.T)) + Gamma)))
mu[..., [t]] = A.dot(mu[..., [t-1]]) + K[..., t].dot(x[:, [t]] - B.dot(A.dot(mu[..., [t-1]])))
V[..., t] = (np.eye(p) - K[..., t].dot(B)).dot(L[..., t])
#L[..., t] = A.dot(V[..., t].dot(A.T)) + Sigma
# Smoother
V_tilde = np.zeros(V.shape)
mu_tilde = np.zeros(mu.shape)
V_tilde[..., -1] = V[..., -1]
mu_tilde[..., [-1]] = mu[..., [-1]]
for t in range(T - 2, -1, -1):
temp = V[..., t].dot(A.T.dot(inv(L[..., t])))
V_tilde[..., t] = V[..., t] + temp.dot(V_tilde[..., t+1] - L[..., t]).dot(temp.T)
mu_tilde[..., [t]] = mu[..., [t]] + temp.dot(mu_tilde[..., [t+1]] - A.dot(mu[..., [t]]))
In [173]:
mu[:, 0:2]
Out[173]:
In [171]:
print ('Non smoothed result:', np.sum((mu[:, :-1] - z[:, 1:]).T ** 2))
print('Smoothed result:', np.sum((mu_tilde - z).T ** 2))
plt.plot(mu_tilde.T)
Out[171]:
In [134]:
mu[:, :-1].shape
Out[134]:
In [146]:
# Filter
mu = np.zeros(z.shape)
K = np.zeros((p, q, T))
V = np.zeros((p, p, T))
L = np.zeros((p, p, T))
K[...,0] = Sigma.dot(B.T.dot(inv(B.dot(Sigma.dot(B.T)) + Gamma)))
mu[..., [0]] = A.dot(mu0) + W.dot(Relu(mu0)) + \
K[..., 0].dot(x[:, [0]] - B.dot(A.dot(mu0) + W.dot(Relu(mu0))))
V[..., 0] = (np.eye(p) - K[..., 0].dot(B)).dot(Sigma)
L[..., 0] = (A + W.dot(dRelu(mu0))).dot(V[..., 0].dot((A + W.dot(dRelu(mu0))).T)) + Sigma
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]]) + W.dot(Relu(mu[..., [t-1]])) + \
K[..., t].dot(x[:, [t]] - B.dot(A.dot(mu[..., [t-1]]) + W.dot(Relu(mu[..., [t-1]]))))
V[..., t] = (np.eye(p) - K[..., t].dot(B)).dot(L[..., t-1])
L[..., t] = (A + W.dot(dRelu(mu[..., [t-1]]))).dot(V[..., t].dot(A + W.dot(dRelu(mu[..., [t-1]])))) + Sigma
# Smoother
V_tilde = np.zeros(V.shape)
mu_tilde = np.zeros(mu.shape)
V_tilde[..., -1] = V[..., -1]
mu_tilde[..., [-1]] = mu[..., [-1]]
for t in range(T - 2, -1, -1):
temp = V[..., t].dot((A + W.dot(dRelu(mu[..., [t]]))).T.dot(inv(L[..., t])))
V_tilde[..., t] = V[..., t] + temp.dot(V_tilde[..., t+1] - L[..., t]).dot(temp.T)
mu_tilde[..., [t]] = mu[..., [t]] + temp.dot(mu_tilde[..., [t+1]] - A.dot(mu[..., [t]]) + W.dot(Relu(mu[..., [t]])))
In [147]:
plt.plot(mu_tilde.T)
Out[147]:
In [148]:
plt.plot(z.T)
Out[148]:
In [117]:
print ('Non smoothed result:', np.sum((mu[:, :-1] - z[:, 1:]).T ** 2))
print('Smoothed result:', np.sum((mu_tilde - z).T ** 2))
In [39]:
plt.plot(z[2, :].T)
plt.plot(mu_tilde[2, :].T, ':')
Out[39]:
In [130]:
mu.shape
Out[130]:
In [115]:
W
Out[115]:
In [116]:
W
Out[116]:
In [13]:
w = np.asarray([[1,0], [0,0], [0,1]])
In [14]:
w.shape
Out[14]:
In [44]:
np.maximum(np.arange(10), 5)
Out[44]:
In [139]:
plt.plot((mu[:, :-1] - z[:, 1:]).T ** 2)
Out[139]:
In [132]:
plt.plot(z.T)
Out[132]:
In [136]:
A.dot(np.zeros(3) + .02)
Out[136]:
In [152]:
z[:, 1]
Out[152]:
In [ ]: