% t1=V1data(:,1);   %time
% %V1data(:,2)=1;    %VehID
% N1ego=V1data(:,3);   % this is where Robot1 thinks it is (from its TRN filter)
% E1ego=V1data(:,4);
% Z1ego=V1data(:,5);
% N1var=V1data(:,6);  % this is the uncertainty in Robot1's estimate of its own position
% E1var=V1data(:,7);
% Z1var=V1data(:,8);
% R1mod=V1data(:,9);  % The range to Veh 3   (all zeros except at time points where real data exist)
% H1=V1data(:,10); % The bearing to Veh 3
% R1var=V1data(:,11);  %R1var=.03*range
% H1var=V1data(:,12);  % The measurement uncertainty in the bearing
% measurement

% t3=VEGOdata(:,1);   %time
% VEGOdata(:,2)=3;    %VehID
% N3nav=VEGOdata(:,3);  % These Vehicle3's AHRS position estimate (available in its LCM logs
% E3nav=VEGOdata(:,4);
% Z3nav=VEGOdata(:,5);
% Nprocessnoise=VEGOdata(:,6);  %  .05m = process noise sigma on NEZ
% Eprocessnoise=VEGOdata(:,7);
% Zprocessnoise=VEGOdata(:,8);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  These are the data that will come from the lcm logs
        V1data=csvread('Vehicle1log.csv');
        V2data=csvread('Vehicle2log.csv');
        VEGOdata=csvread('VehicleEGOlog.csv');

        t1=V1data(:,1);   %time
        t2=V2data(:,1);      
        t3=VEGOdata(:,1); 

        tclock=t3;   % use the ego vehicles time as the clock

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% initialize arrays for plotting outputs
[n,dum]=size(VEGOdata);

        tout=zeros(n,1);
        N31out=zeros(n,1);
        E31out=zeros(n,1);
        N31out=zeros(n,1);
        E32out=zeros(n,1);
        N32out=zeros(n,1);
        E32out=zeros(n,1);

        E1hitout=zeros(n,1);
        N1hitout=zeros(n,1);
        E2hitout=zeros(n,1);
        N2hitout=zeros(n,1);
        N3bestout=zeros(n,1);
        E3bestout=zeros(n,1);
        E3egoout=zeros(n,1);
        N3egoout=zeros(n,1);


        sigmaN1=zeros(n,1);
        sigmaE1=zeros(n,1);
        sigmaN2=zeros(n,1);
        sigmaE2=zeros(n,1);
        sigmaNbest=zeros(n,1);
        sigmaEbest=zeros(n,1);

        P31out=zeros(2,2,n);
        P32out=zeros(2,2,n);
        Pbestout=zeros(2,2,n);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Initialize variables
        Nic=VEGOdata(1,3);
        Eic=VEGOdata(1,4);
        N31=Nic;  %conditions at t=1
        E31=Eic;
        N32=Nic;
        E32=Eic;
        N3ego=Nic;
        E3ego=Eic;

        P31=10000*eye(2);  %initialze state covariances to a large value
        P32=10000*eye(2);
        Pbest=10000*eye(2);
        
        timelast=VEGOdata(1,1);
        N3navlast=Nic;
        E3navlast=Eic;
        sigmathreshold=7;   %threshold for declaring a filter has converged
        Qsig=.03;  %nominal process noise = Qsig*(distancetravelled)^2 * eye(2)
        
        %these variables are just for plotting
        N31out(1)=N31;
        E31out(1)=E31;
        N32out(1)=N32;
        E32out(1)=E32;
        N3egoout(1)=Nic;
        E3egoout(1)=Eic;
        N3bestout(1)=Nic;
        E3bestout(1)=Eic;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Every clock tick 

for i=2:n

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  Collect data about your own (EGO) vehcile
         time=VEGOdata(i,1);   %time
         N3nav=VEGOdata(i,3);  % Read lcm log to find AHRS position
         E3nav=VEGOdata(i,4);

% Calculate process update (delta motion input) that will be used by all filters on this vehicle
        deltaN=N3nav-N3navlast;   
        deltaE=E3nav-E3navlast;   
        N3navlast=N3nav;
        E3navlast=E3nav;
        
% Process update (motion) of the two MR filters
        N31=N31+deltaN;
        E31=E31+deltaE;
        N32=N32+deltaN;
        E32=E32+deltaE;
        
%Modify the process noise for each MR filter to deal with correlation
        distanceTravelled=sqrt(deltaN^2+deltaE^2);
        Q=(Qsig*distanceTravelled)^2 * eye(2);   %Process noise covariance of a normal KF
        [Q1bar,Q2bar]= CalculateQbar(Q, P31, P32, Pbest);  

% Process update of the MR filter covariances
        P31=P31+Q1bar;   %Process update of the state covariance
        P32=P32+Q2bar;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% check for meas update 

 % MeasUpdate Pontus hit (veh1) 
        C1a=and(t1(:,1)>timelast,t1(:,1)<=time);   %did you get a message from Veh1 during this interval
        C1=and(C1a,V1data(:,9)>50);                % check to see if the range measurment is >50
        CC1=sum(C1)==1;
        CC2=(sqrt(V1data(C1,6)+V1data(C1,7)))<=sigmathreshold;   % check to see if the sending vehicle's TRN filter was converged
        if and(CC1,CC2)      % verify Pontus is converged.  If so, you have a valid hit 
            
            
            tmeas=t1(C1);
            f=(time-tmeas)/(time-timelast);  
            Nmeas=N31-f*deltaN;
            Emeas=E31-f*deltaE;

            % Collect all data sent from the Vehicle 1 message
            Nj=V1data(C1,3);                  
            Ej=V1data(C1,4);
            Njvar=V1data(C1,6);
            Ejvar=V1data(C1,7);
            Bearing=V1data(C1,10);
            Range=V1data(C1,9);
            Rangesig=V1data(C1,11);  %rangesigma=3% of range  RangeSigma=Rangesig*Range
            RangeSigma=Rangesig*Range;     
            BearingSigma=V1data(C1,12);  
            
            %  Generate the delta measurement
            [deltaz,Rij,Hi,Hj,Pj] = Range_Bearing(Range,Bearing,RangeSigma,BearingSigma,Nmeas,Emeas,Nj,Ej,Njvar,Ejvar);

            % Do the Kalman or CI filter update
            Pij=P31;
            NF=N31;
            EF=E31;
            [N31f,E31f,P31f,Pa,Pb] = Filterfun(NF,EF,deltaz,Pij,Rij,Hi,Hj,Pj);
            N31=N31f;
            E31=E31f;
            P31=P31f;      
            P31last=P31;

        end  

% MeasUpdate Tethys hit (veh2)
         C2a=and(t2(:,1)>timelast,t2(:,1)<=time);
         C2=and(C2a,V2data(:,9)>50);
         CC3=sum(C2)==1;
         CC4=(sqrt(V2data(C2,6)+V2data(C2,7)))<=sigmathreshold;
         if and(CC3,CC4)       % verify good hit
     
            % interpolate N32 and E32 back to meas time for the deltaz calcuation
            tmeas=t2(C2);
            f=(time-tmeas)/(time-timelast);
            Nmeas=N32-f*deltaN;
            Emeas=E32-f*deltaE;   

            % Collect all data sent from the Vehicle 2 message
            Nj=V2data(C2,3);                  
            Ej=V2data(C2,4);
            Njvar=V2data(C2,6);
            Ejvar=V2data(C2,7);
            Bearing=V2data(C2,10);
            Range=V2data(C2,9);
            Rangesig=V2data(C2,11);  %rangesigma=3% of range  RangeSigma=Rangesig*Range
            RangeSigma=Rangesig*Range;     
            BearingSigma=V2data(C2,12);  %  

            %  Generate the delta measurement
            [deltaz,Rij,Hi,Hj,Pj] = Range_Bearing(Range,Bearing,RangeSigma,BearingSigma,Nmeas,Emeas,Nj,Ej,Njvar,Ejvar);   

            % Do the Kalman or CI filter update
            Pij=P32;
            NF=N32;
            EF=E32;
            [N32,E32,P32,Pa,Pb] = Filterfun(NF,EF,deltaz,Pij,Rij,Hi,Hj,Pj);
            P32last=P32;
        
        end
      
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Form the best estimate  (Note, this just merges the 2 MR filters.
%  For the test case we are running, the Ego filter didn't converge, so
%  it is beging ignored here.  Eventually, we'll add the Ego position and
%  covariance to the CombineFilters script.  We have a choice, we can use
%  the mmse estimate of position and covariance, or we can use the particle
%  cloud.  


        [N3best,E3best,Pbest] = CombineFilters(N31,E31,N32,E32,P31,P32);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        timelast=time;


% save some stuff for plotting
 
        N31out(i)=N31;
        E31out(i)=E31;
        N32out(i)=N32;
        E32out(i)=E32;
        N3bestout(i)=N3best;
        E3bestout(i)=E3best;

        P31out(:,:,i)=P31;
        sigmaN1(i,1)=sqrt(P31(1,1));
        sigmaE1(i,1)=sqrt(P31(2,2));

        P32out(:,:,i)=P32;
        sigmaN2(i,1)=sqrt(P32(1,1));
        sigmaE2(i,1)=sqrt(P32(2,2));

        Pbestout(:,:,i)=Pbest;
        sigmaNbest(i,1)=sqrt(Pbest(1,1));
        sigmaEbest(i,1)=sqrt(Pbest(2,2));
 
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%



tc=tclock-tclock(1)*ones(n,1);

load E31N31  %This is a reference for comparison
figure(1)
plot(E31out,N31out,'k',E31outsave,N31outsave,'r')
title('Northings and Eastings of MR filter #1')

figure(2)
plot(E32out,N32out,'k')
title('Northings and Eastings of MR filter #2')

figure(3)
plot(E3bestout,N3bestout,'k',E31out,N31out,'g',E32out,N32out,'c')
title('Northings and Eastings of CoNav result (k), MR#1 (g), MR#2 (c)')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
