def _predict_update(self, t):
"""Calculate fileter update
Args:
t {int} : observation time
"""
# extract parameters for time t-1
b = _last_dims(self.b, t - 1, 1)
Q = _last_dims(self.Q, t - 1, 2)
# calculate predicted distribution for time t
self.x_pred[t] = self.F @ self.x_filt[t - 1] + b
self.V_pred = self.F @ self.V_filt @ self.F.T + Q
def _predict_update(self, t, F=None):
"""Calculate fileter update
Args:
t {int} : observation time
"""
# extract parameters for time t-1
if F is None:
F = _last_dims(self.F, t - 1, 2)
Q = _last_dims(self.Q, t - 1, 2)
b = _last_dims(self.b, t - 1, 1)
# calculate predicted distribution for time t
self.x_pred[t] = F @ self.x_filt[t - 1] + b
self.V_pred[t] = F @ self.V_filt[t - 1] @ F.T + Q
def _filter_update(self, t):
"""Calculate fileter update without noise
Args:
t {int} : observation time
Attributes:
K [n_dim_sys, n_dim_obs] {numpy-array, float}
: Kalman gain matrix for time t
"""
# extract parameters for time t
H = _last_dims(self.H, t, 2)
R = _last_dims(self.R, t, 2)
d = _last_dims(self.d, t, 1)
# calculate filter step
K = self.V_pred[t] @ (
H.T @ self.xp.linalg.pinv(H @ (self.V_pred[t] @ H.T) + R))
self.x_filt[t] = self.x_pred[t] + K @ (self.y[t] -
(H @ self.x_pred[t] + d))
self.V_filt[t] = self.V_pred[t] - K @ (H @ self.V_pred[t])
def _update_transition_matrix_with_variance(self, t):
"""Update transition matrix with variance of transition matrix
(this function is under construction)
Args:
t {int} : observation time
"""
H = _last_dims(self.H, t, 2)
R = _last_dims(self.R, t, 2)
Rb = _last_dims(self.R, t-1, 2)
Gh = self.y[t-self.update_interval+1:t+1].T \
@ self.xp.linalg.pinv(self.y[t-self.update_interval:t].T)
Fh = self.xp.linalg.pinv(H) @ Gh @ H
self.F = self.F - self.eta * self.xp.minimum(self.xp.maximum(-self.cutoff, self.F - Fh), self.cutoff)
self.Fs[t//self.update_interval] = self.F
# Calculate variance of observation transition
Sh = H @ self.V_pred[t] @ H.T + R - Gh @ (H @ self.V_filt[t-1] @ H.T + Rb) @ Gh.T
# Calculate variance of transition matrix
nu = self.xp.zeros(self.n_dim_sys * self.update_interval, dtype=self.dtype)
kappa = self.xp.zeros(self.n_dim_sys * self.update_interval, dtype=self.dtype)
X = self.xp.zeros((self.n_dim_sys * self.update_interval, self.n_dim_sys**2), dtype=self.dtype)
for i,s in enumerate(range(t-self.update_interval, t)):
Q = _last_dims(self.Q, s, 2)
R = _last_dims(self.R, s, 2)
X0 = self.xp.square(self.x_filt[s]) + self.xp.diag(self.V_filt[s])
for j in range(self.n_dim_sys):
X[self.n_dim_sys*i+j, self.n_dim_sys*j:self.n_dim_sys*(j+1)] = X0
nu0 = self.xp.diag(self.V_pred[s+1])
nu[self.n_dim_sys*i:self.n_dim_sys*(i+1)] = nu0
kappa0 = self.xp.diag(self.xp.linalg.pinv(H) @ (Gh @ (H @ self.V_filt[s] @ H.T) @ Gh.T \
+ Sh - R) @ self.xp.linalg.pinv(H.T) - Q)
kappa[self.n_dim_sys*i:self.n_dim_sys*(i+1)] = kappa0
self.FV[t//self.update_interval] = (self.xp.linalg.pinv(X) @ (kappa - nu))\
.reshape(self.n_dim_sys, self.n_dim_sys)
def _predict_update_lag(self, t, L):
"""Calculate fileter update without noise
Args:
t {int} : observation time
"""
# extract parameters for time t-1
Q = _last_dims(self.Q, t - 1, 2, self.use_gpu)
# calculate predicted distribution for time t
low = max(0, t - L)
self.x_pred[t] = self.F @ self.x_filt[t - 1]
self.V_pred[t] = self.F @ self.V_filt[t - 1] @ self.F.T + Q
self.V_pred[low:t] = self.V_filt[low:t] @ self.F.T
def _update_transition_matrix(self, s, tau, F=None):
"""Calculate estimation of state transition matrix by maximization of likelihood.
Args:
s {int} : last time for fixed-interval smoothing
"""
self._backward(s, tau, F)
if "F" in self.em_vars:
res1 = self.xp.zeros((self.n_dim_sys, self.n_dim_sys),
dtype=self.dtype)
res2 = self.xp.zeros((self.n_dim_sys, self.n_dim_sys),
dtype=self.dtype)
for t in range(s - tau + 1, s + 1):
b = _last_dims(self.b, t - 1, 1)
res1 += self.V_pair[t] + self.xp.outer(self.x_smooth[t],
self.x_smooth[t - 1])
res1 -= self.xp.outer(b, self.x_smooth[t - 1])
res2 += self.V_smooth[t - 1] \
+ self.xp.outer(self.x_smooth[t - 1], self.x_smooth[t - 1])
F_est = res1 @ self.xp.linalg.pinv(res2)
if "b" in self.em_vars and tau > 1:
b_est = self.xp.zeros(self.n_dim_sys, dtype=self.dtype)
for t in range(s - tau + 1, s):
F = _last_dims(self.F, t - 1)
b_est += self.x_smooth[t] - F @ self.x_smooth[t - 1]
b_est *= (1.0 / (tau - 1))
# update transition offset
self.b = self.b - self.etab * self.xp.minimum(
self.xp.maximum(-self.cutoffb, self.b - b_est), self.cutoffb)
return F_est
def _update_transition_matrix_approximately(self, t):
res1 = self.xp.zeros((self.n_dim_sys, self.n_dim_sys),
dtype=self.dtype)
res2 = self.xp.zeros((self.n_dim_sys, self.n_dim_sys),
dtype=self.dtype)
for s in range(t + 1, t + self.tau + 1):
b = _last_dims(self.b, s - 1, 1)
res1 += self.V_pair[s] + self.xp.outer(self.x_filt[s],
self.x_filt[s - 1])
res1 -= self.xp.outer(b, self.x_filt[s - 1])
res2 += self.V_filt[s - 1] \
+ self.xp.outer(self.x_filt[s - 1], self.x_filt[s - 1])
F_est = res1 @ self.xp.linalg.pinv(res2)
# update transition matrix
self.F = self.F - self.eta * self.xp.minimum(
self.xp.maximum(-self.cutoff, self.F - F_est), self.cutoff)
if self.store_transition_matrices_on:
self.Fs[t // self.tau + 1] = self.F
def _filter_update_lag(self, t, L):
"""Calculate fileter update without noise
Args:
t {int} : observation time
"""
# extract parameters for time t-1
R = _last_dims(self.R, t, 2, self.use_gpu)
# calculate filter step
low = max(0, t - L)
self.x_filt[t] = self.x_pred[t].copy()
K = self.V_pred[low:t + 1] @ (self.H.T @ self.xp.linalg.pinv(
self.H @ (self.V_pred[t] @ self.H.T) + R)) # lag x Nx x Ny
self.x_filt[low:t +
1] = self.x_filt[low:t +
1] + K @ (self.y[t] -
(self.H @ self.x_pred[t]))
# lag x Nx x Nx
self.V_filt[low:t +
1] = self.V_pred[low:t + 1] - K @ (self.H @ self.V_pred[t])
def _backward(self, s, tau, F=None):
"""Calculate smoothed estimation by RTS-smoother.
Args:
s {int} : last time for fixed-interval smoothing
Attributes:
A [n_dim_sys, n_dim_sys] {numpy-array, float}
: fixed interval smoothed gain
"""
if F is None:
F = _last_dims(self.F, t - 1, 2)
# pairwise covariance
A = self.xp.zeros((self.n_dim_sys, self.n_dim_sys), dtype=self.dtype)
self.x_smooth[s] = self.x_filt[s].copy()
self.V_smooth[s] = self.V_filt[s].copy()
# t in [s-tau, s-1]
for t in reversed(range(s - tau, s)):
# visualize calculating time
print("\r expectation step calculating... t={}".format(s - t) +
"/" + str(tau),
end="")
# calculate fixed interval smoothing gain
A = self.V_filt[t] @ F @ self.xp.linalg.pinv(self.V_pred[t + 1])
# fixed interval smoothing
self.x_smooth[t] = self.x_filt[t] \
+ A @ (self.x_smooth[t + 1] - self.x_pred[t + 1])
self.V_smooth[t] = self.V_filt[t] \
+ A @ (self.V_smooth[t + 1] - self.V_pred[t + 1]) @ A.T
# calculate pairwise covariance
self.V_pair[t + 1] = self.V_smooth[t + 1] @ A.T