class KGD: def __init__(self, x0, dx0, sigma_Q=0.01, sigma_R=2.0): #setup filter: self._N=len(x0) N=self._N z_0=np.hstack([x0, dx0]).reshape(2*N,1) Q=sigma_Q*np.eye(2*N)#dynamics noise R=sigma_R*np.eye(N)#observation noise C=np.hstack([np.zeros((N,N)), np.eye(N)]) self.kf=Kalman(init_state=z_0, init_cov=np.eye(2*N)*0.01, C_t=C, Q_t=Q, R_t=R ) def step(self, x, dx, alpha=0.01, callback=None): N=self._N A_t=np.block( [[np.eye(N) , -alpha*np.eye(N)], # [np.zeros((N,N)) , 1-alpha*ddx]] [np.zeros((N,N)) , np.eye(N)]] ) z_hat = self.kf.filter(dx.T.reshape(N,1), A_t=A_t) x_hat, dx_hat=z_hat[:N], z_hat[N:] if callback: callback(x, t, dx_hat.reshape(x.shape)) # x+=-alpha*dx_hat.flatten() return x - alpha*dx_hat.flatten()
def _rmsprop_filtered(g,
x,
callback=None,
num_iters=200,
alpha=lambda t: 0.1,
gamma=lambda t: 0.9,
eps=1e-8,
sigma_Q=0.01,
sigma_R=2.0):
"""
Implements the Kalman RMSProp algorithm from
https://arxiv.org/pdf/1810.12273.pdf.
"""
# Initialize the Kalman filter
N = len(x)
dx = g(x)
r = np.ones(len(x)) #should be ones.
z_0 = np.hstack([r, x, dx]).reshape(3 * N, 1)
Q = sigma_Q * np.eye(3 * N) #dynamics noise
R = sigma_R * np.eye(N) #observation noise
C = np.hstack([np.zeros((N, 2 * N)), np.eye(N)])
kf = Kalman(init_state=z_0,
init_cov=np.eye(3 * N) * 0.01,
C_t=C,
Q_t=Q,
R_t=R)
for t in range(num_iters):
# evaluate the gradient
dx = g(x)
#construct transition matrix
beta_t = alpha(t) / np.sqrt((gamma(t) * r + (1 - gamma(t)) *
(dx**2)) + eps)
A_t = np.block([[
np.eye(N) * gamma(t),
np.zeros((N, N)), (1 - gamma(t)) * np.diag(dx)
], [np.zeros((N, N)), np.eye(N), -beta_t * np.eye(N)],
[np.zeros((N, N)),
np.zeros((N, N)),
np.eye(N)]])
#increment the kalman filter
z_hat = kf.filter(dx.T.reshape(N, 1), A_t=A_t)
r_hat, x_hat, dx_hat = z_hat[:N], z_hat[N:2 * N], z_hat[2 * N:]
#perform the Kalman RMSProp update
r = gamma(t) * r + (1 - gamma(t)) * dx_hat.flatten()**2
x += -alpha(t) / (np.sqrt(r) + eps) * dx_hat.flatten()
if callback: callback(x, t, dx_hat.reshape(x.shape))
return x
def _momgd_filtered(g,
x,
callback=None,
num_iters=200,
alpha=lambda t: 0.1,
mu=lambda t: 0.9,
sigma_Q=0.01,
sigma_R=2.0):
"""
Implements the Kalman Gradient Descent + momentum algorithm from
https://arxiv.org/pdf/1810.12273.pdf.
"""
# Initialize the Kalman filter
N = len(x)
dx = g(x)
v = np.zeros(len(x))
z_0 = np.hstack([v, x, dx]).reshape(3 * N, 1)
Q = sigma_Q * np.eye(3 * N) #dynamics noise
R = sigma_R * np.eye(N) #observation noise
C = np.hstack([np.zeros((N, 2 * N)), np.eye(N)])
kf = Kalman(init_state=z_0,
init_cov=np.eye(3 * N) * 0.01,
C_t=C,
Q_t=Q,
R_t=R)
for t in range(num_iters):
# evaluate the gradient
dx = g(x)
#construct transition matrix
A_t = np.block(
[[np.eye(N) * mu(t),
np.zeros((N, N)), (1 - mu(t)) * np.eye(N)],
[
-alpha(t) * np.eye(N),
np.eye(N), -alpha(t) * (1 - mu(t)) * np.eye(N)
], [np.zeros((N, N)),
np.zeros((N, N)),
np.eye(N)]])
#increment the kalman filter
z_hat = kf.filter(dx.T.reshape(N, 1), A_t=A_t)
v_hat, x_hat, dx_hat = z_hat[:N], z_hat[N:2 * N], z_hat[2 * N:]
if callback: callback(x, t, dx_hat.reshape(x.shape))
#perform the KGD + momentum update
v = mu(t) * v - (1 - mu(t)) * dx_hat.flatten(
) #don't flatte, use .reshpape(x.shape)
x += alpha(t) * v
return x
class KRMSProp:
"""
A stateful version of Kalman RMSProp from https://arxiv.org/pdf/1810.12273.pdf
which is used in distributed Kalman RMSProp.
"""
def __init__(self, x0, dx0, sigma_Q=0.01, sigma_R=2.0):
self._N = len(x0)
N = self._N
r = np.ones(len(x0)) #should be ones.
z_0 = np.hstack([r, x0, dx0]).reshape(3 * N, 1)
Q = sigma_Q * np.eye(3 * N) #dynamics noise
R = sigma_R * np.eye(N) #observation noise
C = np.hstack([np.zeros((N, 2 * N)), np.eye(N)])
self.kf = Kalman(init_state=z_0,
init_cov=np.eye(3 * N) * 0.01,
C_t=C,
Q_t=Q,
R_t=R)
self._r = r
def step(self, x, dx, callback=None, alpha=0.01, gamma=0.9, eps=1e-8):
r = self._r
N = self._N
beta_t = alpha / np.sqrt((gamma * r + (1 - gamma) * (dx**2) + eps))
A_t = np.block(
[[np.eye(N) * gamma,
np.zeros((N, N)), (1 - gamma) * np.diag(dx)],
[np.zeros((N, N)),
np.eye(N), -beta_t * np.eye(N)],
[np.zeros((N, N)), np.zeros((N, N)),
np.eye(N)]])
z_hat = self.kf.filter(dx.T.reshape(N, 1), A_t=A_t)
r_hat, x_hat, dx_hat = z_hat[:N], z_hat[N:2 * N], z_hat[2 * N:]
r = gamma * r + (1 - gamma) * dx_hat.flatten()**2
self._r = r
return x - alpha / (np.sqrt(r) + eps) * dx_hat.flatten()
# Initialize kalman Filter
kalman = Kalman(x0, P0, F, H, Q, R)
# initialize measurement
measure = Measurement(0, 80, dt)
# Temporary state values and measurement values
measured_speeds = []
positions = []
speeds = []
# Perform measurement for the time period
for index, k in enumerate(time_peride):
Z = measure.measure() # train speed measurement
X = kalman.filter(Z) # train position prediction
measured_speeds.append(Z[0][0])
positions.append(X[0][0])
speeds.append(X[1][0])
# Error covariance for position
kalman_P_list = [P[1][1] for P in kalman.P_list]
# Kalman gain for position
kalman_K_list = [K[1][0] for K in kalman.K_list]
# Plot results
plt.plot(time_peride, measured_speeds, color='gray', linestyle=':')
plt.plot(time_peride, speeds)
plt.xlabel('Measurement samples [sample/s]')
