%---------------------------------------------------------------------------------------------------------------------------------------    
%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;
%---------------------------------------------------------------------------------------------------------------------------------