%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% How to use this class:
% 1. Initialize a robot object:
%    call LRAUVDataClass(id,nRob,particles,t) with
%    - id: unique ID number for this vehicle (eg. 2)
%    - nRob: total number of robots in the system (eg. 4)
%    - particles: array of all particles output by TRN. This is a T*Np X 5
%                 array, where T is the number of timesteps, Np is the
%                 number of particles. Each row is particle number, weight,
%                 N, E, D
%    - t: Tx1 vector of times that correspond to the particle increments
%     
% 2. Add inertial data
%    save positions to obj.posKft, and inertial timestamps to obj.tKft
% 
% 3. Add truth data (if available)
%    save to obj.truth
% 
% 4. Add relative measurements to one other vehicle
%    Call obj.addRelative(t_rh,R,Rtruth,H,Htruth,Rthresh) with
%    - t_rh: Vector of times for range and heading measurements
%    - R: Vector of range measurements
%    - Rtruth: (optional) vector of true ranges
%    - H: Vector of heading measurements
%    - Htruth: (optional) vector of true headings
%    - Rthresh: (optional) if Rtruth was provided, filters out measurements
%                with an error above Rthresh
%
%    Optional: call resetParticles to override TRN and keep particles as a
%    grid
%
% 5. Call obj.generateMessages(target) to build structures to send to the
%    other vehicle (target)
%
% 6: repeat 4-5 with other target vehicles (if more measurements available)
%
% 7. Repeat 1-6 with other robots
%
% 8. Step through time, calling obj.MRProcessUpdate(t) to perform process
%    update at each timestep, and obj.MultiRobotUpdate(data,t) every time
%    another vehicle has a measurement available for this vehicle. Call
%    getBestEst() to get the multi-robot estimate of position. Plot() also
%    availabel to visualize results
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


classdef LRAUVDataClass < handle
    properties
        id % Number of this vehicle
        nRob % Total number of vehicles
        Np = 10000; % Number of particles
        dim = 2; % Number of dimensions for Cooperative TRN to aid
        
        part % cell 1xT, each entry 3xNp
        wt % T x Np
        mu % 3 x T
        Sigma % cell 1xT, each entry 3x3
        procNoise = 3; % * percent distance travelled = sigma
        
        muComm % CI filter means (cell nRob x T)
        SigmaComm % CI filter covariances (cell nRob x T)
        bestEst % best estimate of position at each timestep (3 x T)
        bestSig % best estimate covariance at each timestep (cell 1xT)
        tTRN % timesteps Tx1 
        truth % true position if available
        
        range % range measurements
        tRange % range measurement timestamps
        sigmaRange % noise on range measurements
        
        heading % heading measurements
        tHeading % heading measurement timestamps
        sigmaHeading % noise on heading measurements
        
        messages % message structure (data to pass to other vehicles)
        
        posKft % position from inertial nav (usually higher rate)
        tKft % timestamps of inertial nav
        
        convergedThresh = 15; % convergence threshold to transmit to other vehicles
    end
    
    methods
        %%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Constructor to initialize a vehicle                             %
        % takes an ID number for this vehicle, the total number of        %
        % vehicles, an array of particles and weights from TRN, and the   %
        % corresponding timestamps for the particles.                     %
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        
        function obj = LRAUVDataClass(id,nRob,particles,t)
           
            obj.id = id;
            obj.nRob = nRob;
            
            % interpret the particle array and transform it into a cell
            obj.addParticles(particles);
            obj.tTRN = t;
            
            % initialize all other variables
            obj.muComm = cell(nRob,length(obj.tTRN));
            obj.SigmaComm = cell(nRob,length(obj.tTRN));
            obj.bestEst = zeros(3,length(obj.tTRN));
            obj.bestSig = cell(1,length(obj.tTRN));
            
            obj.messages = {};
        end
        
        %%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Interpret the particle array, turn it into a cell array for     %
        % each timestep                                                   %
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        function addParticles(obj,particles)
            maxt = size(particles,1)/obj.Np; % get the length of the time vector
            obj.part = cell(1,maxt); % initialize the cell array
            obj.wt = zeros(maxt,obj.Np); % initialize the weight vectors
            
            t = 1;
            partPrev = 0;
            obj.part{t} = zeros(3,obj.Np);
            for i=1:size(particles,1) % iterate through the particle array
                partnum = particles(i,1)+1;
                if partnum < partPrev % if the particle number has reset, advance time
                    t = t+1;
                    obj.part{t} = zeros(3,obj.Np); % initialize next particle set
                end
                
                % copy over particle data
                obj.part{t}(:,partnum) = particles(i,3:5)';
                obj.wt(t,partnum) = particles(i,2); % copy weights
                partPrev = partnum;
            end
            
            % Find the best-fit Gaussian at each timestep
            obj.mu = zeros(3,maxt);
            obj.Sigma = cell(1,maxt);
            for t = 1:maxt
                partT = obj.part{t}; % get the particles at this time
                wtT = obj.wt(t,:); % get the weights
                obj.mu(:,t) = partT*wtT'; % find the MMSE
                
                Sig = zeros(3,3); % calculate the covariance
                for i=1:obj.Np
                    Sig = Sig + wtT(i)*(partT(:,i)-obj.mu(:,t))*(partT(:,i)-obj.mu(:,t))';
                end
                obj.Sigma{t} = Sig; % store it
            end
        end
        
        %%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Add relative measurements to the class.                         %
        % Requires the time vector, range measurements, and heading       %
        % measurements. If truth data is also provided for range and      %
        % heading, can also filter the measurements to only keep those    %
        % belose a threshold                                              %
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        function addRelative(obj,t_rh,R,Rtruth,H,Htruth,Rthresh)
            if ~isempty(Rtruth) % if truth data was provided
                % keep data with positive range and within tolerance
                inds = find(R > 0 & abs(R-Rtruth) < Rthresh);
                R = R(inds); % throw out bad measurements
                Rtruth = Rtruth(inds);
                Rcorrection = mean(R-Rtruth); % calculate the measurement bias
                obj.range = R - Rcorrection; % adjust the measurements
                obj.tRange = t_rh(inds); % store the time vector
                obj.sigmaRange = std((obj.range-Rtruth)./Rtruth); % calculate std dev
            else % no truth data was provided
                inds = find(R > 0); % keep data with positive values
                R = R(inds);
                obj.range = R; % store range
                obj.tRange = t_rh(inds); % store time vector
                obj.sigmaRange = 4; % assume sigma is 4 percent of range
            end
            
            if ~isempty(Htruth) % if truth data was provided
                H = H(inds); % only keep headings that have a corresponding range (not necessary)
                Htruth = Htruth(inds); 
                Hcorrection = mean(H-Htruth); % calculate the bias
                obj.heading = H - Hcorrection; % correct the measurements
                obj.tHeading = t_rh(inds); % store the time vector
                obj.sigmaHeading = std(obj.heading-Htruth); % calculate the covariance
            else
                H = H(inds); 
                obj.heading = H; % store the measurements
                obj.tHeading = t_rh(inds); % store the time vector
                obj.sigmaHeading = deg2rad(7.5); % assume the noise is 7.5deg std dev
            end
        end
        
        %%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Builds the message structure to send to the target vehicle      %
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        function generateMessages(obj,target)
            obj.messages = {};
            rangeInd = 1;
            headingInd = 1;

            % run through time (fine timesteps)
            for i=1:length(obj.tKft)
                % if it's time for the next range measurement
                if rangeInd <= length(obj.tRange) && obj.tKft(i) >= obj.tRange(rangeInd)
                    % get the corresponding TRN time
                    t = find(obj.tTRN >= obj.tRange(rangeInd),1,'first');
                    
                    % build the data structure
                    data.id = obj.id;
                    data.otherId = target;
                    % interpolate the TRN position to the correct timestep
                    [~,dPos2] = obj.interpolateBetweenTRN(obj.tRange(rangeInd));
                    data.mu = obj.mu(:,t) - dPos2; % adjust the position
                    data.Sigma = obj.Sigma{t};
                    data.time = obj.tRange(rangeInd);
                    
                    % copy the measurement into the structure
                    data.rangeMeas = obj.range(rangeInd);
                    data.Rrange = obj.sigmaRange^2;
                    
                    % if there's a corresponding heading measurement
                    if obj.tKft(i) >= obj.tHeading(headingInd)
                        % add the heading measurement
                        data.headingMeas = obj.heading(headingInd);
                        data.Rheading = obj.sigmaHeading^2;
                        headingInd = headingInd + 1;
                    else % leave heading empty
                        data.headingMeas = [];
                        data.Rheading = [];
                    end
                    rangeInd = rangeInd + 1;
                    obj.messages{target,end+1} = data;
                    
                    % if it's time for the next heading measurement
                elseif headingInd <= length(obj.tHeading) && obj.tKft(i) >= obj.tHeading(headingInd)
                    % get the corresponding TRN time
                    t = find(obj.tTRN >= obj.tHeading(headingInd),1,'first');
                    
                    % build the structure
                    data.id = obj.id;
                    data.otherId = target;
                    % interpolate the TRN position to the correct timestep
                    [~,dPos2] = obj.interpolateBetweenTRN(obj.tHeading(headingInd));
                    data.mu = obj.mu(:,t) - dPos2;
                    data.Sigma = obj.Sigma{t};
                    data.time = obj.tHeading(headingInd);
                    
                    % add the heading measurement
                    data.headingMeas = obj.heading(headingInd);
                    data.Rheading = obj.sigmaHeading^2;
                    
                    % in this case, we know there's no corresponding range
                    % measurement (would be handled above)
                    data.rangeMeas = [];
                    data.Rrange = [];
                    
                    headingInd = headingInd + 1;
                    obj.messages{target,end+1} = data;
                end
            end
        end
        
        %%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Reset all particles within a time window. Sets a 200m x 200m    %
        % x 15m box for the particles and overrides the actual TRN data   %
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        function resetParticles(obj,tStart,tEnd)
            partWidths = [200;200;15]; % box for the particles
            [~,tk] = min(abs(obj.tKft - obj.tTRN(tStart))); % get the timesteps
            % generate random particle positions
            obj.part{tStart} = (rand(size(obj.part{tStart})) - 0.5).*partWidths + obj.posKft(:,tk);
            for t = tStart:tEnd
                % clear the weights
                obj.wt(t,:) = 1/obj.Np;
                obj.mu(:,t) = obj.part{t}*obj.wt(t,:)'; % calculate the mmse
                
                % recalculate the best-fit Gaussian
                Sig = zeros(3,3);
                for i=1:obj.Np
                    Sig = Sig + obj.wt(t,i)*(obj.part{t}(:,i)-obj.mu(:,t))*(obj.part{t}(:,i)-obj.mu(:,t))';
                end
                obj.Sigma{t} = Sig;
                
                % process update
                if t < tEnd
                    p1 = (interp1(obj.tKft,obj.posKft',obj.tTRN(t)))';
                    p2 = (interp1(obj.tKft,obj.posKft',obj.tTRN(t+1)))';
                    dPos = p2-p1;
                    Q = (obj.procNoise*0.01*norm(dPos))^2*eye(3);
                    %                     dPos = obj.truth(:,t+1) - obj.truth(:,t);
                    obj.part{t+1} = obj.part{t} + dPos + chol(Q)*randn(3,obj.Np);
                end
            end
        end
        
        %%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Reduce the dataset to only include a specified timespan         %
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        function reduceMission(obj,tStart,tEnd)
            
            % TRN time units:
            iStart = find(obj.tTRN >= tStart,1);
            iEnd = find(obj.tTRN <= tEnd,1,'last');
            newPart = cell(1,iEnd-iStart+1);
            newSig = cell(1,iEnd-iStart+1);
            muCommNew = cell(obj.nRob,iEnd-iStart+1);
            SigmaCommNew = cell(obj.nRob,iEnd-iStart+1);
            bestSigNew = cell(1,iEnd-iStart+1);
            for i=1:length(newPart)
                newPart{i} = obj.part{iStart+i-1};
                newSig{i} = obj.Sigma{iStart+i-1};
                bestSigNew{i} = obj.bestSig{iStart+i-1};
                for r=1:obj.nRob
                    muCommNew{r,i} = obj.muComm{r,iStart+i-1};
                    SigmaCommNew{r,i} = obj.SigmaComm{r,iStart+i-1};
                end
            end
            obj.part = newPart;
            obj.Sigma = newSig;
            obj.muComm = muCommNew;
            obj.SigmaComm = SigmaCommNew;
            obj.bestSig = bestSigNew;
            
            obj.wt = obj.wt(iStart:iEnd,:);
            obj.mu = obj.mu(:,iStart:iEnd);
            obj.bestEst = obj.bestEst(:,iStart:iEnd);
            obj.truth = obj.truth(:,iStart:iEnd);
            obj.tTRN = obj.tTRN(iStart:iEnd);
            
            % KFT time units:
            iStart = find(obj.tKft >= tStart,1);
            iEnd = find(obj.tKft <= tEnd,1,'last');
            obj.posKft = obj.posKft(:,iStart:iEnd);
            obj.tKft = obj.tKft(iStart:iEnd);
            
            %Range time units:
            iStart = find(obj.tRange >= tStart,1);
            iEnd = find(obj.tRange <= tEnd,1,'last');
            obj.range = obj.range(iStart:iEnd);
            obj.tRange = obj.tRange(iStart:iEnd);
            
            % Heading time units
            iStart = find(obj.tHeading >= tStart,1);
            iEnd = find(obj.tHeading <= tEnd,1,'last');
            obj.heading = obj.heading(iStart:iEnd);
            obj.tHeading = obj.tHeading(iStart:iEnd);
        end
        
        % for a timestep in between TRN times, find the delta position to
        % the two nearest TRN times
        function [dPos1,dPos2] = interpolateBetweenTRN(obj,t)
            posKft2 = (interp1(obj.tKft,obj.posKft',t))';
            indTRNprev = find(obj.tTRN <= t,1,'last');
            posKft1 = (interp1(obj.tKft,obj.posKft',obj.tTRN(indTRNprev)))';
            dPos1 = posKft2 - posKft1;
            
            indTRNnext = find(obj.tTRN >= t, 1, 'first');
            posKft3 = (interp1(obj.tKft,obj.posKft',obj.tTRN(indTRNnext)))';
            dPos2 = posKft3 - posKft2;
        end
        
        %%%%%%%%%% Cooperative TRN methods: %%%%%%%%%
        
        %%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Merge the filters at time t to find a best estimate of position %
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        function [posEst,Sig] = getBestEst(obj,t)
            d = obj.dim;
            % merge the Gaussians
            [muCombined,SigmaCombined] = obj.combineEKFs(t); 
            if isempty(muCombined) % if there are no multi-robot filters
                posEst = obj.mu(:,t);
                Sig = obj.Sigma{t};
            else % if there are multi-robot filters
                reweight = zeros(1,obj.Np); % create new weight vector
                for i=1:obj.Np % iterate through particles, calculate weight
                    SigmaCombined = SigmaCombined(1:d,1:d);
                    reweight(i) = obj.wt(i)*exp(-0.5*((obj.part{t}(1:d,i)-muCombined)'/SigmaCombined)*(obj.part{t}(1:d,i)-muCombined));
                end
                reweight = reweight/sum(reweight); % normalize weight
                posEst = obj.part{t}*reweight'; % calculate MMSE
                
                Sig = zeros(3,3); % calculate approximate Gaussian
                for i=1:obj.Np
                    Sig = Sig + reweight(i)*(obj.part{t}(:,i)-posEst)*(obj.part{t}(:,i)-posEst)';
                end
            end
        end
        
        %%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Merge the Gaussian filters (multi-robot filters)                %
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        function [muCombined,SigmaCombined] = combineEKFs(obj,t)
            d = obj.dim;
            Icomb = zeros(d,d); % information form of covariance
            ycomb = zeros(d,1); % information form of mean
            foundFlag = 0;
            for i=1:obj.nRob
                if ~isempty(obj.muComm{i,t})
                    Pinv = inv(obj.SigmaComm{i,t}(1:d,1:d)); % inverse of covariance
                    Icomb = Icomb + Pinv; % information meas update
                    ycomb = ycomb + Pinv*obj.muComm{i,t}(1:d);
                    foundFlag = 1; % we found at least one multi-robot filter
                end
            end
            if foundFlag % if there was a multi-robot filter, return it
                SigmaCombined = inv(Icomb);
                muCombined = SigmaCombined*ycomb;
            else % return empty set
                muCombined = [];
                SigmaCombined = [];
            end
        end
        
        %%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Plot the particles, mean, and true position (if known)
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        function h = plot(obj,t,h,color,criticalRob)
            if nargin < 3
                h = [];
            end
            colors = [68,119,170;102,204,238;34,136,51;220, 197, 51;238,102,119;170,51,119;187,187,187]/256;
            holdstatus = ishold;
            hold on
            posEst = obj.getBestEst(t);
            if nargin < 3
                if ~isempty(obj.truth)
                    hand1 = plot(obj.truth(1,t),obj.truth(2,t),'ko');
                else
                    hand1 = [];
                end
                hand2 = plot(obj.part{t}(1,:),obj.part{t}(2,:),'r.');
                hand3 = plot(posEst(1),posEst(2),'bx');
                hand4 = plot(obj.mu(1,t),obj.mu(2,t),'kd');
            else
                if ~isempty(obj.truth) && obj.id == criticalRob
                    hand1 = plot3(obj.truth(1,t),obj.truth(2,t),-100,'Marker','*','Color',colors(5,:),'LineWidth',2,'MarkerSize',8);
                else
                    hand1 = [];
                end
                hand2 = plot3(obj.part{t}(1,:),obj.part{t}(2,:),-99*ones(size(obj.part{t}(2,:))),'Marker','.','Color',color,'LineStyle','none');
                hand3 = [];
                hand4 = [];
%                 if obj.id == criticalRob
%                     hand3 = plot3(posEst(1),posEst(2),-100,'Marker','x','Color','k','LineWidth',4,'MarkerSize',15);
% %                     hand4 = plot3(obj.mu(1,t),obj.mu(2,t),-100,'Marker','d','Color','k','LineWidth',4,'MarkerSize',15);
%                 end
            end
            h = [h hand1 hand2 hand3 hand4];

            for i=1:size(obj.muComm,1)
                if ~isempty(obj.muComm{i,t}) && obj.id == criticalRob
                    ell = obj.error_ellipse(obj.muComm{i,t},obj.SigmaComm{i,t});
                    hand4 = plot3(ell(:,1),ell(:,2),-100*ones(size(ell(:,2))),'Color',colors(5,:),'LineWidth',2);
                    h = [h, hand4];
                end
            end
            
            if ~holdstatus
                hold off
            end
        end
        
        %%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Multi-robot filter process update
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        function MRProcessUpdate(obj,t)
            d = obj.dim;
            
            % merge the filters
            [~,Pbest] = obj.getBestEst(t);
            Pinv = inv(Pbest(1:d,1:d));
            for i = 1:obj.nRob
                if t > 1 && ~isempty(obj.muComm{i,t-1})
                    % calculate the delta position from inertial data
                    p1 = (interp1(obj.tKft,obj.posKft',obj.tTRN(t-1)))';
                    p2 = (interp1(obj.tKft,obj.posKft',obj.tTRN(t)))';
                    deltaPos = p2-p1; 
                    
                    % calculate process noise
                    Q = (obj.procNoise*0.01*norm(deltaPos))^2*eye(d);
                    % update the mean
                    obj.muComm{i,t} = obj.muComm{i,t-1}+deltaPos(1:d);
                    
                    %update the covariance
                    Qbar = Q + obj.SigmaComm{i,t-1}*Pinv*Q + Q*Pinv*obj.SigmaComm{i,t-1};
                    obj.SigmaComm{i,t} = obj.SigmaComm{i,t-1} + Qbar;
                end
            end
        end
        
        %%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Multi-robot filter measurement update
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        function MultiRobotUpdate(obj,robData,t)
            if ~isempty(robData)
                d = obj.dim;
                % merge the filters
                [~,Pbest] = obj.getBestEst(t);
                Pinv = inv(Pbest(1:d,1:d));
                
                % for each received measurement
                for i = 1:length(robData)
                    sigDiag = diag(robData(i).Sigma);
                    % if the other vehicle has converged sufficiently
                    if all(sigDiag(1:d) < obj.convergedThresh)
                        
                        % interpolate to the measurement time
                        [~,dPos2] = obj.interpolateBetweenTRN(robData(i).time);
                        % if this is the first communication, initialize
                        % the prior
                        if isempty(obj.muComm{robData(i).id,t})
                            muPrior = obj.mu(1:d,t)-dPos2(1:d);
                        else
                            muPrior = obj.muComm{robData(i).id,t}-dPos2(1:d);
                        end
                        SigmaPrior = obj.SigmaComm{robData(i).id,t};
                        % do the update
                        [muUp,SigmaUp] = obj.CCIUpdate(muPrior, SigmaPrior,robData(i));
                        
                        % adjust the result to the TRN time units
                        obj.muComm{robData(i).id,t} = muUp + dPos2(1:d);
                        Q = obj.procNoise*0.01*norm(dPos2)*eye(d);
                        Qbar = Q + SigmaUp*Pinv*Q + Q*Pinv*SigmaUp;
                        obj.SigmaComm{robData(i).id,t} = SigmaUp  + Qbar;
                    end
                end
                
            end
            
        end
        
        %%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % CI for the multi-robot filters
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        function [muUp, SigmaUp] = CCIUpdate(obj,muPrior,SigmaPrior,data)
            if ~isempty(SigmaPrior) % if the vehicle has been seen before
                % linearize the measurement
                [P2d,P2i,Hi,y] = obj.buildMeasurementMatrices(data,muPrior);
                % optimize for Omega
                [w,~] = fminbnd(@(w) obj.splitTraceObjective(w,SigmaPrior,zeros(size(SigmaPrior)),P2d,P2i,Hi),0,1);
            else % initialize the filter
                SigmaPrior = 1e4*eye(obj.dim); % large value for covariance
                [P2d,P2i,Hi,y] = obj.buildMeasurementMatrices(data,muPrior);
                w=0.1; % accept the new measurement
            end
            
            % CI equations
            P1 = SigmaPrior/w;
            P2 = P2d/(1-w) + P2i;
            K = P1*Hi'/(Hi*P1*Hi' + P2);
            muUp = muPrior + K*y;
            SigmaUp = (eye(obj.dim)-K*Hi)*P1;
        end
        
        %%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Linearize a measurement and generate the sensitivity matrix     %
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        function [P2d,P2i,Hi,y] = buildMeasurementMatrices(obj,robData,muPrior)
            d = obj.dim;
            wrapping = [0 2*pi -2*pi];
            
            % if we have a range measurement
            if ~isempty(robData.rangeMeas)
                % calculate measurement covariance
                sigmaR = sqrt(robData.Rrange);
                Rrange = (sigmaR/100*robData.rangeMeas)^2;
                if ~isempty(robData.headingMeas)
                    % range and bearing
                    % z is measurement vector
                    z = [robData.rangeMeas; robData.headingMeas];
                    % delta is estimated position difference between
                    % vehicles
                    delta =  muPrior - robData.mu(1:obj.dim);
                    % zHat is predicted measurement
                    zHat = [norm(delta); atan2(delta(2),delta(1))];
                    
                    % calculate sensitivity matrix
                    H1 = delta'/zHat(1);
                    H2 = [-delta(2), delta(1), 0]/(delta(1)^2 + delta(2)^2);
                    H2 = H2(1:d);
                    Hi = [H1;H2];
                    Hj = -Hi;
                    
                    % measurement covariance matrix
                    R = diag([Rrange, robData.Rheading]);
                    y = z - zHat;
                    
                    % wrap heading measurement to 0 to 2pi
                    [~,ind] = min([abs(y(2)+wrapping(1)),abs(y(2)+wrapping(2)),abs(y(2)+wrapping(3))]);
                    y(2) = y(2) + wrapping(ind);
                    
                else % range only
                    z = robData.rangeMeas;
                    delta =  muPrior - robData.mu(1:obj.dim);
                    zHat = norm(delta);
                    Hi = delta'/zHat;
                    Hj = -Hi;
                    
                    R = Rrange;
                    y = z - zHat;
                end
                
            elseif ~isempty(robData.headingMeas) % heading only
                z = robData.headingMeas;
                delta = muPrior-robData.mu(1:obj.dim); % vector from i to obj
                zHat = atan2(delta(2),delta(1));
                H = [-delta(2), delta(1), 0]/(delta(1)^2 + delta(2)^2);
                Hi = H(1:d);
                Hj = -Hi;
                
                R = robData.Rheading;
                y = z - zHat;
                [~,ind] = min([abs(y(2)+wrapping(1)),abs(y(2)+wrapping(2)),abs(y(2)+wrapping(3))]);
                y(2) = y(2) + wrapping(ind);
            end
            P2d = Hj*robData.Sigma(1:obj.dim,1:obj.dim)*Hj';
            P2i = R;
            
        end
        
        %%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Check if multi-robot filters have gotten lost. Not currently used
        % because things work well, but may be needed to reinitialize
        % filters if things break
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        function overlap = checkFilterConsistency(obj,filtID,t)
            d = obj.dim;
            muFilt = obj.muComm{filtID,t};
            SigmaFilt = obj.SigmaComm{filtID,t};
            if isempty(muFilt)
                overlap = false;
                return;
            end
            
            SigMean = (obj.Sigma{t}(1:d,1:d) + SigmaFilt)/2;
            muDiff = obj.mu(1:d,t) - muFilt;
            dist = sqrt((muDiff'/SigMean)*muDiff)/2;
            
            chisquare_val = 5.99146455; % 95% confidence ellipse (2D)
            if dist < sqrt(chisquare_val)
                overlap = true;
            else
                overlap = false;
            end
        end
        
    end
    
    methods (Static)

        % Split CI objective function for optimizing omega
        function f = splitTraceObjective(w,P1d,P1i,P2d,P2i,H)
            P1 = P1d/w + P1i;
            P2 = P2d/(1-w) + P2i;
            K = P1*H'/(H*P1*H' + P2);
            Pup = P1-K*H*P1;
            
            f = trace(Pup);
        end
        
        % calculate a 3-sigma confidence ellipse
        function ell = error_ellipse(mu,Sigma)
            if length(mu) > 2
                mu = mu(1:2);
                Sigma = Sigma(1:2,1:2);
            end
            [eigenvec, eigenval ] = eig(Sigma);
            
            % Get the index of the largest eigenvector
            [largest_eigenvec_ind_c, ~] = find(eigenval == max(max(eigenval)));
            largest_eigenvec = eigenvec(:, largest_eigenvec_ind_c);
            
            % Get the largest eigenvalue
            largest_eigenval = max(max(eigenval));
            
            % Get the smallest eigenvector and eigenvalue
            if(largest_eigenvec_ind_c == 1)
                smallest_eigenval = max(eigenval(:,2));
            else
                smallest_eigenval = max(eigenval(:,1));
            end
            
            % Calculate the angle between the x-axis and the largest eigenvector
            angle = atan2(largest_eigenvec(2), largest_eigenvec(1));
            
            % This angle is between -pi and pi.
            % Let's shift it such that the angle is between 0 and 2pi
            if(angle < 0)
                angle = angle + 2*pi;
            end
            
            % Get the 3 sigma confidence interval error ellipse
%             chisquare_val = sqrt(5.99146455);
            chisquare_val = sqrt(11.829);
            theta_grid = linspace(0,2*pi);
            phi = angle;
            X0=mu(1);
            Y0=mu(2);
            a=chisquare_val*sqrt(largest_eigenval);
            b=chisquare_val*sqrt(smallest_eigenval);
            
            % the ellipse in x and y coordinates
            ellipse_x_r  = a*cos( theta_grid );
            ellipse_y_r  = b*sin( theta_grid );
            
            %Define a rotation matrix
            R = [ cos(phi) sin(phi); -sin(phi) cos(phi) ];
            
            %let's rotate the ellipse to some angle phi
            r_ellipse = [ellipse_x_r;ellipse_y_r]' * R;
            
            ell = [r_ellipse(:,1) + X0, r_ellipse(:,2) + Y0];
        end
    end
end