env,
sensor=GPS,
search_alg=Astar)
print("searching the path......")
searchpath = simplerobot.search_path(handles, goalconfig)
print("Find the path!")
targetinput = simplerobot.path2input(searchpath)
simplerobot.update(env, handles)
kf = KF(simplerobot)
pid = PID(searchpath,
kp=np.array([5e-1, 5e-1, 0]),
kd=np.array([5e-2, 5e-2, 0]),
ki=np.array([5e-2, 5e-2, 0]))
for ii in range(targetinput.shape[0]):
simplerobot.input = targetinput[ii, :] + pid.FeedBack(kf.mu)
simplerobot.predict(env)
simplerobot.update(env, handles)
kf.update(env, handles)
time.sleep(0.05)
print("Finish case3.1!")
print("\n\n\n")
time.sleep(4)
############################## CASE3.2 ##############################
######## robot dynamic: SimpleDynamicRobot
######## sensor: self-defined sensor
######## map: room
######## estimation: KF
pf.clearParticles()
pf.handles = []
handles = []
