function x=slam
% Basic Simultaneous Localisation and Mapping Algorithm usign EKF using Encoder and Laser
% Juan Nieto j.nieto@acfr.usyd.edu.au
% Eduardo Nebot nebot@acfr.usyd.edu.au
% More information http://acfr.usyd.edu.au/homepages/academic/enebot/summer_sch.htm
%
%
%EkfSlam, this file is using "Nearest Neighbour" data association
close all; clear all;
%Beacons Positions, taken with a kinematic GPS ( 2cm ), used only to Compare results
global beacons;
FileBEACON='beac_juan3.mat';
load(FileBEACON);
beacons=estbeac; clear estbeac;
DeltaT=1;
T0=0;
TF=112; % max = 112 secs;
global GPSLon GPSLat
load('data_set'); % File with data stored according to spec provided
%-----------------------------------------------------
To=min([TimeGPS(1),Time_VS(1),TimeLaser(1)]);
TimeLaser=TimeLaser-To;
Time_VS=Time_VS-To; %Init time --> 0
TimeGPS=TimeGPS-To;
Time=Time-To;
%-----------------------------------------------------
[tf,If]=FINDT(TimeGPS,TF);% to plot the gps data till TF
GPSLon=GPSLon(1:If);
GPSLat=GPSLat(1:If);
%----------------------------------------------------------------------------------------
%Prepare matrices to save data: f( Number of predictions + Number of updates Laser
% + Number of updates GPS)
global xest Pest TimeGlobal FlagS innov innvar;
numberbeacons=50;
NumPred=size(Time,2);
NumUpdaL=size(TimeLaser,2);
NumUpdaG=size(TimeGPS,2);
xest=zeros(3,NumPred+2*NumUpdaL+NumUpdaG);
Pest=zeros(numberbeacons,NumPred+2*NumUpdaL+NumUpdaG);
innov=zeros(2,2*(NumUpdaL+NumUpdaG));
innvar=zeros(2,2*(NumUpdaL+NumUpdaG));
TimeGlobal=zeros(1,NumPred+2*NumUpdaL+NumUpdaG);
FlagS=zeros(1,NumPred+2*NumUpdaL+NumUpdaG); %This flag is used to know if we
%saved an update or a prediction
%----------For the the Jacobians, states and covariances--------------------
global Pp Jh xp
Pp=zeros(2*numberbeacons+3,2*numberbeacons+3);
Jh=zeros(2,2*numberbeacons+3);
xp=zeros(2*numberbeacons+3,1);
MatrixA=zeros(2*numberbeacons+3,2*numberbeacons+3); %Auxiliar matrix for J*Pest*J'
MatrixB=zeros(2*numberbeacons+3,2*numberbeacons+3);
%-------------On Line Plot-----------------------------------------------------------------
global FlagWait FlagEnd hhh;
FlagWait = 0 ;
FlagEnd =0 ;
figure(10) ;clf ;
hold on;
uicontrol('Style','pushbutton','String','PAUSE','Position',[10 5 50 30], 'Callback', 'fOnOff(1);');
uicontrol('Style','pushbutton','String','END ','Position',[10 5+35 50 30], 'Callback', 'fOnOff(2);');
title('EKFSlam');xlabel('East (meters)');ylabel('North (meters)');
plot(GPSLon,GPSLat,'r.');axis([-10,20,-25,20]);%axis([2,33,-25,25]);%
plot(beacons(:,1),beacons(:,2),'b*')
hhh(1)=plot(0,0,'b','erasemode','none') ; %path estimated
hhh(2)=plot(0,0,'r','erasemode','xor') ; %what is used from the frame
hhh(3)=plot(0,0,'b.','erasemode','xor') ; %laser, all the frame
hhh(4)=plot(0,0,'r+','erasemode','xor') ; %high intensity only
hhh(5)=plot(0,0,'r','erasemode','xor') ; %covariance ellipse x-y position
hhh(6)=plot(0,0,'r','erasemode','xor') ; %covariance ellipse x-y beacon
hhh(7)=plot(0,0,'r','erasemode','xor') ; %covariance ellipse x-y beacon
hhh(8)=plot(0,0,'sr','erasemode','xor') ; %car
hhh(9)=plot(0,0,'go','erasemode','xor') ; %beacons' position estimated
legend('GPS','Beacons','Estimated Path','Laser Data','All laser','H. Inten.')
hold off;
%-----------------------------------------------------------------
% Filter Tuning
% These are the parameter you need to modify to improve the operation
% of the filter ( Good Luck :-) )
%
global sigmaU sigma_laser sigma_gps;
%Internal Sensors ( dead - reckoning )
sigmastear=(7)*pi/180; %Qu=7
sigmavel=0.7; %Qv=0.7
%Observations: laser Range and Bearing ( Sick laser )
% We are estimating the centre of a 6 cm pole
SIGMA_RANGE=0.10; %Rr=0.1
SIGMA_BEARING=(1)*pi/180; %Ro=1
% Observations: GPS
% This is only used at the beginning to estimate absolute heading
% and then compare the localisation results with GPS
sigmagps=0.05;
% sensors error covariance
sigmaU=[sigmavel*sigmavel 0;
0 sigmastear*sigmastear];
sigma_laser=[SIGMA_RANGE^2 0;
0 SIGMA_BEARING^2] ;
sigma_gps=[sigmagps*sigmagps 0;
0 sigmagps*sigmagps] ;
%--------------Initial conditions & some constants----------------------------------------
global Pt isave index_update beacon2show tglobal trefresh plotall; %trefresh is to refresh
%the path in the plot
finit=-112*pi/180; % or you could use something like: atan2(GPSLat(2)-GPSLat(1),GPSLon(2)-GPSLon(1));
xinit=[GPSLon(1);GPSLat(1);finit];
Pinit= [0.1 0.0 0.0 ;
0.0 0.1 0.0 ;
0.0 0.0 (15)*pi/180 ];
u=[Velocity(1) ; Steering(1) ];
FlagStates=[-1;-1;-1]; %Initialization of flags to count the number of "hits" of each beacon
Pt=3; %Pointer to the last state, at the beggining we have the three model's states
t1=cputime; trefresh=T0+2;
iglobal=0; isave=1; index_update=1; tglobal=0;
xp(1:Pt)=xinit; Pp(1:Pt,1:Pt)=Pinit;
plotellipse=0; %Flag to plot the covariance of the vehicle and some landmarks
% 1: plot 0: no plot
plotall=1; %check in the plot function
beacon2show=[4 5]; %These are the beacons which the covariance ellipse will be show
%In this example only two beacons are selected.
%-----------------------------------Running filter------------------------------------------
disp('Running filter...')
%************************** Navigation Loop Start **************************************
while (tglobal<TF)
iglobal=iglobal+1;
tglobal=Time(iglobal);
if (iglobal>1)
dt=tglobal-Time(iglobal-1);
else
dt=0.025;
end
%Perform a prediction with delta = previous time - actual time
pred(dt,u); %Prediction
SaveStates(xp(1:3),diag(Pp(1:Pt,1:Pt)),tglobal,0); %save data
set(hhh(1),'XData',xp(1),'YData',xp(2)) ;
if plotellipse
plotCovariance; %plot the covariance ellipse (1-sigma)
else
set(hhh(8),'XData',xp(1),'YData',xp(2)) ;
end
%New Information, If External: do update, if dead-reckioning : set u for next predition
%------------------------------------------------------------------------------------------------------------------------
% GPS is sensor 1, Only used for DeltaT to evaluate initial heading !!
if (Sensor(iglobal)==1) & (tglobal<(T0+DeltaT))
%GPS
zgps=[GPSLon(Index(iglobal));GPSLat(Index(iglobal))];
[in ins]=update_gps(zgps);
SaveStates(xp(1:3),diag(Pp(1:Pt,1:Pt)),tglobal,1); SaveInnov(in,diag(ins));
end
%------------------------------------------------------------------------------------------------
%Sensor =2 are Dead reckoning sensors
if Sensor(iglobal)==2
%Sensors
u=[Velocity(Index(iglobal)) ;Steering(Index(iglobal))]; %SPEED IN m/s, stearing in rads.
end
%-----------------------------------------------------------------------------------------------
% Sensor = 3 is the Laser
if (Sensor(iglobal)==3) & (tglobal>(T0+DeltaT))
%Laser
bias=0*pi/180;
[LASERr LASERo RR a]=getdata(Laser(Index(iglobal),:), Intensity(Index(iglobal),:)); %estimate beacon centre
zlaser=[LASERr ; LASERo+bias];
%------------------------------------------------------------------------------------------------
laserview(RR,a,LASERr,LASERo); %plot the laser frame
%------------------------Update--------------------------------------------------------------------
for w=1:size(LASERr,2)
index1=0;
if Pt==3 % this is the first beacon and will be incorporated
% this can be improved building a list to avoid spurious observ.
new_state(zlaser(:,w));
Pt=Pt+2; %pointer to the last state in the s.v.
FlagStates(Pt-1:Pt)=1;
[index1,in,ins]=asoc_update(zlaser(:,w),(Pt-1),1); %Update of the new state
SaveStates(xp(1:3),diag(Pp(1:Pt,1:Pt)),tglobal,1); %save states
SaveInnov(in,diag(ins)); %save innovations
set(hhh(1),'XData',xp(1),'YData',xp(2)) ; % on line plot
else
[closest]=Zone_Probe(zlaser(:,w)); %if not first association is needed ( look only in reduced area )
closest=4+2.*(closest-1); %pointer to the X beacons position in the state vector
j=1;i=0; qu=[];possible=[];
while (j<=length(closest))
i=closest(j);
[index1,in,ins,q1]=asoc_update(zlaser(:,w),i,0); %q1 is the likelihood value, assoc only here
if index1==1
possible=[possible i];
qu=[qu q1];
end
j=j+1;
end
if ~isempty(possible)
if length(possible)>1
disp('Multi hypothesis problem'); % this may be a problem !
end
[value,index2]=min(qu);
twin=possible(index2); %nearest nighbour==max. likelihood
FlagStates(twin:twin+1)=FlagStates(twin:twin+1)+1;
[index1,in,ins]=asoc_update(zlaser(:,w),twin,1); % perform the update with best
SaveStates(xp(1:3),diag(Pp(1:Pt,1:Pt)),tglobal,1);
SaveInnov(in,diag(ins));
set(hhh(1),'XData',xp(1),'YData',xp(2)) ;
else
new_state(zlaser(:,w));
Pt=Pt+2;
FlagStates(Pt-1:Pt)=1;
[index1,in,ins]=asoc_update(zlaser(:,w),Pt-1,1); %Update of the new state
end
end
end
end
while FlagWait,
if FlagEnd, return
end %This is for the buttons of "pause" and "end" in the animation plot
pause(0.5) ;
end;
if FlagEnd; hold on;plot(xest(1,1:isave-1),xest(2,1:isave-1),'b');hold off; return
end
end
%************************ Navigation Loop End **********************************
xest=xest(:,1:isave-1); Pest=Pest(:,1:isave-1);
innov=innov(:,1:index_update-1); innvar=innvar(:,1:index_update-1);
FlagS=FlagS(:,1:isave-1);
TimeGlobal=TimeGlobal(:,1:isave-1);
disp('Completed Filtering:')
t2=cputime; %To know the real time of the algorithm
treal=TF-T0,
time=t2-t1,
taverage=(t2-t1)/iglobal,
%----------------------------Fiter completed --------------------------------------
%This is to select just the beacons we saw more than "hits" times. ( for display purposes
global estbeacons
hits=4;
aux=FlagStates(4:2:Pt);
ii=find(aux>=hits);
xpos=4+2.*(ii-1);
estbeacons(:,1)=xp(xpos) ; estbeacons(:,2)=xp(xpos+1);
numbeac=size(estbeacons,1);
plots;
return;
%--------------------------------------------------------------------------------------------------------------------------------------
%---------------------------------End of the main function-----------------------------------------------------------------------------
%--------------------------------------------------------------------------------------------------------------------------------------
%------------------------------------------------------------------------------
% Auxiliary functions
%-------------------------------------------------------------------------------
function pred(dt,u)
% Function to generate a one-step vehicle prediction .
global Pt;
global Pp xp MatrixA MatrixB;
global sigmaU;
%----------------------------------------------------------------------
%Car parameters
L=2.83 ; h=0.76; b = 1.21-1.42/2; a = 3.78;
%-----------------------------------------------------------------------
XSIZE=Pt; N=(Pt-3)/2; %Pt=3+2*N
% input error transfer matrix (df/du)
ve=u(1);
vc=ve/(1-tan(u(2))*h/L); %The velocity has to be translated from the back
%left wheel to the center of the axle
dvc_dve=(1-tan(u(2))*h/L)^-1;
dvc_dalpha=ve*h/(L*(cos(u(2)))^2*(1-tan(u(2))*h/L));
aux=(cos(u(2))^(-2));
T1=a*sin(xp(3))+b*cos(xp(3));
T2=a*cos(xp(3))-b*sin(xp(3));
b1=(cos(xp(3))-tan(u(2))/L*T1)*dvc_dve;
b3=(sin(xp(3))+tan(u(2))/L*T2)*dvc_dve;
b5=tan(u(2))/L*dvc_dve;
b2=-T1*vc/L*aux+b1*dvc_dalpha;
b4=T2*vc/L*aux+b1*dvc_dalpha;
b6=vc/L*aux+tan(u(2))/L*dvc_dalpha;
Bv=dt*[b1 b2;
b3 b4;
b5 b6];
% state transition matrix evaluation (df/dx) -------------------------------
J1=[1 0 -dt*(vc*sin(xp(3))+vc/L*tan(u(2))*(a*cos(xp(3))-b*sin(xp(3))))
0 1 dt*(vc*cos(xp(3))-vc/L*tan(u(2))*(a*sin(xp(3))+b*cos(xp(3)))) %Jacobian
0 0 1 ];
I=eye(Pt-3,Pt-3);
% first state prediction -------------------------------------------------------
xp(1)=xp(1) + dt*vc*cos(xp(3))-dt*vc/L*tan(u(2))*(a*sin(xp(3))+b*cos(xp(3)));
xp(2)=xp(2) + dt*vc*sin(xp(3))+dt*vc/L*tan(u(2))*(a*cos(xp(3))-b*sin(xp(3)));
xp(3)=xp(3) + dt*vc/L*tan(u(2));
xp(3)=NormalizeAngle(xp(3));
%J*Pest*J' ---------------------------------------------------------------------
P1=Pp(1:3,1:Pt);
P2=Pp(4:Pt,1:Pt);
Aux=[J1*P1
I*P2];
Aux1=Aux(1:3,1:3); Aux2=Aux(1:3,4:Pt); Aux3=Aux(4:Pt,1:3); Aux4=Aux(4:Pt,4:Pt);
MatrixA(1:Pt,1:Pt)=[Aux1*J1' Aux2*I
Aux3*J1' Aux4*I];
clear Aux Aux1 Aux2 Aux3 Aux4;
%---------------------------------------------------------------------------------
%B*sigmaU*B'----------------------------------------------------------------------
MatrixB(1:Pt,1:Pt)=[Bv*sigmaU*Bv' zeros(3,Pt-3)
zeros(Pt-3,Pt) ];
%---------------------------------------------------------------------------------
Pp(1:Pt,1:Pt)= MatrixA + MatrixB;
return;
%---------------------------------------------------------------------------------------------------------------------------------------
%Update with GPS data
function [innov, S]=update_gps(zgps)
global xp Pp Pt
global sigma_gps;
H=zeros(2,Pt); H(1,1)=1; H(2,2)=1;
S=H*Pp(1:Pt,1:Pt)*H' + sigma_gps;
W=Pp(1:Pt,1:Pt)*H'* inv(S);
Pp(1:Pt,1:Pt)=Pp(1:Pt,1:Pt)-W*S*W';
innov=[zgps-H*xp(1:Pt)];
xp(1:Pt)=xp(1:Pt)+W*innov;
return;
%---------------------------------------------------------------------------------------------------------------------------------------
% This function perfor association or update accoding to the value of updatee
% updatee=0 -> association only, updatee=1 -> association and update
function [index,innov, S, q1]=asoc_update(zlaser,pointer,updatee)
global Pt;
global Pp xp Jh;
global sigma_laser;
beacon=[xp(pointer) xp(pointer+1)]; %Xi,Yi
dx=xp(1)-beacon(1);
dy=xp(2)-beacon(2);
d=(dx^2+dy^2)^0.5;
Jh(1:2,1:3)=[dx/d dy/d 0;
-dy/(d^2) dx/(d^2) -1];
Jh(1:2,4:Pt)=zeros(2,(Pt-3));
Jh(1:2,(pointer):(pointer+1))=[-dx/d -dy/d ; dy/(d^2) -dx/(d^2)];
H=[d ; atan2((beacon(2)-xp(2)),(beacon(1)-xp(1)))-xp(3) + pi/2]; %h(xpred)
H(2)=NormalizeAngle(H(2));
S=Jh(1:2,1:Pt)*Pp(1:Pt,1:Pt)*(Jh(1:2,1:Pt))' + sigma_laser;
innov=[zlaser-H];
innov(2)=NormalizeAngle(innov(2));
if updatee==1 %is an update, if this flag is zero is an asociation
W=Pp(1:Pt,1:Pt)*(Jh(1:2,1:Pt))'* inv(S);
Pp(1:Pt,1:Pt)=Pp(1:Pt,1:Pt)-W*S*W'; %Update of the error covariance matrix
xp(1:Pt)=xp(1:Pt)+W*innov;
xp(3)=NormalizeAngle(xp(3));
index=0; q1=0;
else
chi=5.99; %95% confidence
q=(innov')*(inv(S))*innov;
if (q<chi) %Chi square test
index=1;
q1=q+log(det(S));
else
index=0; q1=0;
end;
clear q W;
end
return;
%---------------------------------------------------------------------------------------------------------------------------------------
% Insert a new state assigning a large error ( simpler approach )
function new_state(zlaser)
global Pt xp Pp;
%First of all, calculate position beacon in the cartesian global coordiante.
xx = xp(1)+zlaser(1)*cos(zlaser(2)+xp(3)-pi/2) ;yy = xp(2)+zlaser(1)*sin(zlaser(2)+xp(3)-pi/2) ;
xp(Pt+1)=xx;
xp(Pt+2)=yy;
clear xx yy;
Pp(Pt+1,1:(Pt))=0; %x row
Pp(1:Pt,Pt+1)=0; %x column
Pp(Pt+1,Pt+1)=10^6; %x diagonal
Pp(Pt+2,1:(Pt+1))=0; %y row
Pp(1:Pt+1,Pt+2)=0; %y column
Pp(Pt+2,Pt+2)=10^6; %y diagonal ( this may introduce numerical problems if not choosen properly )
return;
%---------------------------------------------------------------------------------------------------------------------------------------
%Function to find index in data between TO and TF
function [t,I]=FINDT(Var,ttt)
ii=find(Var>=ttt);
I=ii(1);
t=Var(I);
return;
%---------------------------------------------------------------------------------------------------------------------------------------
%function for the on-line plot
function fOnOff(x)
global FlagWait FlagEnd;
if x==1, FlagWait=~FlagWait ; return ; end ;
if x==2, FlagEnd=1 ; return ; end ;
return ;
%---------------------------------------------------------------------------------------------------------------------------------------
%Transform GPS lat and Long to local navigation frame centred at a reference pt
function [GPSTIME,LONG,LAT] =ReadGpsData(file)
load(file) ;
file;
LONG = GPS(:,4)' ;
LAT = -GPS(:,3)' ; %We are in the south, the latitude is negative
GPSTIME = GPS(:,1)'/1000 ;
LAT0 = -33.8884; %put any point on the map, is not a good idea to put "magic numbers" (as in this case), the best
LONG0 = 151.1948; %would be to take the average of the initial points
a = 6378137.0; b = a*(1-0.003352810664747); %Linalization from latitude and long. to meters in a local area
kpi = pi/180 ;
cf = cos(LAT0*kpi) ; sf = abs(sin(LAT0*kpi)) ;
Ro = a*a*cf/abs(sqrt(a*a*cf*cf + b*b*sf*sf)) ;
RR = b/a * abs(sqrt( a*a-Ro*Ro))/sf ;
LAT =(LAT - LAT0 )*RR*kpi ;
LONG=(LONG- LONG0)*Ro*kpi ;
return ;
%-----------------------------------------------------------------------------------------------------------------------------------------
%read steering data, not used in this case
function [Time,STEERING,SPEED1] = ReadUteData(file)
load(file) ;
STEERING = SENSORS(:,4)' ;
SPEED1 = SENSORS(:,6)' ;
Time = SENSORS(:,1)'/1000 ;
return ;
%-----------------------------------------------------------------------------------------------------------------------------------------
%This function perform the estimation of the beacon centre, It can be improved
%There is a more general version: detectrees that works well for all cylindrical
%objects
function [LASERr,LASERo,RR,a]=getdata(laser,intensity)
LASERr=[];
LASERo=[];
first=0;
max_range=30; min_range=1;
angleDiff=3;
RR=laser; a=intensity;
for i=1:361
if (min_range<RR(i)<max_range) & (a(i)>0)
primera=0;
last=[RR(i),i];
if first==0
init=[RR(i),i];
first=1;
end
else
if first==1
if primera==0
primera=1;
else
if (i-last(2))>2
first=0;
range=mean([init(1),last(1)]);
angle=mean([init(2),last(2)]);
LASERr=[LASERr range];
LASERo=[LASERo (angle-1)/2*pi/180];
end
end
end
end
end
return;
%---------------------------------------------------------------------------------------------------------------------------------
%Is looking for the beacons that are "min_dist" close to te observation
% in this case it is set to 3 meters. Modify if necesary
function [closest]=Zone_Probe(zlaser);
global Pt xp;
min_dist=3;
ii=[4:2:Pt-1];
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
d=((xx-xp(ii)).^2+(yy-xp(ii+1)).^2).^0.5;
iii=find(d<min_dist);
closest=iii;
clear xx yy;
return;
return;
%---------------------------------------------------------------------------------------------------------------------------------
%Plot the laser scan
function laserview(RR,a,LASERr,LASERo)
global xp hhh;
global isave xest tglobal trefresh;
aa = [0:360]*pi/360 ;
ii=find(RR<50 & RR>1) ;
aa2=aa(ii) ; xx = RR(ii).*cos(aa2+xp(3)-pi/2) ;yy = RR(ii).*sin(aa2+xp(3)-pi/2) ; %All points
set(hhh(3),'XData',xx+xp(1),'YData',yy+xp(2));
pause(0.01);
ii=find(a>0) ;
aa2=aa(ii) ; xx = RR(ii).*cos(aa2+xp(3)-pi/2) ;yy = RR(ii).*sin(aa2+xp(3)-pi/2) ; %High intensity points
set(hhh(4),'XData',xx+xp(1),'YData',yy+xp(2));
pause(0.01);
ll = length(LASERr) ;
if ll>0,
xx = LASERr.*cos(LASERo+xp(3)-pi/2) ; %The points we are taking from one frame
yy = LASERr.*sin(LASERo+xp(3)-pi/2) ;
set(hhh(9),'XData',xx+xp(1),'YData',yy+xp(2));
index=[1:3:3*ll];
xpoints=zeros(3*ll,1); ypoints=zeros(3*ll,1); %this is to plot the lines between the beacons and the car
xpoints(index)=xp(1); ypoints(index)=xp(2);
xpoints(index+1)=xx+xp(1); ypoints(index+1)=yy+xp(2);
xpoints(index+2)=NaN;ypoints(index+2)=NaN;
set(hhh(2),'XData',xpoints,'YData',ypoints)
pause(0.01);
else
set(hhh(2),'XData',0,'YData',0)
end ;
if (tglobal-trefresh)>2 %every "trefresh" seconds is doing s refresh of the whole path
set(hhh(1),'XData',xest(1,1:isave-1),'YData',xest(2,1:isave-1))
trefresh=tglobal;
end
return;
%---------------------------------------------------------------------------------------------------------------------------------
% saving the state
function SaveStates(states,diagcov,times,Flag)
global isave xest Pest TimeGlobal Pt FlagS;
xest(:,isave)=states;
Pest(1:Pt,isave)=diagcov;
TimeGlobal(1,isave)=times;
FlagS(1,isave)=Flag;
isave=isave+1;
return;
%----------------------------------------------------------------------------------------------------------------------------------
%saving innovations
function SaveInnov(in,ins);
global innov innvar index_update;
innov(:,index_update)=in;
innvar(:,index_update)=ins;
index_update=index_update+1;
return;
%----------------------------------------------------------------------------------------------------------------------------------
%transform angles to -pi to pi
function ang2 = NormalizeAngle(ang1)
if ang1>pi, ang2 =ang1-2*pi ; return ; end ;
if ang1<-pi, ang2 =ang1+2*pi ; return ; end ;
ang2=ang1;
return
%----------------------------------------------------------------------------------------------------------------------------------
%plot 1-sigma uncertainty region for a P covariance matrix.
% Jose-1999
function xxx=ellipse(X0,P,veces,color,figu)
R = chol(P)'; % R*R'= P, X = R*X2
r = 1 ; %circle's radius in space X2
aaa = [0:veces]*2*pi/veces ; % sample angles
xxx = [ r*cos(aaa) ; r*sin(aaa) ] ; % polar to x2,y2
xxx = R*xxx ; % x2,y2 to x,y
xxx(1,:) = X0(1)+xxx(1,:); % reference to center X0
xxx(2,:) = X0(2)+xxx(2,:);
return;
%----------------------------------------------------------------------------------------------------------------------------------
function Rxx=auto(x) %This is used just for the plot
[N,nul]=size(x');
M=round(N/2);
Xpsd=fft(x);
Pxx=Xpsd.*conj(Xpsd)/N;
% the inverse is the autocorrelation
Rxx=real(ifft(Pxx));
Rxx=Rxx(1:M);
fact=Rxx(1);
for i=1:M
Rxx(i)=Rxx(i)/fact;
end
return
%---------------------------------------------------------------------------------------------------------------------------------
%Include all your off-line plots here ( runs when finish )
function plots
global xest Pest GPSLon GPSLat beacons;
global innov innvar;
global FlagS TimeGlobal estbeacons plotall;
ii=find(FlagS==1);
timeinnov=TimeGlobal(ii); %Innovations time stamps
figure(1);clf
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;
legend('Estimated','Est. Sample','Est. Beac.','Beacons','GPS')
xlabel('East Meters')
ylabel('North Meters')
title('Path')
if plotall
figure(2);clf
plot(xest(1,:),xest(2,:),'r');grid on;
xlabel('East Meters')
ylabel('North Meters')
title('Path Estimated')
figure(3);clf
hold on
axis([timeinnov(1) timeinnov(size(timeinnov,2)) -0.5 0.5]);grid on;
plot(timeinnov(1,:),innov(1,:)), title('Zr Innovations')%
plot(timeinnov(1,:),2*sqrt(innvar(1,:)),'r')
plot(timeinnov(1,:),-2*sqrt(innvar(1,:)),'r')
xlabel('Time(secs)');ylabel('Desviation(m)');
legend('Innovations','Sta. Desv. Inn. (95%)')
hold off
figure(4);clf
Rxx1=auto(innov(1,:));
Rxx2=auto(innov(2,:));
M=round(size(innov,2)/2);
bounds=2*sqrt(1/(M));
plot([1:M],Rxx1,'b',[1:M],Rxx2,'r',[1:M],bounds,'g',[1:M],-bounds,'g');grid on;
title('Autocorrelation of Innovation Sequence')
legend('Var. Zr','Var. Ztheta','95% Confi. Bounds')
figure(5);clf
hold on
axis([TimeGlobal(1) TimeGlobal(size(TimeGlobal,2)) 0 1]);grid on;
plot(TimeGlobal,sqrt(Pest(1,:)),'b')
plot(TimeGlobal,sqrt(Pest(2,:)),'r')
plot(TimeGlobal,sqrt(Pest(3,:)),'g')
xlabel('Time(secs)');ylabel('Desviation(m)');
title('Model Covariance');
legend('X var.','Y var.','Steer.')
hold off
figure(6);clf
hold on
axis([TimeGlobal(1) TimeGlobal(size(TimeGlobal,2)) 0 1]);grid on;
plot(TimeGlobal,sqrt(Pest(4,:)),'b'); plot(TimeGlobal,sqrt(Pest(5,:)),'b.')
plot(TimeGlobal,sqrt(Pest(8,:)),'r'); plot(TimeGlobal,sqrt(Pest(9,:)),'r.')
plot(TimeGlobal,sqrt(Pest(12,:)),'g'); plot(TimeGlobal,sqrt(Pest(13,:)),'g.')
plot(TimeGlobal,sqrt(Pest(16,:)),'m'); plot(TimeGlobal,sqrt(Pest(17,:)),'m.')
xlabel('Time(secs)');ylabel('Desviation(m)');
title('Beacons Covariances');
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)')
hold off
end
return;
%---------------------------------------------------------------------------------------------------------------------------------
function plotCovariance
global hhh xp Pp beacon2show Pt;
xxx=ellipse(xp(1:2),Pp(1:2,1:2),50); set(hhh(5),'XData',xxx(1,:),'YData',xxx(2,:)) ; %Position
if Pt>=(3+2*beacon2show(2))
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);
set(hhh(6),'XData',xxx(1,:),'YData',xxx(2,:)) ;
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);
set(hhh(7),'XData',xxx(1,:),'YData',xxx(2,:)) ;
end
return;
%---------------------------------------------------------------------------------------------------------------------------------
D61
1105
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
