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