def get_control(self, odom):
R = np.array([[np.cos(self.th), -np.sin(self.th)],[np.sin(self.th), np.cos(self.th)]])
p_b = - np.matmul(R.T, self.init_org) + np.matmul(R.T, odom[:2])
p_b_tp1 = [p_b[0] + self.x_interval, self.b * np.sin(self.a * (p_b[0] + self.x_interval))]
p_g_tp1 = self.init_org + np.matmul(R, p_b_tp1)
th_b_tp1 = np.arctan(self.b * self.a * np.cos( p_b_tp1[0] * self.a ))
th_g_tp1 = util.wrap_around(th_b_tp1 + self.th)
ang_vel = util.wrap_around(th_g_tp1 - odom[2]) / self.sampling_period
lin_vel = np.sqrt(np.sum((p_g_tp1 - odom[:2])**2)) / self.sampling_period
return np.clip(np.array([lin_vel, ang_vel]), self.limit[0], self.limit[1])
def residual_z_(z, zp):
"""
z : observation, [r, alpha]
zp : predicted observation
"""
r_z = z - zp
r_z[1] = util.wrap_around(r_z[1])
return r_z
def get_control(self, odom):
th = self.d_th/180.0*np.pi
r = np.sqrt((odom[0] - self.maporigin[0])**2 + (odom[1] - self.maporigin[1])**2)
alpha = np.arctan2(odom[1] - self.maporigin[0], odom[0] - self.maporigin[1])
x = r*np.cos(alpha+th) + self.maporigin[0] + np.random.random() - 0.5
y = r*np.sin(alpha+th) + self.maporigin[1] + np.random.random() - 0.5
v = np.sqrt((x - odom[0])**2 + (y - odom[1])**2)/self.sampling_period
w = util.wrap_around(np.arctan2(y - odom[1], x - odom[0]) - odom[2])/self.sampling_period
return np.clip(np.array([v,w]), self.limit[0], self.limit[1])
def residual_x_(x, xp):
"""
x : state, [x, y, theta]
xp : predicted state
"""
if dim == 3 or dim == 5:
r_x = x - xp
r_x[2] = util.wrap_around(r_x[2])
return r_x
else:
return None
def SE2Dynamics(x, dt, u):
assert(len(x)==3)
tw = dt * u[1]
# Update the agent state
if abs(tw) < 0.001:
diff = np.array([dt*u[0]*np.cos(x[2]+tw/2),
dt*u[0]*np.sin(x[2]+tw/2),
tw])
else:
diff = np.array([u[0]/u[1]*(np.sin(x[2]+tw) - np.sin(x[2])),
u[0]/u[1]*(np.cos(x[2]) - np.cos(x[2]+tw)),
tw])
new_x = x + diff
new_x[2] = util.wrap_around(new_x[2])
return new_x
def update(self, observed, z_t, x_t):
# Kalman Filter Prediction and Update
# Prediction
state_predicted = np.matmul(self.A, self.state)
cov_predicted = np.matmul(np.matmul(self.A, self.cov), self.A.T)+ self.W
# Update
if observed:
r_pred, alpha_pred, diff_pred = util.relative_measure(state_predicted, x_t)
if self.dim == 2:
Hmat = np.array([[diff_pred[0],diff_pred[1]],
[-diff_pred[1]/r_pred, diff_pred[0]/r_pred]])/r_pred
elif self.dim == 4:
Hmat = np.array([[diff_pred[0], diff_pred[1], 0.0, 0.0],
[-diff_pred[1]/r_pred, diff_pred[0]/r_pred, 0.0, 0.0]])/r_pred
else:
raise ValueError('target dimension for KF must be either 2 or 4')
innov = z_t - np.array([r_pred, alpha_pred])
innov[1] = util.wrap_around(innov[1])
R = np.matmul(np.matmul(Hmat, cov_predicted), Hmat.T) \
+ self.obs_noise_func((r_pred, alpha_pred))
K = np.matmul(np.matmul(cov_predicted, Hmat.T), LA.inv(R))
C = np.eye(self.dim) - np.matmul(K, Hmat)
cov_new = np.matmul(C, cov_predicted)
state_new = state_predicted + np.matmul(K, innov)
else:
cov_new = cov_predicted
state_new = state_predicted
if LA.det(cov_new) < 1e6:
self.cov = cov_new
if not(self.collision_func(state_new[:2])):
self.state = np.clip(state_new, self.limit[0], self.limit[1])
def collision(self, odom):
self.init_org = odom[:2]
self.th = util.wrap_around(odom[2] + np.random.random()*np.pi/2 - np.pi/4)