12/12/11 5:18 PM

C:\Users\Nav\Desktop\Deskto...\slam.m

1 of 16

1 function x=slam
2 % Basic Simultaneous Localisation and Mapping Algorithm usign EKF using Encoder and
Laser
3 % Juan Nieto
j.nieto@acfr.usyd.edu.au
4 % Eduardo Nebot
nebot@acfr.usyd.edu.au
5 % More information
http://acfr.usyd.edu.au/homepages/academic/enebot/summer_sch.
htm
6 %
7 %
8 %EkfSlam, this file is using "Nearest Neighbour" data association
9
10 close all; clear all;
11
12 %Beacons Positions, taken with a kinematic GPS ( 2cm ), used only to Compare
results
13 global beacons;
14 FileBEACON='beac_juan3.mat';
15 load(FileBEACON);
16 beacons=estbeac; clear estbeac;
17
18 DeltaT=1;
19 T0=0;
20 TF=112;
% max = 112 secs;
21
22 global GPSLon GPSLat
23
24 load('data_set');
% File with data stored according to spec provided
25
26 %----------------------------------------------------27 To=min([TimeGPS(1),Time_VS(1),TimeLaser(1)]);
28 TimeLaser=TimeLaser-To;
29 Time_VS=Time_VS-To;
%Init time --> 0
30 TimeGPS=TimeGPS-To;
31 Time=Time-To;
32 %----------------------------------------------------33 [tf,If]=FINDT(TimeGPS,TF);% to plot the gps data till TF
34 GPSLon=GPSLon(1:If);
35 GPSLat=GPSLat(1:If);
36
37 %
--------------------------------------------------------------------------------------38 %Prepare matrices to save data: f( Number of predictions + Number of updates Laser
39 %
+ Number of updates GPS)
40 global xest Pest TimeGlobal FlagS innov innvar;
41
42 numberbeacons=50;
43 NumPred=size(Time,2);
44 NumUpdaL=size(TimeLaser,2);
45 NumUpdaG=size(TimeGPS,2);
46
47 xest=zeros(3,NumPred+2*NumUpdaL+NumUpdaG);
48 Pest=zeros(numberbeacons,NumPred+2*NumUpdaL+NumUpdaG);
49 innov=zeros(2,2*(NumUpdaL+NumUpdaG));
50 innvar=zeros(2,2*(NumUpdaL+NumUpdaG));

12/12/11 5:18 PM

C:\Users\Nav\Desktop\Deskto...\slam.m

2 of 16

51 TimeGlobal=zeros(1,NumPred+2*NumUpdaL+NumUpdaG);
52 FlagS=zeros(1,NumPred+2*NumUpdaL+NumUpdaG);
%This flag is used to know if
we
53
%saved an update or a
prediction
54
55 %----------For the the Jacobians, states and covariances-------------------56 global Pp Jh xp
57
58 Pp=zeros(2*numberbeacons+3,2*numberbeacons+3);
59 Jh=zeros(2,2*numberbeacons+3);
60 xp=zeros(2*numberbeacons+3,1);
61 MatrixA=zeros(2*numberbeacons+3,2*numberbeacons+3);
%Auxiliar matrix for
J*Pest*J'
62 MatrixB=zeros(2*numberbeacons+3,2*numberbeacons+3);
63
64 %-------------On Line
Plot----------------------------------------------------------------65 global FlagWait FlagEnd hhh;
66 FlagWait = 0 ;
67 FlagEnd =0 ;
68
69 figure(10) ;clf ;
70 hold on;
71 uicontrol('Style','pushbutton','String','PAUSE','Position',[10 5 50 30],
'Callback', 'fOnOff(1);');
72 uicontrol('Style','pushbutton','String','END ','Position',[10 5+35 50 30],
'Callback', 'fOnOff(2);');
73
74 title('EKFSlam');xlabel('East (meters)');ylabel('North (meters)');
75 plot(GPSLon,GPSLat,'r.');axis([-10,20,-25,20]);%axis([2,33,-25,25]);%
76 plot(beacons(:,1),beacons(:,2),'b*')
77 hhh(1)=plot(0,0,'b','erasemode','none') ;
%path estimated
78 hhh(2)=plot(0,0,'r','erasemode','xor') ;
%what is used from the frame
79 hhh(3)=plot(0,0,'b.','erasemode','xor') ;
%laser, all the frame
80 hhh(4)=plot(0,0,'r+','erasemode','xor') ;
%high intensity only
81 hhh(5)=plot(0,0,'r','erasemode','xor') ;
%covariance ellipse x-y position
82 hhh(6)=plot(0,0,'r','erasemode','xor') ;
%covariance ellipse x-y beacon
83 hhh(7)=plot(0,0,'r','erasemode','xor') ;
%covariance ellipse x-y beacon
84
85 hhh(8)=plot(0,0,'sr','erasemode','xor') ;
%car
86 hhh(9)=plot(0,0,'go','erasemode','xor') ;
%beacons' position estimated
87 legend('GPS','Beacons','Estimated Path','Laser Data','All laser','H. Inten.')
88
89 hold off;
90
91 %----------------------------------------------------------------92 % Filter Tuning
93 % These are the parameter you need to modify to improve the operation
94 % of the filter ( Good Luck :-) )
95 %
96 global sigmaU sigma_laser sigma_gps;
97
98 %Internal Sensors ( dead - reckoning )
99
sigmastear=(7)*pi/180;
%Qu=7

12/12/11 5:18 PM

C:\Users\Nav\Desktop\Deskto...\slam.m

3 of 16

100
sigmavel=0.7;
%Qv=0.7
101
102 %Observations: laser Range and Bearing
( Sick laser )
103 %
We are estimating the centre of a 6 cm pole
104
SIGMA_RANGE=0.10;
%Rr=0.1
105
SIGMA_BEARING=(1)*pi/180;
%Ro=1
106
107 % Observations: GPS
108 % This is only used at the beginning to estimate absolute heading
109 % and then compare the localisation results with GPS
110
sigmagps=0.05;
111
112
113 % sensors error covariance
114
sigmaU=[sigmavel*sigmavel
0;
115
0
sigmastear*sigmastear];
116
117
sigma_laser=[SIGMA_RANGE^2
0;
118
0
SIGMA_BEARING^2] ;
119
120
sigma_gps=[sigmagps*sigmagps
0;
121
0
sigmagps*sigmagps] ;
122
123
124 %--------------Initial conditions & some
constants---------------------------------------125
126 global Pt isave index_update beacon2show tglobal trefresh plotall;
%trefresh is
to refresh
127
%the path in
the plot
128
129 finit=-112*pi/180; % or you could use something like: atan2(GPSLat(2)-GPSLat(1),
GPSLon(2)-GPSLon(1));
130
131 xinit=[GPSLon(1);GPSLat(1);finit];
132
133 Pinit= [0.1
0.0
0.0
;
134
0.0
0.1
0.0
;
135
0.0
0.0
(15)*pi/180 ];
136
137 u=[Velocity(1) ; Steering(1) ];
138
139 FlagStates=[-1;-1;-1];
%Initialization of flags to count the number of "hits"
of each beacon
140
141 Pt=3;
%Pointer to the last state, at the beggining we have the three
model's states
142 t1=cputime; trefresh=T0+2;
143 iglobal=0; isave=1; index_update=1; tglobal=0;
144 xp(1:Pt)=xinit; Pp(1:Pt,1:Pt)=Pinit;
145 plotellipse=0;
%Flag to plot the covariance of the vehicle and some
landmarks
146
% 1: plot 0: no plot
147 plotall=1;
%check in the plot function

12/12/11 5:18 PM

C:\Users\Nav\Desktop\Deskto...\slam.m

4 of 16

148 beacon2show=[4 5];

%These are the beacons which the covariance ellipse will be
show
149
%In this example only two beacons are selected.
150
151
152 %-----------------------------------Running
filter-----------------------------------------153 disp('Running filter...')
154
155
156 %************************** Navigation Loop Start
**************************************
157
158 while (tglobal<TF)
159
iglobal=iglobal+1;
160
tglobal=Time(iglobal);
161
if (iglobal>1)
162
dt=tglobal-Time(iglobal-1);
163
else
164
dt=0.025;
165
end
166
167
%Perform a prediction with delta = previous time - actual time
168
169
pred(dt,u);
%Prediction
170
SaveStates(xp(1:3),diag(Pp(1:Pt,1:Pt)),tglobal,0);
%save data
171
set(hhh(1),'XData',xp(1),'YData',xp(2)) ;
172
if plotellipse
173
plotCovariance; %plot the covariance ellipse (1-sigma)
174
else
175
set(hhh(8),'XData',xp(1),'YData',xp(2)) ;
176
end
177
178
%New Information, If External: do update, if dead-reckioning : set u for next
predition
179
180
%
----------------------------------------------------------------------------------------------------------------------181
% GPS is sensor 1, Only used for DeltaT to evaluate initial heading !!
182
if (Sensor(iglobal)==1) & (tglobal<(T0+DeltaT))
183
%GPS
184
zgps=[GPSLon(Index(iglobal));GPSLat(Index(iglobal))];
185
[in ins]=update_gps(zgps);
186
SaveStates(xp(1:3),diag(Pp(1:Pt,1:Pt)),tglobal,1); SaveInnov(in,diag(ins));
187
end
188
189
190
%
----------------------------------------------------------------------------------------------191
%Sensor =2 are Dead reckoning sensors
192
193
if Sensor(iglobal)==2
194
%Sensors

12/12/11 5:18 PM

C:\Users\Nav\Desktop\Deskto...\slam.m

5 of 16

195
u=[Velocity(Index(iglobal)) ;Steering(Index(iglobal))];
%SPEED IN m/s,
stearing in rads.
196
end
197
198
%
---------------------------------------------------------------------------------------------199
% Sensor = 3 is the Laser
200
201
if (Sensor(iglobal)==3) & (tglobal>(T0+DeltaT))
202
%Laser
203
bias=0*pi/180;
204
[LASERr LASERo RR a]=getdata(Laser(Index(iglobal),:), Intensity(Index
(iglobal),:)); %estimate beacon centre
205
zlaser=[LASERr ; LASERo+bias];
206
%
----------------------------------------------------------------------------------------------207
laserview(RR,a,LASERr,LASERo);
%plot the laser frame
208
209
%-----------------------Update-------------------------------------------------------------------210
for w=1:size(LASERr,2)
211
index1=0;
212
if Pt==3
% this is the first beacon and will be incorporated
213
% this can be improved building a list to avoid
spurious observ.
214
new_state(zlaser(:,w));
215
Pt=Pt+2;
%pointer to the last state in the s.v.
216
FlagStates(Pt-1:Pt)=1;
217
[index1,in,ins]=asoc_update(zlaser(:,w),(Pt-1),1);
%Update of the
new state
218
SaveStates(xp(1:3),diag(Pp(1:Pt,1:Pt)),tglobal,1);
%save states
219
SaveInnov(in,diag(ins));
%save
innovations
220
set(hhh(1),'XData',xp(1),'YData',xp(2)) ;
% on line plot
221
else
222
[closest]=Zone_Probe(zlaser(:,w));
%if not first association is
needed ( look only in reduced area )
223
closest=4+2.*(closest-1);
%pointer to the X beacons
position in the state vector
224
j=1;i=0; qu=[];possible=[];
225
while (j<=length(closest))
226
i=closest(j);
227
[index1,in,ins,q1]=asoc_update(zlaser(:,w),i,0);
%q1 is the
likelihood value, assoc only here
228
if index1==1
229
possible=[possible i];
230
qu=[qu q1];
231
end
232
j=j+1;
233
end
234
if ~isempty(possible)
235
if length(possible)>1
236
disp('Multi hypothesis problem');
% this may be a

12/12/11 5:18 PM

C:\Users\Nav\Desktop\Deskto...\slam.m

6 of 16

problem !
237
end
238
[value,index2]=min(qu);
239
twin=possible(index2);
%nearest
nighbour==max. likelihood
240
FlagStates(twin:twin+1)=FlagStates(twin:twin+1)+1;
241
[index1,in,ins]=asoc_update(zlaser(:,w),twin,1);
% perform the
update with best
242
SaveStates(xp(1:3),diag(Pp(1:Pt,1:Pt)),tglobal,1);
243
SaveInnov(in,diag(ins));
244
set(hhh(1),'XData',xp(1),'YData',xp(2)) ;
245
else
246
new_state(zlaser(:,w));
247
Pt=Pt+2;
248
FlagStates(Pt-1:Pt)=1;
249
[index1,in,ins]=asoc_update(zlaser(:,w),Pt-1,1);
%Update of
the new state
250
end
251
end
252
253
end
254
end
255
256
257
while FlagWait,
258
if FlagEnd, return
259
end
%This is for the buttons of "pause" and "end" in
the animation plot
260
pause(0.5) ;
261
end;
262
if FlagEnd; hold on;plot(xest(1,1:isave-1),xest(2,1:isave-1),'b');hold off;
return
263
end
264 end
265
266 %************************ Navigation Loop End **********************************
267
268 xest=xest(:,1:isave-1); Pest=Pest(:,1:isave-1);
269 innov=innov(:,1:index_update-1); innvar=innvar(:,1:index_update-1);
270 FlagS=FlagS(:,1:isave-1);
271 TimeGlobal=TimeGlobal(:,1:isave-1);
272
273
274 disp('Completed Filtering:')
275 t2=cputime;
%To know the real time of the algorithm
276 treal=TF-T0,
277 time=t2-t1,
278 taverage=(t2-t1)/iglobal,
279 %----------------------------Fiter completed -------------------------------------280
281 %This is to select just the beacons we saw more than "hits" times. ( for display
purposes
282 global estbeacons
283 hits=4;
284 aux=FlagStates(4:2:Pt);

12/12/11 5:18 PM

C:\Users\Nav\Desktop\Deskto...\slam.m

7 of 16

285 ii=find(aux>=hits);
286 xpos=4+2.*(ii-1);
287 estbeacons(:,1)=xp(xpos) ; estbeacons(:,2)=xp(xpos+1);
288 numbeac=size(estbeacons,1);
289
290
291 plots;
292
293 return;
294 %
------------------------------------------------------------------------------------------------------------------------------------295 %---------------------------------End of the main
function----------------------------------------------------------------------------296 %
------------------------------------------------------------------------------------------------------------------------------------297
298
299
300 %-----------------------------------------------------------------------------301 %
Auxiliary functions
302 %------------------------------------------------------------------------------303
304 function pred(dt,u)
305 % Function to generate a one-step vehicle prediction .
306
global Pt;
307
global Pp xp MatrixA MatrixB;
308
global sigmaU;
309
%---------------------------------------------------------------------310
%Car parameters
311
L=2.83 ; h=0.76; b = 1.21-1.42/2; a = 3.78;
312
%----------------------------------------------------------------------313
XSIZE=Pt; N=(Pt-3)/2;
%Pt=3+2*N
314
315
% input error transfer matrix
(df/du)
316
ve=u(1);
317
vc=ve/(1-tan(u(2))*h/L);
%The velocity has to be translated from the
back
318
%left wheel to the center of the axle
319
dvc_dve=(1-tan(u(2))*h/L)^-1;
320
dvc_dalpha=ve*h/(L*(cos(u(2)))^2*(1-tan(u(2))*h/L));
321
aux=(cos(u(2))^(-2));
322
T1=a*sin(xp(3))+b*cos(xp(3));
323
T2=a*cos(xp(3))-b*sin(xp(3));
324
325
b1=(cos(xp(3))-tan(u(2))/L*T1)*dvc_dve;
326
b3=(sin(xp(3))+tan(u(2))/L*T2)*dvc_dve;
327
b5=tan(u(2))/L*dvc_dve;
328
b2=-T1*vc/L*aux+b1*dvc_dalpha;
329
b4=T2*vc/L*aux+b1*dvc_dalpha;
330
b6=vc/L*aux+tan(u(2))/L*dvc_dalpha;
331
332
Bv=dt*[b1
b2;
333
b3
b4;

12/12/11 5:18 PM

C:\Users\Nav\Desktop\Deskto...\slam.m

8 of 16

334
b5
b6];
335
336 % state transition matrix evaluation
(df/dx) ------------------------------337
J1=[1 0 -dt*(vc*sin(xp(3))+vc/L*tan(u(2))*(a*cos(xp(3))-b*sin(xp(3))))
338
0 1 dt*(vc*cos(xp(3))-vc/L*tan(u(2))*(a*sin(xp(3))+b*cos(xp(3))))
%
Jacobian
339
0 0
1
];
340
I=eye(Pt-3,Pt-3);
341
342 % first state prediction ------------------------------------------------------343
344
xp(1)=xp(1) + dt*vc*cos(xp(3))-dt*vc/L*tan(u(2))*(a*sin(xp(3))+b*cos(xp(3)));
345
xp(2)=xp(2) + dt*vc*sin(xp(3))+dt*vc/L*tan(u(2))*(a*cos(xp(3))-b*sin(xp(3)));
346
xp(3)=xp(3) + dt*vc/L*tan(u(2));
347
xp(3)=NormalizeAngle(xp(3));
348
349 %J*Pest*J' --------------------------------------------------------------------350
P1=Pp(1:3,1:Pt);
351
P2=Pp(4:Pt,1:Pt);
352
Aux=[J1*P1
353
I*P2];
354
Aux1=Aux(1:3,1:3); Aux2=Aux(1:3,4:Pt); Aux3=Aux(4:Pt,1:3); Aux4=Aux(4:Pt,4:Pt);
355
356
MatrixA(1:Pt,1:Pt)=[Aux1*J1' Aux2*I
357
Aux3*J1' Aux4*I];
358
clear Aux Aux1 Aux2 Aux3 Aux4;
359 %--------------------------------------------------------------------------------360
361 %B*sigmaU*B'---------------------------------------------------------------------362
MatrixB(1:Pt,1:Pt)=[Bv*sigmaU*Bv' zeros(3,Pt-3)
363
zeros(Pt-3,Pt)
];
364 %--------------------------------------------------------------------------------365
Pp(1:Pt,1:Pt)= MatrixA + MatrixB;
366 return;
367
368
369 %
-------------------------------------------------------------------------------------------------------------------------------------370 %Update with GPS data
371 function [innov, S]=update_gps(zgps)
372
global xp Pp Pt
373
global sigma_gps;
374
375
H=zeros(2,Pt); H(1,1)=1; H(2,2)=1;
376
377
S=H*Pp(1:Pt,1:Pt)*H' + sigma_gps;
378
W=Pp(1:Pt,1:Pt)*H'* inv(S);
379
Pp(1:Pt,1:Pt)=Pp(1:Pt,1:Pt)-W*S*W';
380
innov=[zgps-H*xp(1:Pt)];
381
382
xp(1:Pt)=xp(1:Pt)+W*innov;
383 return;
384
385

12/12/11 5:18 PM

C:\Users\Nav\Desktop\Deskto...\slam.m

9 of 16

386 %
-------------------------------------------------------------------------------------------------------------------------------------387 % This function perfor association or update accoding to the value of updatee
388 % updatee=0 -> association only, updatee=1 -> association and update
389 function [index,innov, S, q1]=asoc_update(zlaser,pointer,updatee)
390
global Pt;
391
global Pp xp Jh;
392
global sigma_laser;
393
394
beacon=[xp(pointer) xp(pointer+1)];
%Xi,Yi
395
dx=xp(1)-beacon(1);
396
dy=xp(2)-beacon(2);
397
d=(dx^2+dy^2)^0.5;
398
399
Jh(1:2,1:3)=[dx/d
dy/d
0;
400
-dy/(d^2) dx/(d^2) -1];
401
Jh(1:2,4:Pt)=zeros(2,(Pt-3));
402
Jh(1:2,(pointer):(pointer+1))=[-dx/d -dy/d ; dy/(d^2) -dx/(d^2)];
403
404
H=[d ; atan2((beacon(2)-xp(2)),(beacon(1)-xp(1)))-xp(3) + pi/2];
%h
(xpred)
405
406
H(2)=NormalizeAngle(H(2));
407
S=Jh(1:2,1:Pt)*Pp(1:Pt,1:Pt)*(Jh(1:2,1:Pt))' + sigma_laser;
408
innov=[zlaser-H];
409
innov(2)=NormalizeAngle(innov(2));
410
411
if updatee==1
%is an update, if this flag is
zero is an asociation
412
W=Pp(1:Pt,1:Pt)*(Jh(1:2,1:Pt))'* inv(S);
413
Pp(1:Pt,1:Pt)=Pp(1:Pt,1:Pt)-W*S*W';
%Update of the error
covariance matrix
414
xp(1:Pt)=xp(1:Pt)+W*innov;
415
xp(3)=NormalizeAngle(xp(3));
416
index=0; q1=0;
417
else
418
419
chi=5.99;
%95% confidence
420
q=(innov')*(inv(S))*innov;
421
if (q<chi)
%Chi square test
422
index=1;
423
q1=q+log(det(S));
424
else
425
index=0; q1=0;
426
end;
427
clear q W;
428
end
429 return;
430
431
432 %
-------------------------------------------------------------------------------------------------------------------------------------433 % Insert a new state assigning a large error ( simpler approach )

12/12/11 5:18 PM

C:\Users\Nav\Desktop\Deskto...\slam.m

10 of 16

434 function new_state(zlaser)

435
global Pt xp Pp;
436
437
%First of all, calculate position beacon in the cartesian global coordiante.
438
xx = xp(1)+zlaser(1)*cos(zlaser(2)+xp(3)-pi/2) ;yy = xp(2)+zlaser(1)*sin(zlaser
(2)+xp(3)-pi/2) ;
439
440
xp(Pt+1)=xx;
441
xp(Pt+2)=yy;
442
clear xx yy;
443
444
Pp(Pt+1,1:(Pt))=0;
%x row
445
Pp(1:Pt,Pt+1)=0;
%x column
446
Pp(Pt+1,Pt+1)=10^6;
%x diagonal
447
448
Pp(Pt+2,1:(Pt+1))=0;
%y row
449
Pp(1:Pt+1,Pt+2)=0;
%y column
450
Pp(Pt+2,Pt+2)=10^6;
%y diagonal ( this may introduce numerical
problems if not choosen properly )
451
452 return;
453
454 %
-------------------------------------------------------------------------------------------------------------------------------------455 %Function to find index in data between TO and TF
456 function [t,I]=FINDT(Var,ttt)
457
ii=find(Var>=ttt);
458
I=ii(1);
459
t=Var(I);
460 return;
461
462 %
-------------------------------------------------------------------------------------------------------------------------------------463 %function for the on-line plot
464 function fOnOff(x)
465
global FlagWait FlagEnd;
466
if x==1, FlagWait=~FlagWait ; return ; end ;
467
if x==2, FlagEnd=1 ; return ; end ;
468 return ;
469
470 %
-------------------------------------------------------------------------------------------------------------------------------------471 %Transform GPS lat and Long to local navigation frame centred at a reference pt
472 function [GPSTIME,LONG,LAT] =ReadGpsData(file)
473
load(file) ;
474
file;
475
LONG = GPS(:,4)' ;
476
LAT = -GPS(:,3)' ;
%We are in the south, the latitude is negative
477
GPSTIME = GPS(:,1)'/1000 ;
478
479
LAT0 = -33.8884;
%put any point on the map, is not a good idea to put
"magic numbers" (as in this case), the best

12/12/11 5:18 PM

C:\Users\Nav\Desktop\Deskto...\slam.m

11 of 16

480
LONG0 = 151.1948;
%would be to take the average of the initial points
481
482
a = 6378137.0; b = a*(1-0.003352810664747);
%Linalization from
latitude and long. to meters in a local area
483
kpi = pi/180 ;
484
cf = cos(LAT0*kpi) ; sf = abs(sin(LAT0*kpi)) ;
485
Ro = a*a*cf/abs(sqrt(a*a*cf*cf + b*b*sf*sf)) ;
486
RR = b/a * abs(sqrt( a*a-Ro*Ro))/sf ;
487
488
LAT =(LAT - LAT0 )*RR*kpi
;
489
LONG=(LONG- LONG0)*Ro*kpi
;
490 return ;
491 %
---------------------------------------------------------------------------------------------------------------------------------------492 %read steering data, not used in this case
493 function [Time,STEERING,SPEED1] = ReadUteData(file)
494
load(file) ;
495
STEERING = SENSORS(:,4)' ;
496
SPEED1
= SENSORS(:,6)' ;
497
Time
= SENSORS(:,1)'/1000 ;
498 return ;
499
500
501 %
---------------------------------------------------------------------------------------------------------------------------------------502 %This function perform the estimation of the beacon centre, It can be improved
503 %There is a more general version: detectrees that works well for all cylindrical
504 %objects
505 function [LASERr,LASERo,RR,a]=getdata(laser,intensity)
506 LASERr=[];
507
LASERo=[];
508
first=0;
509
max_range=30; min_range=1;
510
angleDiff=3;
511
RR=laser; a=intensity;
512
for i=1:361
513
if (min_range<RR(i)<max_range) & (a(i)>0)
514
primera=0;
515
last=[RR(i),i];
516
if first==0
517
init=[RR(i),i];
518
first=1;
519
end
520
else
521
if first==1
522
if primera==0
523
primera=1;
524
else
525
if (i-last(2))>2
526
first=0;
527
range=mean([init(1),last(1)]);
528
angle=mean([init(2),last(2)]);
529
LASERr=[LASERr range];

12/12/11 5:18 PM

C:\Users\Nav\Desktop\Deskto...\slam.m

12 of 16

530
LASERo=[LASERo (angle-1)/2*pi/180];
531
end
532
end
533
end
534
end
535
end
536 return;
537
538 %
-------------------------------------------------------------------------------------------------------------------------------539 %Is looking for the beacons that are "min_dist" close to te observation
540 % in this case it is set to 3 meters. Modify if necesary
541 function
[closest]=Zone_Probe(zlaser);
542
global Pt xp;
543
min_dist=3;
544
ii=[4:2:Pt-1];
545
xx = xp(1)+zlaser(1)*cos(zlaser(2)+xp(3)-pi/2) ;yy = xp(2)+zlaser(1)*sin(zlaser
(2)+xp(3)-pi/2) ;
%Position
546
d=((xx-xp(ii)).^2+(yy-xp(ii+1)).^2).^0.5;
547
iii=find(d<min_dist);
548
closest=iii;
549
clear xx yy;
550
return;
551 return;
552
553
554 %
-------------------------------------------------------------------------------------------------------------------------------555 %Plot the laser scan
556 function laserview(RR,a,LASERr,LASERo)
557
global xp hhh;
558
global isave xest tglobal trefresh;
559
aa = [0:360]*pi/360 ;
560
ii=find(RR<50 & RR>1) ;
561
aa2=aa(ii) ; xx = RR(ii).*cos(aa2+xp(3)-pi/2) ;yy = RR(ii).*sin(aa2+xp(3)-pi/2)
;
%All points
562
set(hhh(3),'XData',xx+xp(1),'YData',yy+xp(2));
563
pause(0.01);
564
565
ii=find(a>0) ;
566
aa2=aa(ii) ; xx = RR(ii).*cos(aa2+xp(3)-pi/2) ;yy = RR(ii).*sin(aa2+xp(3)-pi/2)
;
%High intensity points
567
set(hhh(4),'XData',xx+xp(1),'YData',yy+xp(2));
568
pause(0.01);
569
570
ll = length(LASERr) ;
571
if ll>0,
572
xx = LASERr.*cos(LASERo+xp(3)-pi/2) ;
%The points we are
taking from one frame
573
yy = LASERr.*sin(LASERo+xp(3)-pi/2) ;
574
set(hhh(9),'XData',xx+xp(1),'YData',yy+xp(2));
575
576
index=[1:3:3*ll];

12/12/11 5:18 PM

C:\Users\Nav\Desktop\Deskto...\slam.m

13 of 16

577
xpoints=zeros(3*ll,1); ypoints=zeros(3*ll,1);
%this is to plot
the lines between the beacons and the car
578
xpoints(index)=xp(1); ypoints(index)=xp(2);
579
xpoints(index+1)=xx+xp(1); ypoints(index+1)=yy+xp(2);
580
xpoints(index+2)=NaN;ypoints(index+2)=NaN;
581
set(hhh(2),'XData',xpoints,'YData',ypoints)
582
pause(0.01);
583
else
584
set(hhh(2),'XData',0,'YData',0)
585
end ;
586
if (tglobal-trefresh)>2
%every
"trefresh" seconds is doing s refresh of the whole path
587
set(hhh(1),'XData',xest(1,1:isave-1),'YData',xest(2,1:isave-1))
588
trefresh=tglobal;
589
end
590 return;
591
592 %
-------------------------------------------------------------------------------------------------------------------------------593 % saving the state
594 function SaveStates(states,diagcov,times,Flag)
595
global isave xest Pest TimeGlobal Pt FlagS;
596
xest(:,isave)=states;
597
Pest(1:Pt,isave)=diagcov;
598
TimeGlobal(1,isave)=times;
599
FlagS(1,isave)=Flag;
600
isave=isave+1;
601 return;
602
603 %
--------------------------------------------------------------------------------------------------------------------------------604 %saving innovations
605 function SaveInnov(in,ins);
606
global innov innvar index_update;
607
innov(:,index_update)=in;
608
innvar(:,index_update)=ins;
609
index_update=index_update+1;
610 return;
611
612 %
--------------------------------------------------------------------------------------------------------------------------------613 %transform angles to -pi to pi
614 function ang2 = NormalizeAngle(ang1)
615
if ang1>pi, ang2 =ang1-2*pi ; return ; end ;
616
if ang1<-pi, ang2 =ang1+2*pi ; return ; end ;
617
ang2=ang1;
618 return
619
620 %
--------------------------------------------------------------------------------------------------------------------------------621 %plot 1-sigma uncertainty region for a P covariance matrix.

12/12/11 5:18 PM

C:\Users\Nav\Desktop\Deskto...\slam.m

14 of 16

622 % Jose-1999
623 function xxx=ellipse(X0,P,veces,color,figu)
624
625
626
R = chol(P)'; % R*R'= P, X = R*X2
627
r = 1 ; %circle's radius in space X2
628
aaa = [0:veces]*2*pi/veces ;
% sample angles
629
xxx = [ r*cos(aaa) ; r*sin(aaa) ] ; % polar to x2,y2
630
xxx = R*xxx ;
% x2,y2 to x,y
631
xxx(1,:) = X0(1)+xxx(1,:);
% reference to center X0
632
xxx(2,:) = X0(2)+xxx(2,:);
633 return;
634
635 %
--------------------------------------------------------------------------------------------------------------------------------636 function Rxx=auto(x)
%This is used just for the plot
637
638
[N,nul]=size(x');
639
M=round(N/2);
640
641
Xpsd=fft(x);
642
Pxx=Xpsd.*conj(Xpsd)/N;
643
644
% the inverse is the autocorrelation
645
Rxx=real(ifft(Pxx));
646
Rxx=Rxx(1:M);
647
fact=Rxx(1);
648
for i=1:M
649
Rxx(i)=Rxx(i)/fact;
650
end
651 return
652
653
654 %
-------------------------------------------------------------------------------------------------------------------------------655 %Include all your off-line plots here ( runs when finish )
656 function plots
657
global xest Pest GPSLon GPSLat beacons;
658
global innov innvar;
659
global FlagS TimeGlobal estbeacons plotall;
660
ii=find(FlagS==1);
661
timeinnov=TimeGlobal(ii);
%Innovations time stamps
662
663 figure(1);clf
664 plot(xest(1,:),xest(2,:),'c',xest(1,:),xest(2,:),'b.',estbeacons(:,1),estbeacons(:,
2),'r*',beacons(:,1),beacons(:,2),'bo',GPSLon,GPSLat,'g.');grid on;
665 legend('Estimated','Est. Sample','Est. Beac.','Beacons','GPS')
666 xlabel('East Meters')
667 ylabel('North Meters')
668 title('Path')
669
670 if plotall
671
figure(2);clf

12/12/11 5:18 PM

C:\Users\Nav\Desktop\Deskto...\slam.m

15 of 16

672
plot(xest(1,:),xest(2,:),'r');grid on;
673
xlabel('East Meters')
674
ylabel('North Meters')
675
title('Path Estimated')
676
677
figure(3);clf
678
hold on
679
axis([timeinnov(1) timeinnov(size(timeinnov,2)) -0.5 0.5]);grid on;
680
plot(timeinnov(1,:),innov(1,:)), title('Zr Innovations')%
681
plot(timeinnov(1,:),2*sqrt(innvar(1,:)),'r')
682
plot(timeinnov(1,:),-2*sqrt(innvar(1,:)),'r')
683
xlabel('Time(secs)');ylabel('Desviation(m)');
684
legend('Innovations','Sta. Desv. Inn. (95%)')
685
hold off
686
687
figure(4);clf
688
Rxx1=auto(innov(1,:));
689
Rxx2=auto(innov(2,:));
690
M=round(size(innov,2)/2);
691
bounds=2*sqrt(1/(M));
692
plot([1:M],Rxx1,'b',[1:M],Rxx2,'r',[1:M],bounds,'g',[1:M],-bounds,'g');grid on;
693
title('Autocorrelation of Innovation Sequence')
694
legend('Var. Zr','Var. Ztheta','95% Confi. Bounds')
695
696
figure(5);clf
697
hold on
698
axis([TimeGlobal(1) TimeGlobal(size(TimeGlobal,2)) 0 1]);grid on;
699
plot(TimeGlobal,sqrt(Pest(1,:)),'b')
700
plot(TimeGlobal,sqrt(Pest(2,:)),'r')
701
plot(TimeGlobal,sqrt(Pest(3,:)),'g')
702
xlabel('Time(secs)');ylabel('Desviation(m)');
703
title('Model Covariance');
704
legend('X var.','Y var.','Steer.')
705
hold off
706
707
figure(6);clf
708
hold on
709
axis([TimeGlobal(1) TimeGlobal(size(TimeGlobal,2)) 0 1]);grid on;
710
plot(TimeGlobal,sqrt(Pest(4,:)),'b'); plot(TimeGlobal,sqrt(Pest(5,:)),'b.')
711
plot(TimeGlobal,sqrt(Pest(8,:)),'r'); plot(TimeGlobal,sqrt(Pest(9,:)),'r.')
712
plot(TimeGlobal,sqrt(Pest(12,:)),'g'); plot(TimeGlobal,sqrt(Pest(13,:)),'g.')
713
plot(TimeGlobal,sqrt(Pest(16,:)),'m'); plot(TimeGlobal,sqrt(Pest(17,:)),'m.')
714
xlabel('Time(secs)');ylabel('Desviation(m)');
715
title('Beacons Covariances');
716
legend('Beac.1(east)','Beac.1(north)','Beac.3(east)','Beac.3(north)','Beac.5
(east)','Beac.5(north)','Beac.7(east)','Beac.7(north)')
717
hold off
718 end
719 return;
720 %
-------------------------------------------------------------------------------------------------------------------------------721 function plotCovariance
722
global hhh xp Pp beacon2show Pt;
723
xxx=ellipse(xp(1:2),Pp(1:2,1:2),50); set(hhh(5),'XData',xxx(1,:),'YData',xxx

12/12/11 5:18 PM

C:\Users\Nav\Desktop\Deskto...\slam.m

16 of 16

(2,:)) ;
%Position
724
if Pt>=(3+2*beacon2show(2))
725
xxx=ellipse(xp(4+2*(beacon2show(1)-1):4+2*(beacon2show(1)-1)+1),Pp(4+2*
(beacon2show(1)-1):4+2*(beacon2show(1)-1)+1,4+2*(beacon2show(1)-1):4+2*(beacon2show(1)
-1)+1),50);
726
set(hhh(6),'XData',xxx(1,:),'YData',xxx(2,:)) ;
727
xxx=ellipse(xp(4+2*(beacon2show(2)-1):4+2*(beacon2show(2)-1)+1),Pp(4+2*
(beacon2show(2)-1):4+2*(beacon2show(2)-1)+1,4+2*(beacon2show(2)-1):4+2*(beacon2show(2)
-1)+1),50);
728
set(hhh(7),'XData',xxx(1,:),'YData',xxx(2,:)) ;
729
end
730 return;
731 %
-------------------------------------------------------------------------------------------------------------------------------732