def compute_z(a,A,b,B,I,w_0):
# Make sure of column vectors:
a, b = to_column(a), to_column(b)
# Compute z:
denominator = np.sqrt(np.linalg.det((mo.mldivide(A,B)+I)))
z = (w_0/denominator) * np.exp(-.5*((a-b).T).dot(mo.mldivide((A+B),(a-b))))
return(z[0][0])
def filtering(self, x0, p0, Y):
# 0. Initiation
N_TIME_STEPS = len(Y)
self.X_DIM = x0.shape[1]
self.Y_DIM = Y.shape[1]
x_ = p_ = x__ = p__ = None
X_ = P_ = X__ = P__ = None
#
# FILTERING LOOP
for k in range(0, N_TIME_STEPS):
print(str(k) + "/" + str(N_TIME_STEPS))
#
# 1. Prediction
if(k == 0):
x__, p__ = x0.T, p0
else:
B = self.model.getB(x_, k)
Q = self.model.Q
x__, p__, _ = self.integrate(x_, p_, self.model.f, k)
p__ = p__ + B.dot(Q).dot(B.T)
p__ = symetrize_cov(p__)
#
# 2. Update
G = self.model.getG(x__, k)
R = self.model.R
mu, S, C = self.integrate(x__, p__, self.model.h, k)
S = S + G.dot(R).dot(G.T)
S = symetrize_cov(S)
#
# 3. Get estimates
eps = Y[k] - to_row(mu)
eps = to_column(eps)
# TODO: Version with K computed
# K = mo.mrdivide(C, S)
# K = C.dot(np.linalg.inv(S))
# x_ = x__ + K.dot(eps)
# p_ = p__ - K.dot(S).dot(K.T)
# p_ = symetrize_cov(p_)
# TODO: Version without direct computation of K
x_ = x__ + C.dot(mo.mldivide(S, eps))
p_ = p__ - C.dot(mo.mrdivide(C,S).T)
p_ = symetrize_cov(p_)
#
# 4. Save results
X__ = x__.T if k==0 else np.vstack([X__, x__.T])
P__ = np.array([p__]) if k==0 else np.vstack([P__, [p__]])
X_ = x_.T if k==0 else np.vstack([X_, x_.T])
P_ = np.array([p_]) if k==0 else np.vstack([P_, [p_]])
return(X_, X__, P_, P__)
def compute_z(a,A,b,B,I,w_0):
# Make sure of column vectors:
a, b = to_column(a), to_column(b)
# Compute z:
denominator = np.sqrt(np.linalg.det((mo.mldivide(A,B)+I)))
z = (w_0/denominator) * np.exp(-.5*((a-b).T).dot(mo.mldivide((A+B),(a-b))))
return(z[0][0])
# - compute z
z = [compute_z(a, A, x0, p0, I, w_0) for a in X]
z = to_column(np.array(z))
K = gp.kern.K(X)
K = (K.T + K)/2.0
mu = (z.T).dot( mo.mldivide(K, Y) )
W = mo.mrdivide(z.T, K).squeeze().tolist()
_Sigma = None
_CC = None
for i in range(0,len(z)):
Y_squared_i = ( to_column(Y[i]-mu) ).dot( to_row(Y[i]-mu) )
_Sigma = W[i] * Y_squared_i if i == 0 else _Sigma + W[i] * Y_squared_i
XY_squared_i = ( to_column((X[i]-x0)) ).dot( to_row(Y[i]-mu) )
_CC = W[i] * XY_squared_i if i == 0 else _CC + W[i] * XY_squared_i
_Sigma = _Sigma + cov_Q
Sigma = (_Sigma + _Sigma.T)/2.0
CC = _CC
def integrate(self, m, P, fun, *args):
''' Input:
m - column vector
P - matrix
Output:
x - column vector
Variables:
X, Y - matrix of row vectors
z - column vector
m, x, mu - row vector
'''
# Initial sample and fitted GP:
m = to_row(m)
X = m
Y = np.apply_along_axis(fun, 1, X, *args)
gp = self.gp_fit(X,Y)
# Optimization constraints:
N_SIGMA = 3
P_diag = np.sqrt(np.diag(P))
lower_const = (m - N_SIGMA*P_diag)[0].tolist()
upper_const = (m + N_SIGMA*P_diag)[0].tolist()
# Perform sampling
for i in range(0, self.N_SAMPLES):
# Set extra params to pass to optimizer
user_data = {"gp":gp, "m":m, "P":P, "grid_search": False}
if (X.shape[1] == 1):
''' GRID SEARCH: when we are in 1 dimension '''
user_data['grid_search'] = True
X_grid = np.linspace(lower_const[0], upper_const[0], self.opt_par["GRID_SIZE"])
X_grid = to_column(X_grid)
objective = self.optimization_objective(X_grid, user_data)
max_ind = objective.index(max(objective))
x_star = np.array([X_grid[max_ind]])
else:
''' OPTIMIZATION: for higher dimensions '''
x_star, _, _ = solve(self.optimization_objective, lower_const, upper_const,
user_data=user_data, algmethod = 1,
maxT = self.opt_par["MAX_T"],
maxf = self.opt_par["MAX_F"])
x_star = to_row(x_star)
X = np.vstack((X, x_star))
Y = np.apply_along_axis(fun, 1, X, *args)
gp = self.gp_fit(X, Y)
# Reoptimize GP:
# TODO: Remove unique rows:
if (len(unique_rows(X)) != len(X)):
print("Removing duplicated rows")
X = unique_rows(X)
Y = np.apply_along_axis(fun, 1, X, *args)
gp = self.gp_fit(X, Y)
# Compute integral
# Fitted GP parameters
w_0 = gp.rbf.variance.tolist()[0]
w_d = np.power(gp.rbf.lengthscale.tolist(), 2)
# Prior parameters
A = np.diag(w_d)
I = np.eye(self.X_DIM)
# Compute weigths
z = [self.compute_z(x, A, m, P, I, w_0) for x in X]
z = to_column(np.array(z))
K = gp.kern.K(X); K = (K.T + K)/2.0
W = (mo.mrdivide(z.T, K).squeeze()).tolist()
# Compute mean, covariance and cross-cov
mu_ = (z.T).dot( mo.mldivide(K, Y) )
mu_ = to_row(mu_)
# Initiale Sigma and CC
Sigma_ = CC_ = None
# TODO: Sigma computation as closed form
# Seems to cause problems!
# Sigma_ = w_0/np.sqrt(np.linalg.det( 2*mo.mldivide(A,P) + I) ) - (z.T).dot(mo.mldivide(K,z))
# Compute cov matrix and cross cov matrix
for i in range(0,len(W)):
# TODO: Sigma computation as sumation:
# Seems to work better for multidimensional problems and doesn't
# cause problems (might be slower though):
YY_i = ( to_column(Y[i]-mu_) ).dot( to_row(Y[i]-mu_) )
Sigma_ = W[i] * YY_i if i == 0 else Sigma_ + W[i] * YY_i
XY_i = ( to_column(X[i]-m) ).dot( to_row(Y[i]-mu_) )
CC_ = W[i] * XY_i if i == 0 else CC_ + W[i] * XY_i
mu_ = to_column(mu_)
Sigma_ = symetrize_cov(Sigma_)
# Return results
return(mu_, Sigma_, CC_)