Change the code from problem two so it uses kalman updates from the localizer plugin.
periodic Kalman filtering
N=10
laser "addline startx=-0.9 starty=0.9 endx=0.9 endy=0.9 name='N'"
laser "addline startx=-0.9 starty=-0.9 endx=0.9 endy=-0.9 name='S'"
laser "addline startx=-0.9 starty=-0.9 endx=-0.9 endy=0.9 name='V'"
laser "addline startx=0.9 starty=-0.9 endx=0.9 endy=0.9 name='E'"
laser "setinitpose x=0 y=0 th=0"
laser "setinitcov Cx=0.1 Cy=0.1 Cth=0.1"
laser "push t='1.0' cmd='localize'"
label "run"
stop
ignoreobstacles
drivew 0.4 0.4 90:($targetdist<0.05)
stop
ignoreobstacles
drivew -0.4 0.4 -180:($targetdist<0.05)
stop
ignoreobstacles
drivew -0.4 -0.4 -90:($targetdist<0.05)
stop
ignoreobstacles
drivew 0.4 -0.4 0:($targetdist<0.05)
N=N-1
if(N>0) "run"
each corner Kalman filtering
N=10
laser "addline startx=-0.9 starty=0.9 endx=0.9 endy=0.9 name='N'"
laser "addline startx=-0.9 starty=-0.9 endx=0.9 endy=-0.9 name='S'"
laser "addline startx=-0.9 starty=-0.9 endx=-0.9 endy=0.9 name='V'"
laser "addline startx=0.9 starty=-0.9 endx=0.9 endy=0.9 name='E'"
laser "setinitpose x=0 y=0 th=0"
laser "setinitcov Cx=0.1 Cy=0.1 Cth=0.1"
laser "localize"
label "run"
stop
wait 0.5
laser "localize"
ignoreobstacles
drivew 0.5 0.5 90:($targetdist<0.05)
stop
wait 0.5
laser "localize"
ignoreobstacles
drivew -0.5 0.5 -180:($targetdist<0.05)
stop
wait 0.5
laser "localize"
ignoreobstacles
drivew -0.5 -0.5 -90:($targetdist<0.05)
stop
wait 0.5
laser "localize"
ignoreobstacles
drivew 0.5 -0.5 0:($targetdist<0.05)
N=N-1
if(N>0) "run"
N=10
laser "addline startx=-0.9 starty=0.9 endx=0.9 endy=0.9 name='N'"
laser "addline startx=-0.9 starty=-0.9 endx=0.9 endy=-0.9 name='S'"
laser "addline startx=-0.9 starty=-0.9 endx=-0.9 endy=0.9 name='V'"
laser "addline startx=0.9 starty=-0.9 endx=0.9 endy=0.9 name='E'"
laser "setinitpose x=0 y=0 th=0"
laser "setinitcov Cx=0.1 Cy=0.1 Cth=0.1"
laser "push t='1.0' cmd='localize'"
label "run"
stop
ignoreobstacles
drivew 0.4 0.4 90:($targetdist<0.05)
stop
ignoreobstacles
drivew -0.4 0.4 -180:($targetdist<0.05)
stop
ignoreobstacles
drivew -0.4 -0.4 -90:($targetdist<0.05)
stop
ignoreobstacles
drivew 0.4 -0.4 0:($targetdist<0.05)
N=N-1
if(N>0) "run"
each corner Kalman filtering
N=10
laser "addline startx=-0.9 starty=0.9 endx=0.9 endy=0.9 name='N'"
laser "addline startx=-0.9 starty=-0.9 endx=0.9 endy=-0.9 name='S'"
laser "addline startx=-0.9 starty=-0.9 endx=-0.9 endy=0.9 name='V'"
laser "addline startx=0.9 starty=-0.9 endx=0.9 endy=0.9 name='E'"
laser "setinitpose x=0 y=0 th=0"
laser "setinitcov Cx=0.1 Cy=0.1 Cth=0.1"
laser "localize"
label "run"
stop
wait 0.5
laser "localize"
ignoreobstacles
drivew 0.5 0.5 90:($targetdist<0.05)
stop
wait 0.5
laser "localize"
ignoreobstacles
drivew -0.5 0.5 -180:($targetdist<0.05)
stop
wait 0.5
laser "localize"
ignoreobstacles
drivew -0.5 -0.5 -90:($targetdist<0.05)
stop
wait 0.5
laser "localize"
ignoreobstacles
drivew 0.5 -0.5 0:($targetdist<0.05)
N=N-1
if(N>0) "run"
No comments:
Post a Comment