github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:22 -04:00
Code Issues Packages Projects Releases Wiki Activity

delete duplicated initial pose

Browse Source
This commit is contained in:
ZhenghaoFei
2018-11-24 13:00:45 -08:00
parent b51f611cfe
commit cffb9d7d5d
1 changed files with 13 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
SLAM/GraphBasedSLAM/graph_based_slam.py
View File

@@ -169,7 +169,6 @@ def graph_based_slam(x_init, hz):
print("start graph based slam")
zlist = copy.deepcopy(hz)
zlist.insert(1, zlist[0])
x_opt = copy.deepcopy(x_init)
nt = x_opt.shape[1]
@@ -276,20 +275,28 @@ def main():
xDR = np.zeros((STATE_SIZE, 1)) # Dead reckoning
# history
hxTrue = xTrue
hxDR = xTrue
hxTrue = []
hxDR = []
hz = []
dtime = 0.0
init = False
while SIM_TIME >= time:
if not init:
hxTrue = xTrue
hxDR = xTrue
init = True
else:
hxDR = np.hstack((hxDR, xDR))
hxTrue = np.hstack((hxTrue, xTrue))
time += DT
dtime += DT
u = calc_input()
xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
hxDR = np.hstack((hxDR, xDR))
hxTrue = np.hstack((hxTrue, xTrue))
hz.append(z)
if dtime >= show_graph_dtime: