
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% How to use this class:
% 1. Initialize a robot object:
%    call LinearSimClass(id,pos,posUncertainty,nRob) with
%    - id: unique ID number for this vehicle (eg. 2)
%    - pos: true starting position of the vehicle
%    - posUncertainty: uncertainty bounds for initial particle
%      distribution. 3X2 vector [xmin xmax; ymin ymax; zmin; zmax];
%    - nRob: total number of robots in the system
%     
% 2. Step through time, at each timestep call 
%    a. sim(map,goal,bias,[]) to advance the vehicles
%    b. Build measurement structure to send between vehicles
%    c. MultiRobotUpdate to fuse measurements into the CI filters
%
% 3. call getBestEst to find the best estimate of position, or plot to plot
%    the results
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

classdef AUVSimulatorClass < handle
    properties
        id % Number of this vehicle
        % Truth state parameters
        dim = 2; % number of dimensions to propagate
        pos % m, inertial NED frame
        uStored % latest control command
        sonarStored % latest sonar measurement to seafloor
        nRob % number of vehicles
        
        % State Estimation
        N_p = 10000; % number of particles
        part % particles =  3xN_p
        wt % particle weights = 1xN_p
        mu % mean position estimate
        Sigma % positon estimate covariance
        muComm % mean position estimate
        SigmaComm % positon estimate covariance
        
        uLim = 1; % max speed (m/s)
        
        % Simulation properties
        A = zeros(3,3); % Assuming dynamics xDot = u
        B = eye(3);
        dt = 3; % seconds per timestep
        
        % Measurement parameters
        processNoise = 3; % percent distance travelled
        nBeam = 1; % number of sonar beams
        beamDir % directions of sonar beams
        beamNoise = 4; % percent of range
        sigMap = 0.3; % map noise
        
    end
    
    methods
        %%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Constructor to initialize a vehicle                             %
        % takes an ID number for this vehicle, the true starting position,%
        % the bounds for the initial particle grid, and the total number  %
        % robots in the system                                            %
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        function obj = AUVSimulatorClass(id,pos,posUncertainty,nRob)
            % uncertainty = [xLow xHigh; yLow yHigh; zLow; zHigh];
            obj.id = id;
            obj.pos = pos;
            obj.nRob = nRob;
            
            pos_width = posUncertainty(:,2) - posUncertainty(:,1);
            obj.part = rand(3,obj.N_p).*repmat(pos_width,1,obj.N_p)...
                + repmat(posUncertainty(1:3,1),1,obj.N_p);
            if obj.dim == 2
                obj.part(3,:) = obj.pos(3);
            end
            obj.wt = ones(1,obj.N_p)/obj.N_p;
            obj.mu = obj.part*obj.wt';
            obj.updateSigma();
            
            % Default property values (only uses one beam)
            beamAng = 5;
            obj.beamDir = [0,  sind(beamAng),  sind(-beamAng),  0,              0;
                0,  0,              0,               sind(beamAng),  sind(-beamAng);
                -1, -cosd(beamAng), -cosd(-beamAng), -cosd(beamAng), -cosd(-beamAng)];
            obj.beamDir = obj.beamDir(:,1:obj.nBeam);
            
            obj.muComm = cell(nRob,1);
            obj.SigmaComm = cell(nRob,1);
        end
        
        %%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Simulate one timestep of the vehicle moving towards a goal.     %
        % takes a map object for TRN, a goal, and a drift bias, and       %
        % (optionally) a data structure of measurements from other        %
        % vehicles. Returns 1 if the vehicle is within 1m of the goal, 0  %
        % otherwise
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        function done = sim(obj,map, goal,uBias,robData)
            done = 0;
            if obj.dim == 2
                goal(3,1) = obj.pos(3);
            end
            
            uCommand = obj.clControl(goal);
            uCurrent = uCommand + uBias;
            % move robot
            Q = obj.move(uCurrent);
            
            % predict (motion update)
            
            obj.MotionUpdate(uCurrent,Q);
            
            obj.uStored = uCurrent;
            
            % particle filter update
            sonar = obj.pfBeamUpdate(map);
            obj.sonarStored = sonar;
            
            if exist('robData','var') && ~isempty(robData)
                obj.MultiRobotUpdate(robData);
            end
            
            if norm(obj.pos-goal) < 1
                done = 1;
            end
        end
        
        % Compute a command to drive the vehicle towards the goal
        function uCommand = clControl(obj,goal)
            dir = goal-obj.pos;
            if obj.dim == 2
                dir(3) = 0;
            end
            uCommand = obj.uLim*dir/norm(dir);
        end
        
        % Execute the command to actually move the vehicle
        function Q = move(obj,uCurrent) % motion model
            dposTrue = (obj.A*obj.pos + obj.B*uCurrent)*obj.dt;
            sigProcess = obj.processNoise/100*norm(dposTrue);
            Q = sigProcess^2 *eye(3); % calculate the process noise covariance
            dpos = dposTrue + sigProcess*randn(3,1);
            if obj.dim == 2
                dpos(3) = 0;
            end
            obj.pos = obj.pos + dpos;
        end
        
        % Process update for particle filter and multi-robot CI filters
        function MotionUpdate(obj,u,Q)
            dpos = (obj.A*obj.mu + obj.B*u)*obj.dt;
            
            obj.part = obj.part + dpos + chol(Q)*randn(3,obj.N_p);
            if obj.dim == 2
                obj.part(3,:) = obj.pos(3);
            end
            obj.mu = obj.part*obj.wt';
            obj.updateSigma();
            
            d = obj.dim;
            F = eye(d) + obj.A(1:d,1:d)*obj.dt;
            [~,Pbest] = obj.getBestEstCombined();
            Pinv = inv(Pbest(1:d,1:d));
            for i=1:length(obj.muComm) % EKF initialized, apply update
                if ~isempty(obj.muComm{i})
                    obj.muComm{i} = obj.muComm{i} + dpos(1:d);
                    R = obj.SigmaComm{i};
                    Qbar = Q(1:d,1:d) + R*Pinv*Q(1:d,1:d) + Q(1:d,1:d)*Pinv*R;
                    obj.SigmaComm{i} = obj.SigmaComm{i} + Qbar;
                    if trace(obj.SigmaComm{i}) > 1e6
                        obj.SigmaComm{i} = [];
                        obj.muComm{i} = [];
                    end
                end
            end
        end
        
        % TRN measurement update for particles
        function range = pfBeamUpdate(obj,map)
            range = obj.findRayDistanceStraight(obj.pos,map);
            sigBeam = obj.beamNoise/100*range;
            range = range + sigBeam*randn(1,1);
            
            zHat = zeros(obj.nBeam,obj.N_p);
            Lpf = zeros(obj.nBeam,obj.N_p);
            for j=1:obj.N_p
                for k = 1:obj.nBeam
                    zHat(k,j) = obj.findRayDistanceStraight(obj.part(:,j),map);
                    Lpf(k,j) = exp(-1/2*((range(k)-zHat(k,j))^2/(sigBeam^2 + obj.sigMap^2)));
                end
            end
            
            zHatBar = zHat*obj.wt';
            varZHat = ((zHat - repmat(zHatBar,1,obj.N_p)).^2)*obj.wt';
            delta2 = varZHat - (1 - sum(obj.wt.^2))*obj.sigMap^2;
            alph = delta2*(sigBeam^2 + obj.sigMap^2)./((sigBeam^2 + obj.sigMap^2)*(delta2 + obj.sigMap^2)+sigBeam^2*obj.sigMap^2);
            alph = max(0,alph);
            Lup = ones(size(obj.wt));
            for k=1:obj.nBeam
                if ~any((isnan(zHat(k,:)))) && ~isnan(range(k))
                    Lup = Lup.*Lpf(k,:).^alph(k);
                end
            end
            obj.wt = Lup.*obj.wt;
            obj.wt = obj.wt/sum(obj.wt);
            
            Neff=1/sum(obj.wt.*obj.wt);
            if Neff<.75*obj.N_p
                obj.resample();
            end
            obj.mu = obj.part*obj.wt';
            obj.updateSigma();
        end
        
        % resample particle filter
        function resample(obj)
            xout=zeros(size(obj.part));
            r=(1/obj.N_p -0).*rand(1,1)+0;
            c=obj.wt(1);
            i=1;
            for m=1:obj.N_p
                U=r+(m-1)*(1/obj.N_p);
                while U>c
                    i=i+1;
                    c=c+obj.wt(i);
                end
                xout(:,m)=obj.part(:,i);
            end
            obj.part = xout;
            obj.wt=ones(1,obj.N_p)/obj.N_p;
        end
        
        % Fuse in multi-robot measurements into the CI filters
        function MultiRobotUpdate(obj,robData)
            for i = 1:length(robData)
                % check if Gaussian overlaps with PF (not needed now)
                if ~isempty(obj.muComm{robData(i).id})
                    overlap = obj.checkFilterConsistency(robData(i).id);
                    if ~overlap % reinit
                        obj.muComm{robData(i).id} = [];
                        obj.SigmaComm{robData(i).id} = [];
                    end
                end
                
                %prior:
                muPrior = obj.muComm{robData(i).id};
                SigmaPrior = obj.SigmaComm{robData(i).id};
                [muUp,SigmaUp] = obj.CCIUpdate(muPrior, SigmaPrior,robData(i));
                obj.muComm{robData(i).id} = muUp;
                obj.SigmaComm{robData(i).id} = SigmaUp;
                
            end
            
        end
        
        % CI equations
        function [muUp, SigmaUp] = CCIUpdate(obj,muPrior,SigmaPrior,data)
            if ~isempty(SigmaPrior)
                [P2d,P2i,Hi,y] = obj.buildMeasurementMatrices(data,muPrior);
                [w,~] = fminbnd(@(w) obj.splitTraceObjective(w,SigmaPrior,zeros(size(SigmaPrior)),P2d,P2i,Hi),0,1);
            else
                muPrior = obj.mu(1:obj.dim);
                SigmaPrior = 1e4*eye(obj.dim);
                [P2d,P2i,Hi,y] = obj.buildMeasurementMatrices(data,muPrior);
                w=0.1;
            end
            
            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 measurements and build sensitivity matrix
        function [P2d,P2i,Hi,y] = buildMeasurementMatrices(obj,robData,muPrior)
            d = obj.dim;
            otherMu = robData.mu; % other robot's mean
            otherSig = robData.Sigma; % other robot's covariance
            muPrior = muPrior(1:d); % this robot's prior
            
            z = robData.meas; % relative measurement
            if robData.measType == 1 % If measurement is relative position
                zHat = muPrior - otherMu; % estimated relative position
                Hi = eye(d);
                Hj = -eye(d);
                R = robData.R; % measurement covariance
                
            elseif robData.measType == 2 % If measurement is range-only
                sigmaR = sqrt(robData.R); % sigma is sqrt of cov
                R = (sigmaR*0.01*robData.meas)^2; % calculate R as percent of range
                delta =  muPrior - otherMu; % relative position
                zHat = norm(delta); % expected range
                Hi = delta'/zHat; % Jacobian of range
                Hj = -Hi;
                
            elseif robData.measType == 3 % If measurement is bearing-only
                delta = muPrior-otherMu; % relative position
                zHat = atan2(delta(2),delta(1)); % expected bearing
                Hi = [-delta(2), delta(1), 0]/(delta(1)^2 + delta(2)^2);
                Hi = Hi(1:d); % Jacobian
                Hj = -Hi;
                R = robData.R;
                
            elseif robData.measType == 4 % Measurement is range, bearing, (& elev if 3D)
                delta =  muPrior - otherMu; % relative position
                inPlane = sqrt(delta(1)^2 + delta(2)^2); % component in the NE plane
                if d > 2 % If 3D, build RBE measurement vector
                    zHat = [norm(delta); ...
                        atan2(delta(2),delta(1)); ...
                        atan2(delta(3),inPlane)];
                else % 2D, just range and bearing measurement vector
                    zHat = [norm(delta); ...
                        atan2(delta(2),delta(1))];
                end
                
                H1 = delta'/zHat(1); % Range Jacobian
                if d > 2
                    H2 = [-delta(2), delta(1), 0]/inPlane^2; % bearing Jacobian
                    H3 = [-delta(1)*delta(3)/inPlane, -delta(2)*delta(3)/inPlane, inPlane]/norm(delta)^2; % elevation Jacobian
                    Hi = [H1;H2;H3];
                else
                    H2 = [-delta(2), delta(1)]/inPlane^2; % bearing Jacobian
                    Hi = [H1;H2];
                end
                Hj = -Hi;
                
                R = robData.R;
                sigmaR = sqrt(R(1,1)); % change range noise to percent of range
                R(1,1) = (sigmaR*0.01*robData.meas(1))^2;
            end
            
            P2d = Hj*otherSig*Hj'; % correlated component of covariance
            P2i = R; % independent component of covariance
            y = z - zHat;
            
            % wrap bearing and elevation to 0 to 2pi
            if robData.measType == 4
                wrapping = [0 2*pi -2*pi];
                for i=2:d
                    [~,ind] = min([abs(y(i)+wrapping(1)),abs(y(i)+wrapping(2)),abs(y(i)+wrapping(3))]);
                    y(i) = y(i) + wrapping(ind);
                end
            end
        end
        
        % check if the multi-robot filters substantially overlap with the
        % particle cloud (tests if they've diverged and gotten lost). This
        % shouldn't happen anymore, so not really needed
        function overlap = checkFilterConsistency(obj,filtID)
            % Option 1: check if the 95% confidence ellipses overlap
            obj.mu = obj.part*obj.wt';
            obj.updateSigma();
            
            d = obj.dim;
            muFilt = obj.muComm{filtID};
            SigmaFilt = obj.SigmaComm{filtID};
            
            SigMean = (obj.Sigma(1:d,1:d) + SigmaFilt)/2;
            muDiff = obj.mu(1:d) - 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
            
            
            % Option 2: Check if any particles are within filter's 95%
            % confidence
            %             overlap = false;
            
            %             d = obj.dim;
            %             for j=1:obj.N_p
            %                 val = (obj.part(1:d,j)-muFilt)'/SigmaFilt*(obj.part(1:d,j)-muFilt);
            %                 if val <= chisquare_val
            %                     overlap = true;
            %                 end
            %             end
        end
        
        % compute the approximate best-fit Gaussian for the particles
        function updateSigma(obj)
            Sig = zeros(3,3);
            for i=1:obj.N_p
                Sig = Sig + obj.wt(i)*(obj.part(:,i)-obj.mu)*(obj.part(:,i)-obj.mu)';
            end
            obj.Sigma = Sig;
        end
        
        % merge the multi-robot CI filters into a single mean and
        % covariance
        function [muCombined,SigmaCombined] = combineEKFs(obj)
            d = obj.dim;
            Icomb = zeros(d,d);
            ycomb = zeros(d,1);
            foundFlag = 0;
            for i=1:length(obj.muComm)
                if ~isempty(obj.muComm{i})
                    Pinv = inv(obj.SigmaComm{i}(1:d,1:d));
                    Icomb = Icomb + Pinv;
                    ycomb = ycomb + Pinv*obj.muComm{i}(1:d);
                    foundFlag = 1;
                end
            end
            if foundFlag
                SigmaCombined = inv(Icomb);
                muCombined = SigmaCombined*ycomb;
            else
                muCombined = [];
                SigmaCombined = [];
            end
        end
        
        % Get the best estimate of position by first merging the CI
        % filters, then applying it to the particle filter
        function [posEst,SigFit] = getBestEstCombined(obj)
            d = obj.dim;
            [muCombined,SigmaCombined] = obj.combineEKFs();
            if isempty(muCombined)
                posEst = obj.part*obj.wt';
                obj.updateSigma;
                SigFit = obj.Sigma;
            else
                reweight = zeros(size(obj.wt));
                for i=1:obj.N_p
                    reweight(i) = obj.wt(i)*exp(-0.5*((obj.part(1:d,i)-muCombined)'/SigmaCombined)*(obj.part(1:d,i)-muCombined));
                end
                reweight = reweight/sum(reweight);
                posEst = obj.part*reweight';
                
                SigFit = zeros(3,3);
                for i=1:obj.N_p
                    SigFit = SigFit + reweight(i)*(obj.part(:,i)-posEst)*(obj.part(:,i)-posEst)';
                end
            end
        end
        
        % get the best estimate of position by applying each CI filter
        % individually to the particle filter
        function [posEst,SigFit] = getBestEst(obj)
            d = obj.dim;
            reweight = obj.wt;
            for i=1:obj.N_p
                for j = 1:length(obj.muComm)
                    if ~isempty(obj.muComm{j})
                        reweight(i) = reweight(i)*exp(-0.5*((obj.part(1:d,i)-obj.muComm{j})'/obj.SigmaComm{j})*(obj.part(1:d,i)-obj.muComm{j}));
                    end
                end
            end
            reweight = reweight/sum(reweight);
            posEst = obj.part*reweight';
            
            SigFit = zeros(3,3);
            for i=1:obj.N_p
                SigFit = SigFit + reweight(i)*(obj.part(:,i)-posEst)*(obj.part(:,i)-posEst)';
            end
        end
        
        % plot the vehicles to visualize performance
        function h = plot(obj,h,color,criticalRob)
            if nargin < 2
                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,SigFit] = obj.getBestEst();
            hand1 = [];
            hand2 = [];
            hand3 = [];
            hand4 = [];
            hand5 = [];
            
            if obj.id == criticalRob
                
                %                 hand2 = plot3(posEst(1),posEst(2),-100,'Marker','+','Color',colors(5,:),'LineWidth',2,'MarkerSize',8);
                %                 hand1 = plot3(obj.pos(1),obj.pos(2),-100,'Marker','o','Color',colors(5,:),'LineWidth',3,'MarkerSize',10);
                %                 hand5 = plot3(obj.mu(1),obj.mu(2),-100,'Marker','d','Color',colors(5,:),'LineWidth',2,'MarkerSize',8,'MarkerFaceColor',colors(5,:));
                hand1 = plot3(obj.pos(1),obj.pos(2),-100,'Marker','*','Color',colors(5,:),'LineWidth',2,'MarkerSize',10);
            end
            
            hand3 = plot3(obj.part(1,:),obj.part(2,:),-99*ones(size(obj.part(2,:))),'Marker','.','Color',color,'LineStyle','none','MarkerSize',10);
            %                             hand3 = scatter3(obj.part(1,:),obj.part(2,:),-99*ones(size(obj.part(2,:))),150,color,'.');
            %                             hand3.MarkerFaceAlpha = .7;
            %                             hand3.MarkerEdgeAlpha = .7;
            %
            %                 hand3 = [];
            %                 hand4 = plot3(obj.mu(1),obj.mu(2),-100,'Marker','d','Color','k','LineWidth',4,'MarkerSize',15);
            
            
            h = [h hand1 hand2 hand3 hand4 hand5];
            
            %             ell = obj.error_ellipse(posEst,SigFit);
            %             hand3 = plot(ell(:,1),ell(:,2),'y-','LineWidth',4);
            %             h = [h, hand3];
            
            %             [muCombined,SigmaCombined] = obj.combineEKFs();
            %             if ~isempty(muCombined)% && obj.id == 2
            %                 ell = obj.error_ellipse(muCombined,SigmaCombined);
            %                 hand3 = plot(ell(:,1),ell(:,2),'y-','LineWidth',4);
            %                 h = [h, hand3];
            %             end
            for i=1:length(obj.muComm)
                if ~isempty(obj.muComm{i}) && obj.id == criticalRob
                    ell = obj.error_ellipse(obj.muComm{i},obj.SigmaComm{i});
                    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
        
    end
    
    methods (Static)
        
        % objective function for CI optimization of 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
        
        % RAY TRACING METHOD (not currently used)
        function range = findRayDistance(pos,map,direction)
            direction = direction/norm(direction);
            
            top = max(max(map.U_Nan));
            bottom = min(min(map.U_Nan));
            t1 = (pos(3)-top)/direction(3);
            if t1 > 0
                pos = pos + t1*direction;
            else
                t1 = 0;
            end
            maxrange = 1000;
            t = 0;
            tol = map.res;
            while true
                R = pos + t*direction;
                if R(1) < 1 || R(1) > size(map.U,1) || R(2) < 1 || R(2) > size(map.U,2)
                    range = nan;
                    return;
                end
                if (map.U(round(R(1)),round(R(2))) - tol < R(3)) &&...
                        (R(3) < map.U(round(R(1)),round(R(2))) + tol)
                    range = t + t1;
                    return;
                end
                if R(3) < bottom
                    range = nan;
                    return;
                end
                if t > maxrange
                    range = nan;
                    return;
                end
                t = t + tol;
            end
        end
        
        % assume beams are ONLY straight down (used for single-beam only)
        function range = findRayDistanceStraight(pos,map)
            range = pos(3) - map.getDepth(pos(1),pos(2));
        end
        
        % APPROXIMATE RAY TRACING (NOT USED)
        function range = findRayDistanceApprox(pos,map,direction,meas)
            if isnan(meas)
                range = nan;
                return;
            end
            
            direction = direction/norm(direction);
            
            R = pos + meas*direction;
            
            if R(1) < 1 || R(1) > size(map.U,1) || R(2) < 1 || R(2) > size(map.U,2)
                range = nan;
                return;
            end
            if isnan(map.U_Nan(round(R(1)),round(R(2))))
                range = nan;
                return;
            end
            err = R(3) - map.U_Nan(round(R(1)),round(R(2)));
            
            range = meas + err/abs(direction(3));
        end
        
        % Calculate the 3-sigma covariance 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 95% confidence interval error ellipse
            %             chisquare_val = sqrt(5.99146455);
            chisquare_val = sqrt(11.829);   % 3sigma
            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