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;