def ablt_obsv(self, idx, obs_value, landmark):
ii = 2 * idx
H = sim_env.rot_mtx(self.theta[idx]).getT() * matrix(
[[-1, 0], [0, -1]], dtype=float)
local_s = self.s[ii:ii + 2]
local_sigma = self.sigma[ii:ii + 2, ii:ii + 2]
dis = obs_value[0]
phi = obs_value[1]
hat_z = sim_env.rot_mtx(
self.theta[idx]).getT() * (landmark.position + H * local_s)
z = matrix([dis * cos(phi), dis * sin(phi)]).getT()
sigma_z = sim_env.rot_mtx(phi) * matrix([[
sim_env.var_dis, 0
], [0, dis * dis * sim_env.var_phi]]) * sim_env.rot_mtx(phi).getT()
sigma_invention = H * local_sigma * H.getT() + sigma_z
kalman_gain = local_sigma * H.getT() * sigma_invention.getI()
self.s[ii:ii + 2] = local_s + kalman_gain * (z - hat_z)
self.sigma[ii:ii + 2,
ii:ii + 2] = local_sigma - kalman_gain * H * local_sigma
def rela_obsv_update(self, idx, obs_idx, obs_value): i = 2*idx j = 2*obs_idx H_ij = matrix([[0,0,0,0,0,0,0,0,0,0], [0,0,0,0,0,0,0,0,0,0]], dtype=float) H_ij[0, i] = -1 H_ij[1, i+1] = -1 H_ij[0, j] = 1 H_ij[1, j+1] = 1 H = sim_env.rot_mtx(self.theta[idx]).getT()*H_ij dis = obs_value[0] phi = obs_value[1] hat_z = H * self.s z = matrix([dis*cos(phi), dis*sin(phi)]).getT() sigma_z = sim_env.rot_mtx(phi) * matrix([[sim_env.var_dis, 0],[0, dis*dis*sim_env.var_phi]]) * sim_env.rot_mtx(phi).getT() sigma_invention = H * self.sigma * H.getT() + sigma_z kalman_gain = self.sigma*H.getT()*sigma_invention.getI() self.s = self.s + kalman_gain*(z - hat_z) self.sigma = self.sigma - kalman_gain*H*self.sigma
def ablt_obsv_update(self, idx, obs_value, landmark):
ii = 2 * idx
total_sigma = self.sigma_i + self.sigma_d
H_i = matrix(
[[0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]],
dtype=float)
H_i[0, ii] = -1
H_i[1, ii + 1] = -1
H = sim_env.rot_mtx(self.theta[idx]).getT() * H_i
dis = obs_value[0]
phi = obs_value[1]
hat_z = sim_env.rot_mtx(
self.theta[idx]).getT() * (landmark.position + H_i * self.s)
z = matrix([dis * cos(phi), dis * sin(phi)]).getT()
sigma_z = sim_env.rot_mtx(phi) * matrix([[
sim_env.var_dis, 0
], [0, dis * dis * sim_env.var_phi]]) * sim_env.rot_mtx(phi).getT()
sigma_invention = H * total_sigma * H.getT() + sigma_z
kalman_gain = total_sigma * H.getT() * sigma_invention.getI()
self.s = self.s + kalman_gain * (z - hat_z)
total_sigma = total_sigma - kalman_gain * H * total_sigma
self.sigma_i = (i_mtx_10.copy() - kalman_gain * H) * self.sigma_i * (
i_mtx_10.copy() - kalman_gain *
H).getT() + kalman_gain * sigma_z * kalman_gain.getT()
self.sigma_d = total_sigma - self.sigma_i
def rela_obsv_update(self, idx, obs_idx, obs_value):
ii = 2 * idx
jj = 2 * obs_idx
sigma_i = self.sigma[ii:ii + 2, ii:ii + 2]
sigma_j = self.sigma[jj:jj + 2, jj:jj + 2]
dis = obs_value[0]
phi = obs_value[1]
z = matrix([[dis * cos(phi)], [dis * sin(phi)]])
hat_j = self.s[ii:ii + 2] + sim_env.rot_mtx(self.theta[idx]) * z
sigma_z = sim_env.rot_mtx(phi) * matrix([[
sim_env.var_dis, 0
], [0, dis * dis * sim_env.var_phi]]) * sim_env.rot_mtx(phi).getT()
sigma_hat_j = sigma_i + sim_env.rot_mtx(
self.theta[idx]) * sigma_z * sim_env.rot_mtx(
self.theta[idx]).getT()
ci_coeff = 0.83
sigma_j_next_inv = ci_coeff * sigma_j.getI() + (
1 - ci_coeff) * sigma_hat_j.getI()
self.s[jj:jj + 2] = sigma_j_next_inv.getI() * (
ci_coeff * sigma_j.getI() * self.s[jj:jj + 2] +
(1 - ci_coeff) * sigma_hat_j.getI() * hat_j)
self.sigma[jj:jj + 2, jj:jj + 2] = sigma_j_next_inv.getI()
def rela_obsv_update(self, idx, obs_idx, obs_value):
ii = 2 * idx
jj = 2 * obs_idx
H_ij = matrix(
[[0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]],
dtype=float)
H_ij[0, ii] = -1
H_ij[1, ii + 1] = -1
H_ij[0, jj] = 1
H_ij[1, jj + 1] = 1
H = sim_env.rot_mtx(self.theta[idx]).getT() * H_ij
dis = obs_value[0]
phi = obs_value[1]
hat_z = H * self.s
z = matrix([dis * cos(phi), dis * sin(phi)]).getT()
reduced_sigma = sim_env.z_mtx_10.copy()
reduced_sigma[ii:ii + 2, ii:ii + 2] = self.sigma[ii:ii + 2, ii:ii + 2]
reduced_sigma[jj:jj + 2, jj:jj + 2] = self.sigma[jj:jj + 2, jj:jj + 2]
reduced_sigma[ii:ii + 2, jj:jj + 2] = self.sigma[ii:ii + 2, jj:jj + 2]
reduced_sigma[jj:jj + 2, ii:ii + 2] = self.sigma[jj:jj + 2, ii:ii + 2]
multi_i = self.sigma[ii:ii + 2, ii:ii + 2].getI()
multi_j = self.sigma[jj:jj + 2, jj:jj + 2].getI()
sigma_z = sim_env.rot_mtx(phi) * matrix([[
sim_env.var_dis, 0
], [0, dis * dis * sim_env.var_phi]]) * sim_env.rot_mtx(phi).getT()
sigma_invention = H * reduced_sigma * H.getT() + sigma_z
kalman_gain = reduced_sigma * H.getT() * sigma_invention.getI()
self.s[ii:ii +
2] = self.s[ii:ii + 2] + kalman_gain[ii:ii + 2] * (z - hat_z)
self.s[jj:jj +
2] = self.s[jj:jj + 2] + kalman_gain[jj:jj + 2] * (z - hat_z)
self.sigma = self.sigma - kalman_gain * H * reduced_sigma
multi_i = self.sigma[ii:ii + 2, ii:ii + 2] * multi_i
multi_j = self.sigma[jj:jj + 2, jj:jj + 2] * multi_j
for k in range(sim_env.N):
if (k != idx):
if (k != obs_idx):
kk = 2 * k
self.sigma[ii:ii + 2, kk:kk +
2] = multi_i * self.sigma[ii:ii + 2, kk:kk + 2]
self.sigma[jj:jj + 2, kk:kk +
2] = multi_j * self.sigma[jj:jj + 2, kk:kk + 2]
self.sigma[kk:kk + 2,
ii:ii + 2] = self.sigma[kk:kk + 2, ii:ii +
2] * multi_i.getT()
self.sigma[kk:kk + 2,
jj:jj + 2] = self.sigma[kk:kk + 2, jj:jj +
2] * multi_j.getT()
def rela_obsv(self, obs_idx, obs_value):
i = 2 * self.index
j = 2 * obs_idx
H_ij = matrix(
[[0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]],
dtype=float)
H_ij[0, i] = -1
H_ij[1, i + 1] = -1
H_ij[0, j] = 1
H_ij[1, j + 1] = 1
H = sim_env.rot_mtx(self.theta).getT() * H_ij
dis = obs_value[0]
phi = obs_value[1]
hat_z = H * self.s
z = matrix([dis * cos(phi), dis * sin(phi)]).getT()
# algorithmic update
sigma_z = sim_env.rot_mtx(phi) * matrix([[
sim_env.var_dis, 0
], [0, dis * dis * sim_env.var_phi]]) * sim_env.rot_mtx(phi).getT()
sigma_invention = H * self.sigma * H.getT() + sigma_z
kalman_gain = self.sigma * H.getT() * sigma_invention.getI()
self.s = self.s + kalman_gain * (z - hat_z)
self.sigma = self.sigma - kalman_gain * H * self.sigma
# analytic update
sigma_th_z = max(sim_env.var_dis, sim_env.d_max * sim_env.d_max *
sim_env.var_phi) * sim_env.i_mtx_2.copy()
self.th_sigma = (self.th_sigma.getI() +
H_ij.getT() * sigma_th_z.getI() * H_ij).getI()
def ablt_obsv(self, obs_value, landmark):
i = 2 * self.index
H_i = matrix(
[[0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]],
dtype=float)
H_i[0, i] = -1
H_i[1, i + 1] = -1
H = sim_env.rot_mtx(self.theta).getT() * H_i
dis = obs_value[0]
phi = obs_value[1]
hat_z = sim_env.rot_mtx(
self.theta).getT() * (landmark.position + H_i * self.s)
z = matrix([dis * cos(phi), dis * sin(phi)]).getT()
# algorithmic update
sigma_z = sim_env.rot_mtx(phi) * matrix([[
sim_env.var_dis, 0
], [0, dis * dis * sim_env.var_phi]]) * sim_env.rot_mtx(phi).getT()
sigma_invention = H * self.sigma * H.getT() + sigma_z
kalman_gain = self.sigma * H.getT() * sigma_invention.getI()
self.s = self.s + kalman_gain * (z - hat_z)
self.sigma = self.sigma - kalman_gain * H * self.sigma
# analytical update
sigma_th_z = max(sim_env.var_dis, sim_env.d_max * sim_env.d_max *
sim_env.var_phi) * sim_env.i_mtx_2.copy()
self.th_sigma = (self.th_sigma.getI() +
H_i.getT() * sigma_th_z.getI() * H_i).getI()
def rela_obsv(self, obs_idx, obs_value): ii = 2*self.index jj = 2*obs_idx H_ij = matrix([[0,0,0,0,0,0,0,0,0,0], [0,0,0,0,0,0,0,0,0,0]], dtype=float) H_ij[0, ii] = -1 H_ij[1, ii+1] = -1 H_ij[0, jj] = 1 H_ij[1, jj+1] = 1 H = sim_env.rot_mtx(self.theta).getT()*H_ij dis = obs_value[0] phi = obs_value[1] hat_z = H * self.s z = matrix([dis*cos(phi), dis*sin(phi)]).getT() sigma_z = sim_env.rot_mtx(phi) * matrix([[sim_env.var_dis, 0],[0, dis*dis*sim_env.var_phi]]) * sim_env.rot_mtx(phi).getT() e = 0.83# (iii+1)*0.01 p_1 = (1/e) * self.sigma_d + self.sigma_i p_2 = (1/(1-e)) * H * (self.sigma_i + self.sigma_d) * H.getT() + sigma_z sigma_invention = H * p_1 * H.getT() + p_2 kalman_gain = p_1 *H.getT()*sigma_invention.getI() self.s = self.s + kalman_gain*(z - hat_z) total_sigma = (sim_env.i_mtx_10.copy()-kalman_gain*H) * p_1 self.sigma_i = (sim_env.i_mtx_10.copy()-kalman_gain*H) * self.sigma_i * (sim_env.i_mtx_10.copy()-kalman_gain*H).getT() + kalman_gain * sigma_z * kalman_gain.getT() self.sigma_d = total_sigma - self.sigma_i
def rela_obsv_update(self, obs_idx, obs_value):
ii = 2 * self.index
jj = 2 * obs_idx
total_sigma = self.sigma_i + self.sigma_d
H_ij = matrix(
[[0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]],
dtype=float)
H_ij[0, ii] = -1
H_ij[1, ii + 1] = -1
H_ij[0, jj] = 1
H_ij[1, jj + 1] = 1
H = sim_env.rot_mtx(self.theta).getT() * H_ij
dis = obs_value[0]
phi = obs_value[1]
hat_z = H * self.s
z = matrix([dis * cos(phi), dis * sin(phi)]).getT()
sigma_z = sim_env.rot_mtx(phi) * matrix([[
sim_env.var_dis, 0
], [0, (dis**2) * sim_env.var_phi]]) * sim_env.rot_mtx(phi).getT()
sigma_invention = H * total_sigma * H.getT() + sigma_z
kalman_gain = total_sigma * H.getT() * sigma_invention.getI()
self.s = self.s + kalman_gain * (z - hat_z)
total_sigma = total_sigma - kalman_gain * H * total_sigma
self.sigma_i = (i_mtx_10.copy() - kalman_gain * H) * self.sigma_i * (
i_mtx_10.copy() - kalman_gain *
H).getT() + kalman_gain * sigma_z * kalman_gain.getT()
self.sigma_d = total_sigma - self.sigma_i
def prop_update(self):
dt = sim_env.dt
# select valid motion input
[v, a_v] = [
random.uniform(-sim_env.max_v, sim_env.max_v),
random.uniform(-sim_env.max_oemga, sim_env.max_oemga)
]
v_star = v + random.normal(0, sqrt(sim_env.var_u_v))
pre_update_position = [
self.position[0] + cos(self.theta) * v_star * dt,
self.position[1] + sin(self.theta) * v_star * dt
]
while (not sim_env.inRange(pre_update_position, sim_env.origin)):
[v, a_v] = [
random.uniform(-sim_env.max_v, sim_env.max_v),
random.uniform(-sim_env.max_oemga, sim_env.max_oemga)
]
v_star = v + random.normal(0, sqrt(sim_env.var_u_v))
pre_update_position = [
self.position[0] + cos(self.theta) * v_star * dt,
self.position[1] + sin(self.theta) * v_star * dt
]
# real position update
self.position[0] += cos(self.theta) * v_star * dt
self.position[1] += sin(self.theta) * v_star * dt
self.theta += a_v * dt
# estimation update
ii = 2 * self.index
self.s[ii, 0] = self.s[ii, 0] + cos(self.theta) * v * dt
self.s[ii + 1, 0] = self.s[ii + 1, 0] + sin(self.theta) * v * dt
# covariance update
for j in range(RobotLocalCI.N):
jj = 2 * j
if j == self.index:
self.sigma[jj:jj + 2, jj:jj + 2] += dt * dt * sim_env.rot_mtx(
self.theta) * matrix([
[sim_env.var_u_v, 0], [0, sim_env.var_u_theta]
]) * sim_env.rot_mtx(self.theta).getT()
self.th_sigma[jj:jj + 2,
jj:jj + 2] += dt * dt * sim_env.var_u_v * matrix(
[[1, 0], [0, 1]])
else:
self.sigma[
jj:jj + 2, jj:jj +
2] += dt * dt * sim_env.var_v * sim_env.i_mtx_2.copy()
self.th_sigma[
jj:jj + 2, jj:jj +
2] += dt * dt * sim_env.var_v * sim_env.i_mtx_2.copy()
def rela_obsv(self, idx, obs_idx, obs_value):
i = 2 * idx
j = 2 * obs_idx
H_ij = matrix(
[[0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]],
dtype=float)
H_ij[0, i] = -1
H_ij[1, i + 1] = -1
H_ij[0, j] = 1
H_ij[1, j + 1] = 1
H = sim_env.rot_mtx(self.theta[idx]).getT() * H_ij
dis = obs_value[0]
phi = obs_value[1]
z = matrix([[dis * cos(phi)], [dis * sin(phi)]])
hat_j = z + self.s[i:i + 2]
sigma_i = self.sigma[i:i + 2, i:i + 2]
sigma_j = self.sigma[j:j + 2, j:j + 2]
e = 0.83
sigma_j_next_inv = e * sigma_j.getI() + (1 - e) * sigma_i.getI()
self.s[j:j + 2] = sigma_j_next_inv.getI() * (
e * sigma_j.getI() * self.s[j:j + 2] +
(1 - e) * sigma_i.getI() * hat_j)
self.sigma[j:j + 2, j:j + 2] = sigma_j_next_inv.getI()
def rela_obsv_update(self, idx, obs_idx, obs_value):
ii = 2 * idx
jj = 2 * obs_idx
ci_coeff = 0.83
sigma_j = (1 / ci_coeff) * self.sigma_d[jj:jj + 2, jj:jj +
2] + self.sigma_i[jj:jj + 2,
jj:jj + 2]
sigma_i = (1 /
(1 - ci_coeff)) * self.sigma_d[ii:ii + 2, ii:ii +
2] + self.sigma_i[ii:ii + 2,
ii:ii + 2]
dis = obs_value[0]
phi = obs_value[1]
z = matrix([[dis * cos(phi)], [dis * sin(phi)]])
hat_j = self.s[ii:ii + 2] + sim_env.rot_mtx(self.theta[idx]) * z
kalman_gain = sigma_j * (sigma_i + sigma_j).getI()
self.s[jj:jj +
2] = self.s[jj:jj +
2] + kalman_gain * (hat_j - self.s[jj:jj + 2])
total_sigma = (i_mtx_2 - kalman_gain) * sigma_j
self.sigma_i[jj:jj + 2, jj:jj + 2] = (
i_mtx_2 - kalman_gain) * self.sigma_i[jj:jj + 2, jj:jj + 2] * (
i_mtx_2 - kalman_gain).getT() + kalman_gain * self.sigma_i[
ii:ii + 2, ii:ii + 2] * kalman_gain.getT()
self.sigma_d[jj:jj + 2, jj:jj +
2] = total_sigma - self.sigma_i[jj:jj + 2, jj:jj + 2]
def motion_propagation_update(self, odometry_input, dt):
[v, omega] = odometry_input
ii = 2 * self.index
# estimation update
self.s[ii, 0] = self.s[ii, 0] + cos(self.theta) * v * dt
self.s[ii + 1, 0] = self.s[ii + 1, 0] + sin(self.theta) * v * dt
# covariance update
for j in range(sim_env.N):
jj = 2 * j
if j == self.index:
rot_mtx_theta = sim_env.rot_mtx(self.theta)
self.sigma_i[jj:jj + 2,
jj:jj + 2] += dt * dt * rot_mtx_theta * matrix([[
sim_env.var_u_v, 0
], [0, 0]]) * rot_mtx_theta.T
else:
self.sigma_i[
jj:jj + 2, jj:jj +
2] += dt * dt * sim_env.var_v * sim_env.i_mtx_2.copy()
def prop_update(self): for idx in range(5): dt = sim_env.dt ii = 2*idx # select valid motion input [v, a_v] = [random.uniform(-sim_env.max_v, sim_env.max_v), random.uniform(-sim_env.max_oemga, sim_env.max_oemga)] v_star = v + random.normal(0, sqrt(sim_env.var_u_v)) pre_update_position = [self.position[ii] + cos(self.theta[idx])*v_star*dt, self.position[ii+1] + sin(self.theta[idx])*v_star*dt] while(not sim_env.inRange(pre_update_position, sim_env.origin)): [v, a_v] = [random.uniform(-sim_env.max_v, sim_env.max_v), random.uniform(-sim_env.max_oemga, sim_env.max_oemga)] v_star = v + random.normal(0, sqrt(sim_env.var_u_v)) pre_update_position = [self.position[ii] + cos(self.theta[idx])*v_star*dt, self.position[ii+1] + sin(self.theta[idx])*v_star*dt] # real position update self.position[ii,0] += cos(self.theta[idx])*v_star*dt self.position[ii+1,0] += sin(self.theta[idx])*v_star*dt self.theta[idx] += a_v*dt # estimation update self.s[ii,0] += cos(self.theta[idx])*v*dt self.s[ii+1,0] += sin(self.theta[idx])*v*dt # covariance update self.sigma[ii:ii+2, ii:ii+2] += dt*dt*sim_env.rot_mtx(self.theta[idx])*matrix([[sim_env.var_u_v, 0],[0, sim_env.var_u_theta]])*sim_env.rot_mtx(self.theta[idx]).getT() self.th_sigma[ii:ii+2, ii:ii+2] += dt*dt*sim_env.var_u_v*matrix([[1, 0],[0, 1]])
def ablt_obsv(self, idx, obs_value, landmark):
ii = 2 * idx
H = sim_env.rot_mtx(self.theta[idx]).getT() * matrix(
[[-1, 0], [0, -1]], dtype=float)
local_s = self.s[ii:ii + 2]
local_sigma = self.sigma[ii:ii + 2, ii:ii + 2]
dis = obs_value[0]
phi = obs_value[1]
hat_z = sim_env.rot_mtx(
self.theta[idx]).getT() * (landmark.position + H * local_s)
z = matrix([dis * cos(phi), dis * sin(phi)]).getT()
sigma_z = sim_env.rot_mtx(phi) * matrix([[
sim_env.var_dis, 0
], [0, dis * dis * sim_env.var_phi]]) * sim_env.rot_mtx(phi).getT()
sigma_invention = H * local_sigma * H.getT() + sigma_z
kalman_gain = local_sigma * H.getT() * sigma_invention.getI()
self.s[ii:ii + 2] = local_s + kalman_gain * (z - hat_z)
self.sigma[ii:ii + 2,
ii:ii + 2] = local_sigma - kalman_gain * H * local_sigma
multi_i = (sim_env.i_mtx_2.copy() - kalman_gain * H)
for k in range(5):
if (not (k == idx)):
kk = 2 * k
self.sigma[ii:ii + 2, kk:kk +
2] = multi_i * self.sigma[ii:ii + 2, kk:kk + 2]
self.sigma[kk:kk + 2,
ii:ii + 2] = self.sigma[kk:kk + 2,
ii:ii + 2] * multi_i.getT()
def motion_propagation_update(self, odometry_input, dt): for i in range(sim_env.N): [v, omega] = odometry_input[i] ii = 2*i self.theta[i] = self.theta[i] + omega * dt # estimation update self.s[ii,0] += cos(self.theta[i]) * v * dt self.s[ii+1,0] += sin(self.theta[i]) * v * dt # covariance update rot_mtx_theta_i = sim_env.rot_mtx(self.theta[i]) self.sigma[ii:ii+2, ii:ii+2] += dt*dt*rot_mtx_theta_i*matrix([[sim_env.var_u_v, 0],[0, 0]])*rot_mtx_theta_i.T
