class KF(LSProcess):
def __init__(self, x0: np.ndarray, F: Callable, H: np.ndarray,
Q: np.ndarray, R: np.ndarray, dt: float):
self.F = F
self.H = H
self.Q = Q
self.R = R
self.dt = dt
self.ndim = x0.shape[0]
rep_ndim = self.ndim * (self.ndim + 1) # representational dimension
def f(t, state, z_t, F_t):
x_t, P_t = self.load_vars(state)
z_t = z_t[:, np.newaxis]
K_t = P_t @ self.H @ np.linalg.inv(self.R)
d_x = F_t @ x_t + K_t @ (z_t - self.H @ x_t)
d_P = F_t @ P_t + P_t @ F_t.T + self.Q - K_t @ self.R @ K_t.T
d_state = np.concatenate((d_x, d_P), axis=1)
return d_state.ravel() # Flatten for integrate.ode
def g(t, state):
return np.zeros((rep_ndim, rep_ndim))
x0 = x0[:, np.newaxis]
P0 = np.eye(self.ndim)
self.x_t = x0
self.P_t = P0
iv = np.concatenate((x0, P0),
axis=1).ravel() # Flatten for integrate.ode
self.r = Integrator(f, g, rep_ndim)
self.r.set_initial_value(iv, 0.)
def load_vars(self, state: np.ndarray):
state = state.reshape((self.ndim, self.ndim + 1))
x_t, P_t = state[:, :1], state[:, 1:]
return x_t, P_t
def __call__(self, z_t: np.ndarray):
''' Observe through filter '''
self.r.set_f_params(z_t, self.F(self.t))
self.r.integrate(self.t + self.dt)
x_t, P_t = self.load_vars(self.r.y)
self.x_t, self.P_t = x_t, P_t
x_t = np.squeeze(x_t)
err_t = z_t - x_t @ self.H.T
return x_t.copy(), err_t # x_t variable gets reused somewhere...
class LKF(LSProcess):
def __init__(self, x0: np.ndarray, F: Callable, H: np.ndarray, Q: np.ndarray, R: np.ndarray, dt: float, tau: float = float('inf'), eps=1e-4):
self.F = F
self.H = H
self.Q = Q
self.R = R
self.dt = dt
self.tau = tau
self.eps = eps
self.eta_mu = eta_mu
self.eta_var = eta_var
self.ndim = x0.shape[0]
self.e_zz_t = np.zeros((self.ndim, self.ndim)) # temp var..
self.p_inv_t = np.zeros((self.ndim, self.ndim)) # temp var..
self.kf_err_hist = []
self.kf = KF(x0, F, H, Q, R, dt)
def f(t, state, z_t, err_hist, F_t):
# TODO fix all stateful references in this body
x_t, P_t, eta_t = self.load_vars(state)
z_t = z_t[:, np.newaxis]
K_t = [email protected]@np.linalg.inv(self.R)
d_eta = np.zeros((self.ndim, self.ndim))
if t > self.tau: # TODO warmup case?
H_inv = np.linalg.inv(self.H)
P_kf_t = self.kf.P_t
P_inv = np.linalg.solve(P_kf_t.T@P_kf_t + self.eps*np.eye(self.ndim), P_kf_t.T)
self.p_inv_t = P_inv
tau_n = int(self.tau / self.dt)
err_t, err_tau = err_hist[-1][:,np.newaxis], err_hist[-tau_n][:,np.newaxis]
d_zz = (err_t@err_t.T - err_tau@err_tau.T) / self.tau
self.e_zz_t = d_zz
# E_z = sum(err_hist[-tau_n:])[:,np.newaxis] / tau_n
# d_uu = ((err_t - err_tau)/self.tau)@(E_z.T) + E_z@(((err_t - err_tau)/self.tau).T)
# self.e_zz_t = d_zz - d_uu
# if np.linalg.norm(d_zz) >= 1.0:
# d_zz = np.zeros((self.ndim, self.ndim))
d_eta = (H_inv@d_zz@H_inv.T@P_inv / 2 - eta_t) / self.tau
F_est = F_t - eta_t
d_x = F_est@x_t + K_t@(z_t - self.H@x_t)
d_P = F_est@P_t + P_t@F_est.T + self.Q - [email protected]@K_t.T
d_state = np.concatenate((d_x, d_P, d_eta), axis=1)
return d_state.ravel() # Flatten for integrator
def g(t, state):
return np.zeros((self.ode_ndim, self.ode_ndim))
# state
x0 = x0[:, np.newaxis]
self.x_t = x0
# covariance
P0 = np.eye(self.ndim)
self.P_t = P0
# model variation
eta0 = np.zeros((self.ndim, self.ndim))
self.eta_t = eta0
iv = np.concatenate((x0, P0, eta0), axis=1).ravel() # Flatten for integrator
self.r = Integrator(f, g, self.ode_ndim)
self.r.set_initial_value(iv, 0.)
def load_vars(self, state: np.ndarray):
state = state.reshape(self.ode_shape)
x_t, P_t, eta_t = state[:, :1], state[:, 1:1+self.ndim], state[:, 1+self.ndim:]
return x_t, P_t, eta_t
def __call__(self, z_t: np.ndarray):
''' Observe through filter '''
_, kf_err_t = self.kf(z_t)
self.kf_err_hist.append(kf_err_t)
self.r.set_f_params(z_t, self.kf_err_hist, self.F(self.t))
self.r.integrate(self.t + self.dt)
x_t, P_t, eta_t = self.load_vars(self.r.y)
self.x_t, self.P_t, self.eta_t = x_t, P_t, eta_t
x_t = np.squeeze(x_t)
err_t = z_t - [email protected]
return x_t.copy(), err_t # x_t variable gets reused somewhere...
@property
def ode_shape(self):
return (self.ndim, 1 + 2*self.ndim) # representational dimension: x_t, P_t, eta_t
@property
def ode_ndim(self):
return self.ode_shape[0] * self.ode_shape[1] # for raveled representation
def f_eta(self, eta: np.ndarray):
""" density function of variations """
return stats.multivariate_normal.pdf(eta.ravel(), mean=self.eta_mu.ravel(), cov=self.eta_var.ravel())
class LKF(LSProcess):
def __init__(self, x0: np.ndarray, F: Callable, H: np.ndarray, Q: np.ndarray, R: np.ndarray, dt: float, tau: float = float('inf'), eta_mu: float = 0., eta_var: float = 1., eps=1e-2):
self.F = F
self.H = H
self.Q = Q
self.R = R
self.dt = dt
self.tau = tau
self.eps = eps
self.eta_mu = eta_mu
self.eta_var = eta_var
self.ndim = x0.shape[0]
self.err_hist = []
self.eta_t = np.zeros((self.ndim, self.ndim)) # temp var..
self.e_zz_t = np.zeros((self.ndim, self.ndim)) # temp var..
self.p_inv_t = np.zeros((self.ndim, self.ndim)) # temp var..
self.p_t = np.zeros((self.ndim, self.ndim)) # temp var..
self.zz_t = np.zeros((self.ndim, self.ndim)) # temp var..
self.zz_tau = np.zeros((self.ndim, self.ndim)) # temp var..
alpha = 0.05
def f(t, state, z_t, err_hist, F_t):
# TODO fix all stateful references in this body
state = state.reshape(self.ode_shape)
x_t, P_t, eta_t = state[:, :1], state[:, 1:1+self.ndim], state[:, 1+self.ndim:]
z_t = z_t[:, np.newaxis]
K_t = [email protected]@np.linalg.inv(self.R)
self.p_t = P_t
eta_t = np.zeros((self.ndim, self.ndim)) # TODO warmup case?
if t > self.tau:
H_inv = np.linalg.inv(self.H)
P_inv = np.linalg.solve(P_t.T@P_t + self.eps*np.eye(self.ndim), P_t.T)
self.p_inv_t = P_inv
err_t, err_tau = err_hist[-1][:,np.newaxis], err_hist[0][:,np.newaxis]
self.zz_t = (1 - alpha)*self.zz_t + alpha*err_t@err_t.T
self.zz_tau = (1 - alpha)*self.zz_tau + alpha*err_tau@err_tau.T
d_zz = (self.zz_t - self.zz_tau) / self.tau
# if np.linalg.norm(d_zz) >= 1.0:
# d_zz = np.zeros((self.ndim, self.ndim))
self.e_zz_t = d_zz
# E_z = sum(err_hist)[:,np.newaxis] / len(err_hist)
# d_uu = ((err_t - err_tau)/self.tau)@(E_z.T) + E_z@(((err_t - err_tau)/self.tau).T)
eta_t = H_inv@d_zz@H_inv.T@P_inv / 2
self.propose_eta(eta_t)
F_est = F_t - self.eta_t
d_x = F_est@x_t + K_t@(z_t - self.H@x_t)
d_P = F_est@P_t + P_t@F_est.T + self.Q - [email protected]@K_t.T
d_eta = np.zeros((self.ndim, self.ndim))
d_state = np.concatenate((d_x, d_P, d_eta), axis=1)
return d_state.ravel() # Flatten for integrator
def g(t, state):
return np.zeros((self.ode_ndim, self.ode_ndim))
# state
x0 = x0[:, np.newaxis]
# covariance
P0 = np.eye(self.ndim)
# model variation
eta0 = np.zeros((self.ndim, self.ndim))
iv = np.concatenate((x0, P0, eta0), axis=1).ravel() # Flatten for integrator
self.r = Integrator(f, g, self.ode_ndim)
self.r.set_initial_value(iv, 0.)
def __call__(self, z_t: np.ndarray):
''' Observe through filter '''
self.r.set_f_params(z_t, self.err_hist, self.F(self.t))
self.r.integrate(self.t + self.dt)
x_t = np.squeeze(self.r.y.reshape(self.ode_shape)[:, :1])
err_t = z_t - [email protected]
self.err_hist.append(err_t)
if self.t > self.tau:
self.err_hist = self.err_hist[1:]
return x_t.copy(), err_t # x_t variable gets reused somewhere...
@property
def ode_shape(self):
return (self.ndim, 1 + 2*self.ndim) # representational dimension: x_t, P_t, eta_t
@property
def ode_ndim(self):
return self.ode_shape[0] * self.ode_shape[1] # for raveled representation
def propose_eta(self, eta_new: np.ndarray):
""" Metropolis-Hastings on variation """
# alpha = self.f_eta(eta_new) / self.f_eta(self.eta_mu)
# # pdb.set_trace()
# if stats.uniform.rvs() <= alpha:
# self.eta_t = eta_new
# eta_new = np.clip(eta_new, -.2, .2)
self.eta_t = eta_new
def f_eta(self, eta: np.ndarray):
""" density function of variations """
return stats.multivariate_normal.pdf(eta.ravel(), mean=self.eta_mu.ravel(), cov=self.eta_var.ravel())
class LKF(LSProcess):
def __init__(self,
x0: np.ndarray,
F: Callable,
H: np.ndarray,
Q: np.ndarray,
R: np.ndarray,
dt: float,
tau=float('inf'),
eps=1e-3):
self.F = F
self.H = H
self.Q = Q
self.R = R
self.dt = dt
self.tau = tau
self.eps = eps
self.ndim = x0.shape[0]
self.err_hist = []
self.eta_t = np.zeros((self.ndim, self.ndim)) # temp var..
self.e_zz_t = np.zeros((self.ndim, self.ndim)) # temp var..
self.p_inv_t = np.zeros((self.ndim, self.ndim)) # temp var..
self.p_t = np.zeros((self.ndim, self.ndim)) # temp var..
self.n_est = 20
def f(t, state, z_t, err_hist, F_t):
state = state.reshape(self.ode_shape)
x_t, P_t, eta_t = state[:, :1], state[:, 1:3], state[:, 3:]
self.p_t = P_t
z_t = z_t[:, np.newaxis]
eta_t = np.zeros((self.ndim, self.ndim))
if self.history_loaded(): # TODO: warmup case?
H_inv = np.linalg.inv(self.H)
P_inv = np.linalg.solve(P_t.T @ P_t + eps * np.eye(2), P_t.T)
self.p_inv_t = P_inv
tau_n = int(self.tau / self.dt)
d_zz = np.zeros((self.ndim, self.ndim))
for i in range(self.n_est):
err_t, err_tau = err_hist[-1 - i][:, np.newaxis], err_hist[
-tau_n - i][:, np.newaxis]
d_zz += (err_t @ err_t.T - err_tau @ err_tau.T) / self.tau
d_zz /= self.n_est
self.e_zz_t = d_zz
eta_t = H_inv @ d_zz @ H_inv.T @ P_inv / 2
# eta_t = np.clip(eta_t, -10, 10)
self.eta_t = eta_t # TODO fix hack
F_est = F_t - eta_t
K_t = P_t @ self.H @ np.linalg.inv(self.R)
d_x = F_est @ x_t + K_t @ (z_t - self.H @ x_t)
d_P = F_est @ P_t + P_t @ F_est.T + self.Q - K_t @ self.R @ K_t.T
d_eta = np.zeros(
(self.ndim, self.ndim)
) # H_inv@(err_t@err_t.T - err_tau@err_tau.T)@H_inv.T@P_inv / (2*tau)
d_state = np.concatenate((d_x, d_P, d_eta), axis=1)
return d_state.ravel() # Flatten for integrator
def g(t, state):
return np.zeros((self.ode_ndim, self.ode_ndim))
# state
x0 = x0[:, np.newaxis]
# covariance
P0 = np.eye(self.ndim)
# model variation
eta0 = np.zeros((self.ndim, self.ndim))
iv = np.concatenate((x0, P0, eta0),
axis=1).ravel() # Flatten for integrator
self.r = Integrator(f, g, self.ode_ndim)
self.r.set_initial_value(iv, 0.)
def __call__(self, z_t: np.ndarray):
''' Observe through filter '''
self.r.set_f_params(z_t, self.err_hist, self.F(self.t))
self.r.integrate(self.t + self.dt)
x_t = np.squeeze(self.r.y.reshape(self.ode_shape)[:, :1])
err_t = z_t - x_t @ self.H.T
self.err_hist.append(err_t)
if self.history_loaded():
self.err_hist = self.err_hist[1:]
return x_t.copy(), err_t # x_t variable gets reused somewhere...
def history_loaded(self):
return self.t > self.tau + self.n_est * self.dt
@property
def ode_shape(self):
return (self.ndim, 1 + 2 * self.ndim
) # representational dimension: x_t, P_t, eta_t
@property
def ode_ndim(self):
return self.ode_shape[0] * self.ode_shape[
1] # for raveled representation