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 PersonEstimate(object):
def __init__(self, msg):
self.pos = msg
self.reliability = 0.1
self.k = Kalman()
def update(self, msg):
last = self.pos
self.pos = msg
self.reliability = max(self.reliability, msg.reliability)
ivel = subtract(self.pos.pos, last.pos)
time = (self.pos.header.stamp - last.header.stamp).to_sec()
if time == 0:
return
scale(ivel, 1.0 / time)
self.k.update([ivel.x, ivel.y, ivel.z])
def age(self):
return self.pos.header.stamp
def id(self):
return self.pos.object_id
def velocity(self):
k = self.k.values()
if k == None:
return Vector3()
v = Vector3(k[0], k[1], k[2])
return v
def publish_markers(self, pub):
gen.scale = [.1, .3, 0]
gen.color = [1, 1, 1, 1]
vel = self.velocity()
#scale(vel, 15)
m = gen.marker(points=[self.pos.pos, add(self.pos.pos, vel)])
m.header = self.pos.header
pub.publish(m)
def get_person(self):
p = Person()
p.name = self.id()
p.position = self.pos.pos
p.velocity = self.velocity()
p.reliability = self.reliability
p.tags.append(self.pos.name)
return self.pos.header.frame_id, p
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 )
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 __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
class PersonEstimate:
def __init__(self, msg):
self.pos = msg
self.reliability = 0.1
self.k = Kalman()
def update(self, msg):
last = self.pos
self.pos = msg
self.reliability = max(self.reliability, msg.reliability)
ivel = subtract(self.pos.pos, last.pos)
time = (self.pos.header.stamp - last.header.stamp).to_sec()
scale(ivel, 1.0/time)
self.k.update([ivel.x, ivel.y, ivel.z])
def age(self):
return self.pos.header.stamp
def id(self):
return self.pos.object_id
def velocity(self):
k = self.k.values()
if k==None:
return Vector3()
v = Vector3(k[0],k[1],k[2])
return v
def publish_markers(self, pub):
gen.scale = [.1, .3, 0]
gen.color = [1,1,1,1]
vel = self.velocity()
#scale(vel, 15)
m = gen.marker(points=[self.pos.pos, add(self.pos.pos, vel)])
m.header = self.pos.header
pub.publish(m)
def get_person(self):
p = Person()
p.name = self.id()
p.position = self.pos.pos
p.velocity = self.velocity()
p.reliability = self.reliability
return self.pos.header.frame_id, p
class BoatEstimate(object):
def __init__(self, msg, stamp):
self.det_boat = msg
self.stamp = stamp
self.k = Kalman()
def update(self, msg, stamp):
last = self.det_boat
last_stamp = self.stamp
self.det_boat = msg
self.stamp = stamp
ivel = subtract(self.det_boat.pose.position, last.pose.position)
if ((self.stamp - last_stamp).to_sec() != 0.0):
time = (self.stamp - last_stamp).to_sec()
scale(ivel, 1.0 / time)
self.k.update([ivel.x, ivel.y, ivel.z])
def id(self):
return self.det_boat.id
def velocity(self):
k = self.k.values()
if k == None:
return Vector3()
v = Vector3(k[0], k[1], k[2])
return v
def get_boat(self):
b = Boat()
b.id = self.id()
b.lidar_index = self.det_boat.lidar_index
b.pose = self.det_boat.pose
b.velocity = self.velocity()
b.size = self.det_boat.size
return b
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()
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()
def __init__(self, msg):
self.pos = msg
self.reliability = 0.1
self.k = Kalman()
