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
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 __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 __init__(self, msg):
self.pos = msg
self.reliability = 0.1
self.k = Kalman()
H = np.array([[0, 1]])
# Process noise
Q = np.array([[1, 0], [0, 3]])
# Measurement noise
R = np.array([[10]])
# Initial states x0=0m und v=20m/s
x0 = np.array([[0, 20]]).transpose()
# Initial error covariance matrix
P0 = np.array(5 * np.eye(2))
# 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
def __init__(self, msg, stamp):
self.det_boat = msg
self.stamp = stamp
self.k = Kalman()
