error_arr = [0] * time
for i in range(itr):
print(i)
initial = matrix([1, 1, 1, 2, 2, 1, -1, -1, 1, 3], dtype=float).T
robots = [None] * N
for n in range(N):
robots[n] = RobotLocalCI(n, initial.copy())
landmarks = [None] * M
for m in range(M):
landmarks[m] = sim_env.Landmark(
m,
matrix([0.01, 0.02], dtype=float).getT())
for t in range(time):
# motion propagation
for mp_iteration in range(sim_env.num_of_m_p):
robots[0].prop_update()
robots[1].prop_update()
robots[2].prop_update()
robots[3].prop_update()
robots[4].prop_update()
#robot 0
[dis, phi] = sim_env.relative_measurement(robots[0].position,
robots[0].theta,
gs_ci_robots = [None] * N
for n in range(N):
robots[n] = Robot(_position=initial[2 * n:2 * n + 2])
gs_ci_robots[n] = GS_CI(n, initial.copy())
ls_cen_team = LS_Cen(initial.copy())
ls_ci_team = LS_CI(initial.copy())
ls_sci_team = LS_SCI(initial.copy())
ls_bda_team = LS_BDA(initial.copy())
landmarks = [None] * M
for m in range(M):
landmarks[m] = sim_env.Landmark(
m,
np.matrix(sim_env.landmark_position, dtype=float).getT())
# simulation body
for t in range(total_T):
# reset theta
for n in range(N):
gs_ci_robots[n].theta = robots[n].theta
ls_cen_team.theta[n] = robots[n].theta
ls_ci_team.theta[n] = robots[n].theta
ls_sci_team.theta[n] = robots[n].theta
ls_bda_team.theta[n] = robots[n].theta
# motion propagation