├── ODE_Solvers ├── ode1.m ├── ode2.m ├── ode3.m ├── ode4.m ├── ode5.m └── ode_test.m ├── README.md ├── gifs ├── consensus_65_15.gif └── nonconsensus_130_0.gif ├── mfiles ├── VoronoiLimit.m ├── compute_centroid.m ├── cvtODE.m ├── dintegrate.m ├── main.m └── reshape_state.m ├── plots ├── base_controller.png ├── consensus_65_15.png ├── non_consensus_130_0.png └── position_error_corrected.png └── ref.bib /ODE_Solvers/ode1.m: -------------------------------------------------------------------------------- 1 | function Y = ode1(odefun,tspan,y0,varargin) 2 | %ODE1 Solve differential equations with a non-adaptive method of order 1. 3 | % Y = ODE1(ODEFUN,TSPAN,Y0) with TSPAN = [T1, T2, T3, ... TN] integrates 4 | % the system of differential equations y' = f(t,y) by stepping from T0 to 5 | % T1 to TN. Function ODEFUN(T,Y) must return f(t,y) in a column vector. 6 | % The vector Y0 is the initial conditions at T0. Each row in the solution 7 | % array Y corresponds to a time specified in TSPAN. 8 | % 9 | % Y = ODE1(ODEFUN,TSPAN,Y0,P1,P2...) passes the additional parameters 10 | % P1,P2... to the derivative function as ODEFUN(T,Y,P1,P2...). 11 | % 12 | % This is a non-adaptive solver. The step sequence is determined by TSPAN. 13 | % The solver implements the forward Euler method of order 1. 14 | % 15 | % Example 16 | % tspan = 0:0.1:20; 17 | % y = ode1(@vdp1,tspan,[2 0]); 18 | % plot(tspan,y(:,1)); 19 | % solves the system y' = vdp1(t,y) with a constant step size of 0.1, 20 | % and plots the first component of the solution. 21 | % 22 | 23 | if ~isnumeric(tspan) 24 | error('TSPAN should be a vector of integration steps.'); 25 | end 26 | 27 | if ~isnumeric(y0) 28 | error('Y0 should be a vector of initial conditions.'); 29 | end 30 | 31 | h = diff(tspan); 32 | if any(sign(h(1))*h <= 0) 33 | error('Entries of TSPAN are not in order.') 34 | end 35 | 36 | try 37 | f0 = feval(odefun,tspan(1),y0,varargin{:}); 38 | catch 39 | msg = ['Unable to evaluate the ODEFUN at t0,y0. ',lasterr]; 40 | error(msg); 41 | end 42 | 43 | y0 = y0(:); % Make a column vector. 44 | if ~isequal(size(y0),size(f0)) 45 | error('Inconsistent sizes of Y0 and f(t0,y0).'); 46 | end 47 | 48 | neq = length(y0); 49 | N = length(tspan); 50 | Y = zeros(neq,N); 51 | 52 | Y(:,1) = y0; 53 | for i = 1:N-1 54 | Y(:,i+1) = Y(:,i) + h(i)*feval(odefun,tspan(i),Y(:,i),varargin{:}); 55 | end 56 | Y = Y.'; 57 | -------------------------------------------------------------------------------- /ODE_Solvers/ode2.m: -------------------------------------------------------------------------------- 1 | function Y = ode2(odefun,tspan,y0,varargin) 2 | %ODE2 Solve differential equations with a non-adaptive method of order 2. 3 | % Y = ODE2(ODEFUN,TSPAN,Y0) with TSPAN = [T1, T2, T3, ... TN] integrates 4 | % the system of differential equations y' = f(t,y) by stepping from T0 to 5 | % T1 to TN. Function ODEFUN(T,Y) must return f(t,y) in a column vector. 6 | % The vector Y0 is the initial conditions at T0. Each row in the solution 7 | % array Y corresponds to a time specified in TSPAN. 8 | % 9 | % Y = ODE2(ODEFUN,TSPAN,Y0,P1,P2...) passes the additional parameters 10 | % P1,P2... to the derivative function as ODEFUN(T,Y,P1,P2...). 11 | % 12 | % This is a non-adaptive solver. The step sequence is determined by TSPAN 13 | % but the derivative function ODEFUN is evaluated multiple times per step. 14 | % The solver implements the improved Euler (Heun's) method of order 2. 15 | % 16 | % Example 17 | % tspan = 0:0.1:20; 18 | % y = ode2(@vdp1,tspan,[2 0]); 19 | % plot(tspan,y(:,1)); 20 | % solves the system y' = vdp1(t,y) with a constant step size of 0.1, 21 | % and plots the first component of the solution. 22 | % 23 | 24 | if ~isnumeric(tspan) 25 | error('TSPAN should be a vector of integration steps.'); 26 | end 27 | 28 | if ~isnumeric(y0) 29 | error('Y0 should be a vector of initial conditions.'); 30 | end 31 | 32 | h = diff(tspan); 33 | if any(sign(h(1))*h <= 0) 34 | error('Entries of TSPAN are not in order.') 35 | end 36 | 37 | try 38 | f0 = feval(odefun,tspan(1),y0,varargin{:}); 39 | catch 40 | msg = ['Unable to evaluate the ODEFUN at t0,y0. ',lasterr]; 41 | error(msg); 42 | end 43 | 44 | y0 = y0(:); % Make a column vector. 45 | if ~isequal(size(y0),size(f0)) 46 | error('Inconsistent sizes of Y0 and f(t0,y0).'); 47 | end 48 | 49 | neq = length(y0); 50 | N = length(tspan); 51 | Y = zeros(neq,N); 52 | F = zeros(neq,2); 53 | 54 | Y(:,1) = y0; 55 | for i = 2:N 56 | ti = tspan(i-1); 57 | hi = h(i-1); 58 | yi = Y(:,i-1); 59 | F(:,1) = feval(odefun,ti,yi,varargin{:}); 60 | F(:,2) = feval(odefun,ti+hi,yi+hi*F(:,1),varargin{:}); 61 | Y(:,i) = yi + (hi/2)*(F(:,1) + F(:,2)); 62 | end 63 | Y = Y.'; 64 | -------------------------------------------------------------------------------- /ODE_Solvers/ode3.m: -------------------------------------------------------------------------------- 1 | function Y = ode3(odefun,tspan,y0,varargin) 2 | %ODE3 Solve differential equations with a non-adaptive method of order 3. 3 | % Y = ODE3(ODEFUN,TSPAN,Y0) with TSPAN = [T1, T2, T3, ... TN] integrates 4 | % the system of differential equations y' = f(t,y) by stepping from T0 to 5 | % T1 to TN. Function ODEFUN(T,Y) must return f(t,y) in a column vector. 6 | % The vector Y0 is the initial conditions at T0. Each row in the solution 7 | % array Y corresponds to a time specified in TSPAN. 8 | % 9 | % Y = ODE3(ODEFUN,TSPAN,Y0,P1,P2...) passes the additional parameters 10 | % P1,P2... to the derivative function as ODEFUN(T,Y,P1,P2...). 11 | % 12 | % This is a non-adaptive solver. The step sequence is determined by TSPAN 13 | % but the derivative function ODEFUN is evaluated multiple times per step. 14 | % The solver implements the Bogacki-Shampine Runge-Kutta method of order 3. 15 | % 16 | % Example 17 | % tspan = 0:0.1:20; 18 | % y = ode3(@vdp1,tspan,[2 0]); 19 | % plot(tspan,y(:,1)); 20 | % solves the system y' = vdp1(t,y) with a constant step size of 0.1, 21 | % and plots the first component of the solution. 22 | % 23 | 24 | if ~isnumeric(tspan) 25 | error('TSPAN should be a vector of integration steps.'); 26 | end 27 | 28 | if ~isnumeric(y0) 29 | error('Y0 should be a vector of initial conditions.'); 30 | end 31 | 32 | h = diff(tspan); 33 | if any(sign(h(1))*h <= 0) 34 | error('Entries of TSPAN are not in order.') 35 | end 36 | 37 | try 38 | f0 = feval(odefun,tspan(1),y0,varargin{:}); 39 | catch 40 | msg = ['Unable to evaluate the ODEFUN at t0,y0. ',lasterr]; 41 | error(msg); 42 | end 43 | 44 | y0 = y0(:); % Make a column vector. 45 | if ~isequal(size(y0),size(f0)) 46 | error('Inconsistent sizes of Y0 and f(t0,y0).'); 47 | end 48 | 49 | neq = length(y0); 50 | N = length(tspan); 51 | Y = zeros(neq,N); 52 | F = zeros(neq,3); 53 | 54 | Y(:,1) = y0; 55 | for i = 2:N 56 | ti = tspan(i-1); 57 | hi = h(i-1); 58 | yi = Y(:,i-1); 59 | F(:,1) = feval(odefun,ti,yi,varargin{:}); 60 | F(:,2) = feval(odefun,ti+0.5*hi,yi+0.5*hi*F(:,1),varargin{:}); 61 | F(:,3) = feval(odefun,ti+0.75*hi,yi+0.75*hi*F(:,2),varargin{:}); 62 | Y(:,i) = yi + (hi/9)*(2*F(:,1) + 3*F(:,2) + 4*F(:,3)); 63 | end 64 | Y = Y.'; 65 | 66 | -------------------------------------------------------------------------------- /ODE_Solvers/ode4.m: -------------------------------------------------------------------------------- 1 | function Y = ode4(odefun,tspan,y0,varargin) 2 | %ODE4 Solve differential equations with a non-adaptive method of order 4. 3 | % Y = ODE4(ODEFUN,TSPAN,Y0) with TSPAN = [T1, T2, T3, ... TN] integrates 4 | % the system of differential equations y' = f(t,y) by stepping from T0 to 5 | % T1 to TN. Function ODEFUN(T,Y) must return f(t,y) in a column vector. 6 | % The vector Y0 is the initial conditions at T0. Each row in the solution 7 | % array Y corresponds to a time specified in TSPAN. 8 | % 9 | % Y = ODE4(ODEFUN,TSPAN,Y0,P1,P2...) passes the additional parameters 10 | % P1,P2... to the derivative function as ODEFUN(T,Y,P1,P2...). 11 | % 12 | % This is a non-adaptive solver. The step sequence is determined by TSPAN 13 | % but the derivative function ODEFUN is evaluated multiple times per step. 14 | % The solver implements the classical Runge-Kutta method of order 4. 15 | % 16 | % Example 17 | % tspan = 0:0.1:20; 18 | % y = ode4(@vdp1,tspan,[2 0]); 19 | % plot(tspan,y(:,1)); 20 | % solves the system y' = vdp1(t,y) with a constant step size of 0.1, 21 | % and plots the first component of the solution. 22 | % 23 | 24 | if ~isnumeric(tspan) 25 | error('TSPAN should be a vector of integration steps.'); 26 | end 27 | 28 | if ~isnumeric(y0) 29 | error('Y0 should be a vector of initial conditions.'); 30 | end 31 | 32 | h = diff(tspan); 33 | if any(sign(h(1))*h <= 0) 34 | error('Entries of TSPAN are not in order.') 35 | end 36 | 37 | try 38 | f0 = feval(odefun,tspan(1),y0,varargin{:}); 39 | catch 40 | msg = ['Unable to evaluate the ODEFUN at t0,y0. ',lasterr]; 41 | error(msg); 42 | end 43 | 44 | y0 = y0(:); % Make a column vector. 45 | if ~isequal(size(y0),size(f0)) 46 | error('Inconsistent sizes of Y0 and f(t0,y0).'); 47 | end 48 | 49 | neq = length(y0); 50 | N = length(tspan); 51 | Y = zeros(neq,N); 52 | F = zeros(neq,4); 53 | 54 | Y(:,1) = y0; 55 | for i = 2:N 56 | ti = tspan(i-1); 57 | hi = h(i-1); 58 | yi = Y(:,i-1); 59 | F(:,1) = feval(odefun,ti,yi,varargin{:}); 60 | F(:,2) = feval(odefun,ti+0.5*hi,yi+0.5*hi*F(:,1),varargin{:}); 61 | F(:,3) = feval(odefun,ti+0.5*hi,yi+0.5*hi*F(:,2),varargin{:}); 62 | F(:,4) = feval(odefun,tspan(i),yi+hi*F(:,3),varargin{:}); 63 | Y(:,i) = yi + (hi/6)*(F(:,1) + 2*F(:,2) + 2*F(:,3) + F(:,4)); 64 | end 65 | Y = Y.'; 66 | -------------------------------------------------------------------------------- /ODE_Solvers/ode5.m: -------------------------------------------------------------------------------- 1 | function Y = ode5(odefun,tspan,y0,varargin) 2 | %ODE5 Solve differential equations with a non-adaptive method of order 5. 3 | % Y = ODE5(ODEFUN,TSPAN,Y0) with TSPAN = [T1, T2, T3, ... TN] integrates 4 | % the system of differential equations y' = f(t,y) by stepping from T0 to 5 | % T1 to TN. Function ODEFUN(T,Y) must return f(t,y) in a column vector. 6 | % The vector Y0 is the initial conditions at T0. Each row in the solution 7 | % array Y corresponds to a time specified in TSPAN. 8 | % 9 | % Y = ODE5(ODEFUN,TSPAN,Y0,P1,P2...) passes the additional parameters 10 | % P1,P2... to the derivative function as ODEFUN(T,Y,P1,P2...). 11 | % 12 | % This is a non-adaptive solver. The step sequence is determined by TSPAN 13 | % but the derivative function ODEFUN is evaluated multiple times per step. 14 | % The solver implements the Dormand-Prince method of order 5 in a general 15 | % framework of explicit Runge-Kutta methods. 16 | % 17 | % Example 18 | % tspan = 0:0.1:20; 19 | % y = ode5(@vdp1,tspan,[2 0]); 20 | % plot(tspan,y(:,1)); 21 | % solves the system y' = vdp1(t,y) with a constant step size of 0.1, 22 | % and plots the first component of the solution. 23 | 24 | if ~isnumeric(tspan) 25 | error('TSPAN should be a vector of integration steps.'); 26 | end 27 | 28 | if ~isnumeric(y0) 29 | error('Y0 should be a vector of initial conditions.'); 30 | end 31 | 32 | h = diff(tspan); 33 | if any(sign(h(1))*h <= 0) 34 | error('Entries of TSPAN are not in order.') 35 | end 36 | 37 | try 38 | f0 = feval(odefun,tspan(1),y0,varargin{:}); 39 | catch 40 | msg = ['Unable to evaluate the ODEFUN at t0,y0. ',lasterr]; 41 | error(msg); 42 | end 43 | 44 | y0 = y0(:); % Make a column vector. 45 | if ~isequal(size(y0),size(f0)) 46 | error('Inconsistent sizes of Y0 and f(t0,y0).'); 47 | end 48 | 49 | neq = length(y0); 50 | N = length(tspan); 51 | Y = zeros(neq,N); 52 | 53 | % Method coefficients -- Butcher's tableau 54 | % 55 | % C | A 56 | % --+--- 57 | % | B 58 | 59 | C = [1/5; 3/10; 4/5; 8/9; 1]; 60 | 61 | A = [ 1/5, 0, 0, 0, 0 62 | 3/40, 9/40, 0, 0, 0 63 | 44/45 -56/15, 32/9, 0, 0 64 | 19372/6561, -25360/2187, 64448/6561, -212/729, 0 65 | 9017/3168, -355/33, 46732/5247, 49/176, -5103/18656]; 66 | 67 | B = [35/384, 0, 500/1113, 125/192, -2187/6784, 11/84]; 68 | 69 | % More convenient storage 70 | A = A.'; 71 | B = B(:); 72 | 73 | nstages = length(B); 74 | F = zeros(neq,nstages); 75 | 76 | Y(:,1) = y0; 77 | for i = 2:N 78 | ti = tspan(i-1); 79 | hi = h(i-1); 80 | yi = Y(:,i-1); 81 | 82 | % General explicit Runge-Kutta framework 83 | F(:,1) = feval(odefun,ti,yi,varargin{:}); 84 | for stage = 2:nstages 85 | tstage = ti + C(stage-1)*hi; 86 | ystage = yi + F(:,1:stage-1)*(hi*A(1:stage-1,stage-1)); 87 | F(:,stage) = feval(odefun,tstage,ystage,varargin{:}); 88 | end 89 | Y(:,i) = yi + F*(hi*B); 90 | 91 | end 92 | Y = Y.'; 93 | -------------------------------------------------------------------------------- /ODE_Solvers/ode_test.m: -------------------------------------------------------------------------------- 1 | function ode_test 2 | %ODE_TEST Run non-adaptive ODE solvers of different orders. 3 | % ODE_TEST compares the orders of accuracy of several explicit Runge-Kutta 4 | % methods. The non-adaptive ODE solvers are tested on a problem used in 5 | % E. Hairer, S.P. Norsett, and G. Wanner, Solving Ordinary Differential 6 | % Equations I, Nonstiff Problems, Springer-Verlag, 1987. 7 | % The errors of numerical solutions obtained with several step sizes is 8 | % plotted against the step size used. For a given solver, the slope of that 9 | % line corresponds to the order of the integration method used. 10 | % 11 | 12 | solvers = {'ode1','ode2','ode3','ode4','ode5'}; 13 | Nsteps = [800 400 200 100 75 50 20 11]; 14 | 15 | x0 = 0; 16 | y0 = [1;1;1;1]; 17 | xend = 1; 18 | h = (xend-x0)./Nsteps; 19 | 20 | err = zeros(length(solvers),length(h)); 21 | 22 | for i = 1:length(solvers) 23 | solver = solvers{i}; 24 | for j = 1:length(Nsteps) 25 | N = Nsteps(j); 26 | x = linspace(x0,xend,N+1); 27 | y = feval(solver,@f,x,y0); 28 | err(i,j) = max(max(abs(y - yexact(x)))); 29 | end 30 | end 31 | 32 | figure 33 | loglog(h,err); 34 | title('Different slopes for methods of different orders...'); 35 | xlabel('log(h)'); 36 | ylabel('log(err)'); 37 | legend(solvers{:},4); 38 | 39 | % ------------------------- 40 | function yp = f(x,y) 41 | yp = [ 2*x*y(4)*y(1) 42 | 10*x*y(4)*y(1)^5 43 | 2*x*y(4) 44 | -2*x*(y(3)-1) ]; 45 | 46 | % ------------------------- 47 | function y = yexact(x) 48 | x2 = x(:).^2; 49 | y = zeros(length(x),4); 50 | y(:,1) = exp(sin(x2)); 51 | y(:,2) = exp(5*sin(x2)); 52 | y(:,3) = sin(x2)+1; 53 | y(:,4) = cos(x2); 54 | 55 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Multi-Robot systems project 2 | 3 | This respository implements the algorithm proposed in the paper ["Decentralized, adaptive coverage control for networked robots"](http://web.mit.edu/nsl/www/preprints/Adaptive_Coverage08.pdf). 4 | 5 | ## Simulation plots 6 | 7 | * **Basic (Non-concensus) controller** (t = 0 to t = 30) 8 | 9 | ![Non-Consensus controller](gifs/nonconsensus_130_0.gif) 10 | 11 | * **Concensus controller** (t = 0 to t = 30) 12 | 13 | ![Consensus controller](gifs/consensus_65_15.gif) 14 | 15 | ## Results 16 | * **Controller with known sensory distribution** 17 | 18 | We first show result of the base case where the sensory information is assumed to be known for comparison with other two implemented controllers where the sensory information is not know in advance. The final configuration in this case is the optimal configuration. 19 | 20 | ![Controller with known sensory distribution](plots/base_controller.png) 21 | 22 | * **Basic (Non-concensus) controller** 23 | 24 | In the basic controller we see that robot density near the peaks of the two gaussians represented by red \*. In this case each robot updated its sensory function estimate only based on its own sensor information. Also note from the third plot that the parameter convergence is to true parameter vector a is slow so the final configuration is near-optimal and not optimal. 25 | 26 | ![Non-Consensus controller](plots/non_consensus_130_0.png) 27 | 28 | * **Concensus controller** 29 | 30 | In the consensus controller case each robot updated its sensory function estimate based on its own sensor information as well as the sensory estimates of its neighbors on the 31 | delauny triangulation graph. We can see that the robots achieve final configuration similar to that of the optmial configuration achieved with exactly known 32 | sensory information. This can also be seen from the third plot(paramter convergence) where the mean of norm of parameter error converges to 0. Hence we achieve near optimal configuration. 33 | 34 | ![Consensus controller](plots/consensus_65_15.png) 35 | 36 | * **Position error convergence** 37 | 38 | The estimated position error is the distance of the robot from the centroid of its voronoi cell that is calculated using the current paramter estimate at a given time step. The true position error is the distance of the robot from the centroid of its voronoi cell that is calculated using the true paramter distribution at a given time step. The estimated position error and true position error averaged over all the robots in the network is shown for the network of 20 robots for both the basic and parameter 39 | consensus controllers that were implemented. 40 | 41 | ![Position error from centroid](plots/consensus_65_15.png) 42 | 43 | **Note:** The true position error converges to within 0.01 distance error only for the parameter consensus controller. However, in accordance with Theorem 1 and 2, the estimated 44 | error converges to zero in both cases. This shows that both controllers achieve near optimal configuration. However only the paramter consensus controller has trajectory 45 | rich enough to get accurate enough parameter estimate and achieve close to optimal final configuration. 46 | -------------------------------------------------------------------------------- /gifs/consensus_65_15.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ravipipaliya/coverage-control-networked-robots/935f14c86f72e167ce78706362441628f668b219/gifs/consensus_65_15.gif -------------------------------------------------------------------------------- /gifs/nonconsensus_130_0.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ravipipaliya/coverage-control-networked-robots/935f14c86f72e167ce78706362441628f668b219/gifs/nonconsensus_130_0.gif -------------------------------------------------------------------------------- /mfiles/VoronoiLimit.m: -------------------------------------------------------------------------------- 1 | function [V,C,XY]=VoronoiLimit(varargin) 2 | % -------------------------------------------------------------- 3 | % [V,C,XY]=VoronoiLimit(x,y,additional_variables) 4 | % Provides the Voronoi decomposition of a set of (x,y) data, but with all 5 | % vertices limited to the boundary created by the data itself. 6 | % V contains the vertices of all voronoi-cells and C contains all vertices for each individual 7 | % voronoi-cell. That is: V(C{ij},:) will give you the vertices of the ij'th cell, corresponding 8 | % to the ij'th data point, in two columns. The order of polygon vertices are given in a counter-clockwise 9 | % manner. XY contains updated xy coordinates as limited by any input boundaries. 10 | % 11 | % Addition variables: 12 | % 'bs_ext': Describe an arbitrary external boundary by giving an xy matrix of size (n,1) where n are number of vertices. 13 | % 'bs_int': Describe any number of arbitrary internal boundaries by giving a cell structure of M xy matrices of size 14 | % (Ni,1) where M are number of internal boundaries and Ni are number of vertices in the respective boundaries. 15 | % When defining a single boundary the algorithm automatically converts the given matrix into a cell structure. 16 | % (See examples below). 17 | % 'figure': output figure ('on'/'off'. Default='on'). 18 | % 19 | % EXAMPLES 20 | % Example 0: Run with no input to see graphical example. 21 | % 22 | % Example 1: External and one internal boundary 23 | % bs_int=[.2 .8 .8 .2;.6 .6 .2 .2]'; 24 | % bs_ext=[-.8 .5 1.80 -.8;-.05 1.7 -.05 -.05]'; 25 | % [X,Y] = meshgrid(-.5:.1:1.5, 0.1:.1:1.4); 26 | % X=X(:);Y=Y(:); 27 | % [V,C,XY]=VoronoiLimit(X,Y,'bs_ext',bs_ext,'bs_int',bs_int); 28 | % 29 | % Example 2: No external boundary and two internal boundaries 30 | % bs_int=cell(2,1); 31 | % bs_int{1}=[.2 .8 .8 .2;.6 .6 .2 .2]'; 32 | % bs_int{2}=[.2 .5 .7 .2;1 1 .7 .7]'; 33 | % [X,Y] = meshgrid(-.5:.1:1.5, 0.1:.1:1.4); 34 | % X=X(:);Y=Y(:); 35 | % [V,C,XY]=VoronoiLimit(X,Y,'bs_int',bs_int); 36 | % 37 | % Example 3: As above but without figure output 38 | % [V,C,XY]=VoronoiLimit(X,Y,'bs_int',bs_int,'figure','off'); 39 | % 40 | % Requires the Polybool function of the mapping toolbox to run!. 41 | % I recommend the tool 'export_fig' for exporting figures. It can be found here: 42 | % http://www.mathworks.com/matlabcentral/fileexchange/23629-export-fig 43 | % 44 | % Made by: Jakob Sievers (PhD, Arctic Geophysics) 45 | % Contact: Jakob.Sievers@gmail.com 46 | % -------------------------------------------------------------- 47 | warning('off','map:polygon:noExternalContours'); 48 | version=[3 0 2 2]; 49 | 50 | %% SETUP 51 | 52 | % USERTYPE 53 | [~,hostname]=system('hostname'); 54 | user=0; 55 | if strcmp(hostname(1:end-1),'DESKTOP-PC4MSAH') || strcmp(hostname(1:end-1),'Sievers') 56 | user=1; 57 | end 58 | 59 | 60 | % DETERMINE IF A MORE RECENT VERSION OF OOT IS AVAILABLE ON THE MATHWORKS FILEEXCHANGE 61 | try 62 | onlinedata = webread('http://se.mathworks.com/matlabcentral/fileexchange/34428-voronoilimit'); 63 | try_ver=1; 64 | catch me 65 | try_ver=0; 66 | end 67 | if try_ver==1 68 | try % using hardcoded version (faster) 69 | ixVersion=strfind(onlinedata,'version '); 70 | onlinedata=onlinedata(ixVersion+8:ixVersion+80); 71 | ixsspan=strfind(onlinedata,''); 72 | onlinedata=onlinedata(1:ixsspan(1)-1); 73 | ixsp=strfind(onlinedata,'('); 74 | onlinedata=onlinedata(1:ixsp-2); 75 | ixp=strfind(onlinedata,'.'); 76 | version_online=sum([str2double(onlinedata(1:ixp(1)-1))*1e3 str2double(onlinedata(ixp(1)+1:ixp(2)-1))*1e2 str2double(onlinedata(ixp(2)+1:ixp(3)-1))*1e1 str2double(onlinedata(ixp(3)+1:end))]); 77 | version=sum([version(1)*1e3 version(2)*1e2 version(3)*1e1 version(4)]); 78 | if version_online>version 79 | warndlg(['NOTE: A more recent version (ver. ',num2str(version_online(1,1)),'.',num2str(version_online(1,2)),'.',num2str(version_online(1,3)),'.',num2str(version_online(1,4)),') of VoronoiLimit is now available on the mathworks fileexchange. Currently running version ',num2str(version(1,1)),'.',num2str(version(1,2)),'.',num2str(version(1,3)),'.',num2str(version(1,4))]) 80 | pause(2) 81 | end 82 | catch me 83 | if user==1 84 | warndlg('CRASH #1: HARDCODED version of VoronoiLimit_ver script! Update hardcoded/online script!') 85 | pause(2) 86 | end 87 | try %download/unzip most recent script to determine online version (updated in case Mathworks has changed its website and hardcoded version crashes) 88 | outfilename = websave([pwd,filesep,'Current_version_VoronoiLimit_DONOTDELETE.m'],'https://www.dropbox.com/s/daqya2vv3hh9x9d/Current_version_VoronoiLimit_DONOTDELETE.m?dl=1'); 89 | Current_version_VoronoiLimit_DONOTDELETE(version); 90 | delete(outfilename) 91 | catch me2 92 | if user==1 93 | warndlg('CRASH #2: ONLINE version of VoronoiLimit_ver script! Update hardcoded/online script!') 94 | pause(2) 95 | end 96 | end 97 | end 98 | end 99 | 100 | 101 | %% ALGORITHM BEGINNING 102 | try 103 | if nargin==0 104 | val=600; 105 | x=rand(val,1); 106 | y=rand(val,1); 107 | XY=unique([x,y],'rows'); 108 | x=XY(:,1); 109 | y=XY(:,2); 110 | 111 | %EXTERNAL BOUNDARIES 112 | ButtonName = questdlg('Choose external boundary example:','','Irregular pentagon', 'Triangle', 'Irregular pentagon'); 113 | switch ButtonName 114 | case 'Irregular pentagon' 115 | bs_ext=[min(x)-std(x)/2 min(x)-std(x)/2 0.65 max(x)+std(x)/2 max(x)+std(x)/2 min(x)-std(x)/2;min(y)-std(y)/2 max(y)+std(y)/2 max(y)+std(y)/2 .65 min(y)-std(y)/2 min(y)-std(y)/2]'; 116 | case 'Triangle' 117 | bs_ext=[-.8 .5 1.80 -.8;-.05 1.7 -.05 -.05]'; 118 | end 119 | 120 | %INTERNAL OBJECTS 121 | bs_int=cell(3,1); 122 | rat=1.5; 123 | % rectangle 124 | bs_int{1}=[min(x)+(std(x)*rat) min(x)+(std(x)*rat) max(x)-std(x) max(x)-std(x) min(x)+(std(x)*rat);min(y)+std(y) max(y)-std(y) max(y)-std(y) min(y)+std(y) min(y)+std(y)]'; 125 | t = linspace(0,2*pi)'; 126 | % circle 1 127 | xc=.25; 128 | yc=.7; 129 | rad=.10; 130 | bs_int{2}=[(cos(t)*rad)+xc (sin(t)*rad)+yc]; 131 | % circle 2 132 | xc=.4; 133 | yc=.3; 134 | rad=.16; 135 | bs_int{3}=[(cos(t)*rad)+xc (sin(t)*rad)+yc]; 136 | fig='on'; 137 | else 138 | x=varargin{1}(:); 139 | y=varargin{2}(:); 140 | XY=unique([x,y],'rows'); 141 | x=XY(:,1); 142 | y=XY(:,2); 143 | for ii=3:2:nargin 144 | if strcmp(varargin{ii},'bs_ext') 145 | bs_ext=varargin{ii+1}; 146 | elseif strcmp(varargin{ii},'bs_int') 147 | bs_int=varargin{ii+1}; 148 | if ~iscell(bs_int) 149 | bs_int_cell=cell(1); 150 | bs_int_cell{1}=bs_int; 151 | bs_int=bs_int_cell; 152 | end 153 | elseif strcmp(varargin{ii},'figure') 154 | fig=varargin{ii+1}; 155 | end 156 | end 157 | if exist('fig','var')==0 158 | fig='on'; 159 | end 160 | end 161 | 162 | 163 | x=x(:); 164 | y=y(:); 165 | rx=[min(x) max(x)]; 166 | ry=[min(y) max(y)]; 167 | 168 | bnd=[rx ry]; %data bounds 169 | crs=double([bnd(1) bnd(4);bnd(2) bnd(4);bnd(2) bnd(3);bnd(1) bnd(3);bnd(1) bnd(4)]); %data boundary corners 170 | 171 | if exist('bs_ext','var') 172 | crs=bs_ext; 173 | end 174 | crslim=[min(crs(:,1)) max(crs(:,1)) min(crs(:,2)) max(crs(:,2))]; 175 | crslim_pol=[crslim(1) crslim(3) 176 | crslim(1) crslim(4) 177 | crslim(2) crslim(4) 178 | crslim(2) crslim(3) 179 | crslim(1) crslim(3)]; 180 | if ~any(size(x)==1) || ~any(size(y)==1) || numel(x)==1 || numel(y)==1 181 | disp('Input vectors should be single rows or columns') 182 | return 183 | end 184 | 185 | 186 | dt=delaunayTriangulation(x(:),y(:)); 187 | [V,C]=voronoiDiagram(dt); % This structure gives vertices for each individual point but is missing all "infinite" vertices 188 | [vx,vy]=voronoi(x,y); % This structure includes the "infinite" vertices but provides everything as a completele list of vertices rather than individually for each point. 189 | % Hence we need to add the missing vertices from vx and vy to the V and C structure. 190 | vxyl=[vx(:) vy(:)]; 191 | 192 | 193 | 194 | % combine identical V-entries 195 | epsx=eps(max(abs(crs(:))))*10; 196 | ctr=0; 197 | Vun=V; 198 | while ctr1 267 | ctr=ctr+1; 268 | mv2(ctr:ctr+lval-1,:)=[ones(lval,1).*mv(ii,1) ones(lval,1).*mv(ii,2)]; 269 | ctr=ctr+lval-1; 270 | end 271 | end 272 | end 273 | cpVixt=cell2mat(cpVixt); 274 | 275 | V=[V;mv2]; %add points to V structure 276 | 277 | 278 | %remove spurious double-entries in C/V structure 279 | epsx=eps(max(abs(crs(:))))*10; 280 | for ih=1:length(C) 281 | VC=V(C{ih},:); 282 | TMAT=true(size(VC,1)); 283 | for ii=1:size(VC,1) 284 | for ij=1:size(VC,1) 285 | TMAT(ii,ij)=all(abs(VC(ii,:)-VC(ij,:))<=epsx); 286 | end 287 | end 288 | TMAT=TMAT-eye(size(TMAT)); 289 | if any(TMAT(:)==1) 290 | if all(abs(V(C{ih}(1),:)-V(C{ih}(end),:))<=epsx) 291 | C{ih}(end)=[]; 292 | end 293 | ctr=0; 294 | while ctr=2 || length(ixb)>=2) 321 | % DO THE PROPOSED POINTS OBEY THE FOLLOWING RULES? 322 | % 1: The resulting shape must contain the original centroid 323 | % (0=does not contain centroid. 1=contains centroid) 324 | % 2: None of the end-sections may cross any existing section 325 | % (0=crossing. 1=no crossing) 326 | polygon=[V(C{ij},1),V(C{ij},2)]; 327 | if any(isinf(polygon(:))) 328 | polygon(isinf(sum(polygon,2)),:)=[]; 329 | end 330 | ixok=false(length(ixa),length(ixb),5); 331 | for ic1=1:length(ixa) 332 | for ic2=1:length(ixb) 333 | for ic3=0:4 334 | poly=[[V(lV0+ixa(ic1),1);polygon(:,1);V(lV0+ixb(ic2),1)],[V(lV0+ixa(ic1),2);polygon(:,2);V(lV0+ixb(ic2),2)]]; 335 | poly=unique(poly,'rows','stable'); 336 | if size(poly,1)>2 337 | if ic3>0 %with external point 338 | poly=[[V(lV0+ixa(ic1),1);polygon(:,1);V(lV0+ixb(ic2),1);exb_vertices(ic3,1)],[V(lV0+ixa(ic1),2);polygon(:,2);V(lV0+ixb(ic2),2);exb_vertices(ic3,2)]]; 339 | poly=unique(poly,'rows','stable'); 340 | end 341 | k = convhull(poly(:,1),poly(:,2)); 342 | A = polyarea(poly(:,1),poly(:,2)); 343 | B = polyarea(poly(k,1),poly(k,2)); 344 | if abs(A-B)lV0 403 | inpol=inpolygon(V(C{ij}(RC(ii,1)),1),V(C{ij}(RC(ii,1)),2),crs(:,1),crs(:,2)); 404 | if inpol 405 | if V(C{ij}(RC(ii,1)),1)==V(C{ij}(RC(ii,2)),1) %points aligned vertically (polyfit cannot be used) 406 | if V(C{ij}(RC(ii,1)),2)>V(C{ij}(RC(ii,2)),2) %point DIRECTLY above. Extend upward 407 | V(C{ij}(RC(ii,1)),2)=max(crs(:,2))+crsy/scale; 408 | else %point DIRECTLY below. Extend downward 409 | V(C{ij}(RC(ii,1)),2)=min(crs(:,2))-crsy/scale; 410 | end 411 | else %extend using polyfit 412 | plf=polyfit(V(C{ij}(RC(ii,:)),1),V(C{ij}(RC(ii,:)),2),1); 413 | if V(C{ij}(RC(ii,1)),1)>V(C{ij}(RC(ii,2)),1) %extend point beyond RIGHT boundary 414 | V(C{ij}(RC(ii,1)),1)=max(crs(:,1))+crsx/scale; 415 | V(C{ij}(RC(ii,1)),2)=polyval(plf,V(C{ij}(RC(ii,1)),1)); 416 | else %extend point beyond LEFT boundary 417 | V(C{ij}(RC(ii,1)),1)=min(crs(:,1))-crsx/scale; 418 | V(C{ij}(RC(ii,1)),2)=polyval(plf,V(C{ij}(RC(ii,1)),1)); 419 | end 420 | end 421 | end 422 | end 423 | end 424 | end 425 | 426 | 427 | 428 | % Polybool for restriction of polygons to domain. 429 | % Expand vertices when necessary! 430 | allVixinp=inpolygon(V(:,1),V(:,2),crs(:,1),crs(:,2)); %determine which points in V that are within the data boundaries. 431 | totalbounds=[min([min(V(~isinf(V(:,1)),1)),min(x),min(crs(:,1)),min(mv2(:,1))]) max([max(V(~isinf(V(:,1)),1)),max(x),max(crs(:,1)),max(mv2(:,1))]) min([min(V(~isinf(V(:,1)),2)),min(y),min(crs(:,2)),min(mv2(:,2))]) max([max(V(~isinf(V(:,1)),2)),max(y),max(crs(:,2)),max(mv2(:,2))])]; 432 | tbdx=diff(totalbounds(1:2)); 433 | tbdy=diff(totalbounds(3:4)); 434 | expandX=1; 435 | extremebounds=[totalbounds(1)-(tbdx*expandX) totalbounds(2)+(tbdx*expandX) totalbounds(3)-(tbdy*expandX) totalbounds(4)+(tbdy*expandX)]; 436 | 437 | 438 | Nint=4; 439 | exb_vertices_x=[linspace(extremebounds(1),extremebounds(2),Nint)';linspace(extremebounds(1),extremebounds(2),Nint)';ones(Nint,1)*extremebounds(1);ones(Nint,1)*extremebounds(2)]; 440 | exb_vertices_y=[ones(Nint,1)*extremebounds(3);ones(Nint,1)*extremebounds(4);linspace(extremebounds(3),extremebounds(4),Nint)';linspace(extremebounds(3),extremebounds(4),Nint)']; 441 | exb_vertices=[exb_vertices_x exb_vertices_y]; 442 | 443 | % exb_vertices=[extremebounds(1) extremebounds(4) 444 | % extremebounds(2) extremebounds(4) 445 | % extremebounds(2) extremebounds(3) 446 | % extremebounds(1) extremebounds(3)]; 447 | 448 | 449 | % STEP 1: categorize and apply polybool on all polygons who contain vertices outside the boundaries, but does not cross the boundaries between the first and last point 450 | poly_ok=zeros(size(C)); % 0=ok 451 | % 1=vertices outside boundaries but no crossing of boundary lines (resolve now) 452 | % 2=vertices outside boundaries AND crossing of boundary lines (resolve later) 453 | for ij=1:length(C) 454 | if sum(allVixinp(C{ij}))~=length(C{ij}) 455 | poly_ok(ij)=1; 456 | % Q: when drawing a line between the open ends of the polygon, does it intersect with the extreme (rectangle) data boundaries? 457 | % If so, connect the open ends to the extreme boundaries so that this is no longer the case. 458 | % The goal here is to expand the outer voronoi cells to include all of the domain of the given boundaries 459 | 460 | intersect=false(4,1); 461 | for ii=1:4 462 | [xip,~]=polyxpoly(crslim_pol(ii:ii+1,1),crslim_pol(ii:ii+1,2),V([C{ij}(1) C{ij}(end)],1),V([C{ij}(1) C{ij}(end)],2)); %intersections between lines 463 | if ~isempty(xip) 464 | intersect(ii)=true; 465 | end 466 | end 467 | if any(intersect) %possibly expand outer points 468 | poly_ok(ij)=2; 469 | end 470 | 471 | if poly_ok(ij)==1 472 | poly_ok(ij)=0; 473 | [xb, yb] = polybool('intersection',crs(:,1),crs(:,2),V(C{ij},1),V(C{ij},2)); 474 | % [xb, yb] = polybool('intersection',crs(:,1),crs(:,2),V(C1{ij},1),V(C1{ij},2)); 475 | ix=nan(1,length(xb)); 476 | for il=1:length(xb) 477 | if any(V(:,1)==xb(il)) && any(V(:,2)==yb(il)) 478 | ix1=find(V(:,1)==xb(il)); 479 | ix2=find(V(:,2)==yb(il)); 480 | for ib=1:length(ix1) 481 | if any(ix1(ib)==ix2) 482 | ix(il)=ix1(ib); 483 | end 484 | end 485 | if isnan(ix(il))==1 486 | lv=length(V); 487 | V(lv+1,1)=xb(il); 488 | V(lv+1,2)=yb(il); 489 | allVixinp(lv+1)=1; 490 | ix(il)=lv+1; 491 | end 492 | else 493 | lv=length(V); 494 | V(lv+1,1)=xb(il); 495 | V(lv+1,2)=yb(il); 496 | allVixinp(lv+1)=1; 497 | ix(il)=lv+1; 498 | end 499 | end 500 | C{ij}=ix; 501 | end 502 | end 503 | end 504 | 505 | 506 | % STEP 2: if any polygons remain evaluate whether by expanding them, they encroach on the territory of a neighboring polygon which has been accepted 507 | if any(poly_ok==2) 508 | ixpo=cell(2,1); 509 | for im=1:2 % im=1: run only the first three criteria to accept as many polygons based on this as possible. 510 | % im=2: run the remaining polygons with the final fourth criteria only 511 | ixpo{im}=find(poly_ok==2); 512 | if im==1 513 | diagnostics=zeros(size(exb_vertices,1)*size(exb_vertices,1)*5,length(ixpo{im})); 514 | elseif im==2 515 | diagnostics2=zeros(size(exb_vertices,1)*size(exb_vertices,1)*5,length(ixpo{im})); 516 | for in=1:length(ixpo{im}) 517 | diagnostics2(:,in)=diagnostics(:,ixpo{1}==ixpo{im}(in)); 518 | end 519 | diagnostics=diagnostics2; 520 | end 521 | 522 | if im==2 523 | ixpo_new=ixpo{im}; 524 | end 525 | for ik=1:length(ixpo{im}) 526 | if im==2 527 | % Determine neighboring polygons of all relevant polygons. 528 | % Iteratively sort and run these according to the highest ratio of neighboring polygons which have already been accepted. 529 | neighbors=cell(size(ixpo_new)); 530 | neighbors_ok_ratio=nan(length(ixpo_new),3); 531 | for ij=1:length(ixpo_new) 532 | for ii=1:length(C{ixpo_new(ij)}) 533 | for il=1:length(C) 534 | if any(C{il}==C{ixpo_new(ij)}(ii)) && il~=ixpo_new(ij) 535 | neighbors{ij}=[neighbors{ij};il]; 536 | end 537 | end 538 | 539 | end 540 | neighbors{ij}=unique(neighbors{ij}); 541 | neighbors_ok_ratio(ij,:)=[sum(poly_ok(neighbors{ij})==0)/numel(neighbors{ij}) ixpo_new(ij) ij]; 542 | end 543 | if length(ixpo_new)>1 544 | neighbors_ok_ratio=flipud(sortrows(neighbors_ok_ratio)); 545 | neighbors1=cell(size(neighbors)); 546 | for ij=1:length(ixpo_new) 547 | neighbors1{ij}=neighbors{neighbors_ok_ratio(ij,3)}; 548 | end 549 | neighbors=neighbors1; 550 | ixpo_new=neighbors_ok_ratio(:,2); 551 | else 552 | neighbors(neighbors_ok_ratio(:,2)~=ixpo_new)=[]; 553 | end 554 | end 555 | 556 | if im==1 557 | ij=ixpo{im}(ik); 558 | elseif im==2 559 | ij=ixpo_new(1); 560 | end 561 | 562 | % Q: when drawing a line between the open ends of the polygon, does it intersect with the extreme (rectangle) data boundaries? 563 | % If so, connect the open ends to the extreme boundaries so that this is no longer the case. 564 | % The goal here is to expand the outer voronoi cells to include all of the domain of the given boundaries 565 | % all combinations of connections between open ends and boundary limits are investigated to find the right one 566 | poly0=[V(C{ij},1),V(C{ij},2)]; 567 | 568 | polylog=cell(size(exb_vertices,1)*size(exb_vertices,1)*5,1); 569 | ctr=0; 570 | for iv1=1:size(exb_vertices,1) % extreme boundary vertices 571 | for iv2=1:size(exb_vertices,1) % extreme boundary vertices 572 | for iv3=0:4 % Optional (iv3=0) additional points (iv3=1:4) 573 | ctr=ctr+1; 574 | run=1; 575 | if im==2 && (diagnostics(ctr,ik)==0 || isnan(diagnostics(ctr,ik))) 576 | run=0; 577 | end 578 | %check all possible variations of connections between open ends and extreme boundary limits for the following traits: 579 | % 1) Area of convexhull of points equals area of raw points (i.e. "clean" shape) 580 | % 2) Does not contain any of the other centroids 581 | % 3) A line drawn between the open points does not intersect with the boundaries (as above) 582 | % 4) Does not encroach on the territory of any accepted (poly_ok==0) polygon 583 | 584 | % Define polygon 585 | if run==1 586 | if iv3==0 587 | poly=[exb_vertices(iv1,:);poly0;exb_vertices(iv2,:)]; 588 | else 589 | poly=[exb_vertices(iv1,:);poly0;exb_vertices(iv2,:);exb_vertices(iv3,:)]; 590 | end 591 | poly=unique(poly,'rows','stable'); 592 | polylog{ctr}=poly; 593 | end 594 | 595 | 596 | % if ctr>1 -> check that unique case hasn't been run before 597 | if ctr>1 && run==1 598 | polytest=nan(ctr-1,1); 599 | for ipt=1:length(polytest) 600 | polytest(ipt)=isequal(poly,polylog{ipt}); 601 | end 602 | if any(polytest) %do not run again 603 | run=0; 604 | diagnostics(ctr,ik)=nan; 605 | end 606 | end 607 | 608 | 609 | if run==1 610 | % TEST 1: Area of convex hull 611 | if im==1 612 | k = convhull(poly(:,1),poly(:,2)); 613 | A = polyarea(poly(:,1),poly(:,2)); 614 | B = polyarea(poly(k,1),poly(k,2)); 615 | if abs(A-B)1 645 | links{1}=poly(1:poly0ix,:); 646 | end 647 | if poly0ix+lp0-1index_rem(1)); 866 | if ~isempty(ixf) 867 | C{ij}(ixf)=C{ij}(ixf)-1; 868 | end 869 | end 870 | V(index_rem(1),:)=[]; 871 | index_rem=true(size(V,1),1); 872 | Ctot=unique(cell2mat(C)); 873 | Vn=V(Ctot,:); 874 | Vnix=find(any(isnan(Vn'))); 875 | if ~isempty(Vnix) 876 | Ctot(Vnix)=[]; 877 | end 878 | index_rem(Ctot)=false; 879 | index_rem=find(index_rem); 880 | end 881 | 882 | %Check and repair cells that have been split into closed sub-cells by input boundaries 883 | Csplit=cell(length(C),1); 884 | XYsplit=cell(length(C),1); 885 | splitlog=false(length(C),1); 886 | for ij=1:length(C) 887 | [xClosed, yClosed] = closePolygonParts(V(C{ij},1),V(C{ij},2)); 888 | if any(isnan(xClosed)) 889 | splitlog(ij)=true; 890 | ix=find(~isnan(xClosed)); 891 | diffix=diff(ix)>1; 892 | NUMcell=sum(isnan(xClosed))+1; 893 | Csplit{ij}=cell(NUMcell,1); 894 | XYsplit{ij}=nan(NUMcell,2); 895 | C_temp=C{ij}; 896 | ix_begin=1; 897 | for ik=1:NUMcell 898 | cs_diffix=cumsum(diffix); 899 | if ik>1 900 | ix_begin=2; 901 | end 902 | ix_end=find(cs_diffix>0,1,'first'); 903 | if isempty(ix_end) 904 | ix_end=length(xClosed); 905 | end 906 | Csplit{ij}{ik}=C_temp(ix_begin:ix_end); 907 | inpol=inpolygon(XY(ij,1),XY(ij,2),xClosed(ix_begin:ix_end),yClosed(ix_begin:ix_end)); 908 | if inpol==0 909 | XYsplit{ij}(ik,:)=[mean(xClosed(ix_begin:ix_end)) mean(yClosed(ix_begin:ix_end))]; 910 | else 911 | XYsplit{ij}(ik,:)=XY(ij,:); 912 | end 913 | if ik amin) = 0; 36 | % Iproji(ai(:,i) == amin & dai_pre >= 0) = 0; 37 | dai(:,i) = Tau*(dai_pre - diag(Iproji)*dai_pre); 38 | end 39 | 40 | % Update li and Li 41 | % w_t = exp(-t); 42 | for i = 1:n 43 | w_t = norm([dx(i) dy(i)])/norm(K); % Data weighting function 44 | ki = kappa(px(i),py(i)); 45 | phi_t = ki*a; 46 | dLi(:,:,i) = w_t*(ki'*ki); 47 | dli(:,i) = w_t*(phi_t)*ki'; 48 | end 49 | 50 | % State update 51 | dz = [dx; dy; dai(:); dli(:); dLi(:)]; 52 | 53 | %% Debugging ---------------------------- % 54 | disp(t) 55 | disp(mean(vecnorm(a-ai))) % paramter error 56 | % disp(norm([dx,dy])) % distance to centroid 57 | end -------------------------------------------------------------------------------- /mfiles/dintegrate.m: -------------------------------------------------------------------------------- 1 | % ----------------------------------------------------------------------- % 2 | % ----------------------------------------------------------------------- % 3 | % ----------------------------------------------------------------------- % 4 | 5 | function [Cvi,Cvi_true] = dintegrate(vx,vy,pi_t,ai_t,pos) 6 | global kappa K Fi a sens_info_flag; 7 | 8 | % Sample points 9 | cnt = 10000; % Sample count 10 | xp = min(vx) + rand(cnt,1)*(max(vx)-min(vx)); 11 | yp = min(vy) + rand(cnt,1)*(max(vy)-min(vy)); 12 | in = inpolygon(xp,yp,vx,vy); % subset points in voronoi region 13 | xp = xp(in); 14 | yp = yp(in); 15 | 16 | % Integrals over voronoi region 17 | kq = kappa(xp,yp); 18 | if(sens_info_flag) 19 | phi_est = kq * a; % optimal configuration case when sensory info is known 20 | else 21 | phi_est = kq * ai_t; % Case when sensory info is unknown and estimate is used 22 | end 23 | mv = sum(phi_est); 24 | cx = sum(xp.*phi_est)/mv; 25 | cy = sum(yp.*phi_est)/mv; 26 | phi = kq * a; 27 | cx_true = sum(xp.*phi)/sum(phi); 28 | cy_true = sum(yp.*phi)/sum(phi); 29 | Cvi_true = [cx_true;cy_true]; 30 | 31 | % Check for negative mass 32 | if mv < 0 33 | disp(strcat('Negative mass calculated: ',num2str(mv))); 34 | disp(pos) 35 | disp(ai_t') 36 | end 37 | 38 | % Check for calculated centroid in voronoi region 39 | if inpolygon(cx,cy,vx,vy) == 0 40 | disp('Centroid outside the region'); 41 | disp(ai_t'); 42 | Cvi = pi_t'; 43 | else 44 | Cvi = [cx;cy]; 45 | end 46 | 47 | % Update paramter Fi 48 | k1 = zeros(9,2); 49 | for i = 1:length(xp) 50 | q_pi = ([xp(i) yp(i)] - pi_t); 51 | k1 = k1 + kq(i,:)'*q_pi; 52 | end 53 | Fi(:,:,pos) = (1/mv)*(k1*K*k1'); 54 | 55 | end -------------------------------------------------------------------------------- /mfiles/main.m: -------------------------------------------------------------------------------- 1 | % MAE 598 Multi robot systems 2 | % Project - Ravi Pipaliya 3 | % Distributed Adaptive coverage control 4 | % 11/22/2020 5 | % ----------------------------------------------------------------------- % 6 | % ----------------------------------------------------------------------- % 7 | % ----------------------------------------------------------------------- % 8 | addpath('mfiles\','ODE_Solvers\') 9 | clear all; 10 | rng('default'); % Setting seed for reproducibility 11 | global n K Tau g psi h; 12 | global amin Fi kappa a sens_info_flag; 13 | global est_pos_err tru_pos_err; 14 | 15 | %% Initialize ----% 16 | n = 20; % # of robots 17 | K = 3*eye(2); % Control gain matrix 18 | Tau = eye(9); 19 | g = 130; % Learning rate <100 20 | psi = 0; % Consensus weight <20 21 | h = 0.01; % ode step size 22 | sens_info_flag = 0; % Set 1 to run the case when sensory info is known 23 | 24 | %% Initial Positions ----% 25 | x0 = rand(n,1); % Initial x 26 | y0 = rand(n,1); % Initial y 27 | 28 | %% Model paramters ----% 29 | amin = 0.1; % minimum weight 30 | a = [100 amin*ones(1,7) 100]'; % True weights 31 | ai = amin * ones(9,n); % parameter estimate with each robot 32 | li = zeros(9,n); 33 | Li = zeros(9,9,n); 34 | Fi = zeros(9,9,n); 35 | 36 | %% Sensory basis function ----% 37 | syms qx qy; 38 | sigma = 0.18; % Gaussian sd 39 | mu = [1 1 1 3 3 3 5 5 5; 1 3 5 1 3 5 1 3 5]/6; % Gaussian means 40 | kappa = @(qx,qy) ... 41 | [1/(sigma^2*(2*pi))*exp(-((qx-mu(1,1)).^2 + (qy-mu(2,1)).^2) /(2*sigma^2)),... 42 | 1/(sigma^2*(2*pi))*exp(-((qx-mu(1,2)).^2 + (qy-mu(2,2)).^2) /(2*sigma^2)),... 43 | 1/(sigma^2*(2*pi))*exp(-((qx-mu(1,3)).^2 + (qy-mu(2,3)).^2) /(2*sigma^2)),... 44 | 1/(sigma^2*(2*pi))*exp(-((qx-mu(1,4)).^2 + (qy-mu(2,4)).^2) /(2*sigma^2)),... 45 | 1/(sigma^2*(2*pi))*exp(-((qx-mu(1,5)).^2 + (qy-mu(2,5)).^2) /(2*sigma^2)),... 46 | 1/(sigma^2*(2*pi))*exp(-((qx-mu(1,6)).^2 + (qy-mu(2,6)).^2) /(2*sigma^2)),... 47 | 1/(sigma^2*(2*pi))*exp(-((qx-mu(1,7)).^2 + (qy-mu(2,7)).^2) /(2*sigma^2)),... 48 | 1/(sigma^2*(2*pi))*exp(-((qx-mu(1,8)).^2 + (qy-mu(2,8)).^2) /(2*sigma^2)),... 49 | 1/(sigma^2*(2*pi))*exp(-((qx-mu(1,9)).^2 + (qy-mu(2,9)).^2) /(2*sigma^2))]; 50 | 51 | %% Simulation ----% 52 | tspan = 0:h:30; 53 | est_pos_err = zeros(length(tspan),1); 54 | tru_pos_err = zeros(length(tspan),1); 55 | 56 | z0 = [x0; y0; ai(:); li(:); Li(:)]; % Initial state 57 | z = ode1(@cvtODE,tspan,z0); % Fixed time step ode 58 | 59 | %% Save/Load workspace ----% 60 | % Executed takes long so save workspace to compare iterations 61 | 62 | % save('output/base_z.mat') % Sensory distribution is known 63 | % save('output/z_130_0_err.mat') % Basic non concensus g = 130; psi = 0 64 | % save('output/z_65_15_err.mat') % Concensus g = 65; psi = 15 65 | 66 | % load('output/base_z.mat') % Sensory distribution is known 67 | % load('output/z_130_0_err.mat') % Basic non concensus g = 130; psi = 0 68 | % load('output/z_65_15_err.mat') % Concensus g = 65; psi = 15 69 | 70 | %% Decompose state ----% 71 | par_err = zeros(size(z,1),1); 72 | pxi = zeros(n,size(z,1)); 73 | pyi = zeros(n,size(z,1)); 74 | for i = 1:size(z,1) 75 | [pxi(:,i),pyi(:,i),ain] = reshape_state(z(i,:)'); 76 | par_err(i) = mean(vecnorm(a-ain)); 77 | end 78 | 79 | %% Plots ----% 80 | % a. Initial configuration 81 | figure 82 | subplot(2,2,1) 83 | voronoi(x0,y0,'b.') 84 | hold on 85 | plot(mu(1,1),mu(2,1),'r*') 86 | plot(mu(1,9),mu(2,9),'r*') 87 | title('Initial configuration') 88 | 89 | % b. Final configuration 90 | subplot(2,2,2) 91 | voronoi(pxi(:,end),pyi(:,end),'b.') 92 | hold on 93 | plot(mu(1,1),mu(2,1),'r*') 94 | plot(mu(1,9),mu(2,9),'r*') 95 | title('Final configuration') 96 | 97 | % c. Parameter convergence analysis 98 | subplot(2,2,3) 99 | plot(tspan,par_err,'-r') 100 | title('Parameter convergence') 101 | ylabel('$$Mean ||\tilde{a}_i(t)||$$','interpreter','latex') 102 | xlabel('time') 103 | ylim([0,150]) 104 | 105 | % d. Robot trajectories 106 | subplot(2,2,4) 107 | hold on; 108 | for i=1:n 109 | plot(pxi(i,end)',pyi(i,end)','ko') 110 | plot(pxi(i,:)',pyi(i,:)','--') 111 | end 112 | title('Robot trajectories') 113 | 114 | %% Gifs ----% 115 | % Defining the range of axes 116 | % ax = axes('XLim',[0 1],'YLim',[0 1]); 117 | cf = gcf; 118 | filename = 'gifs/consensus_test.gif'; 119 | for i = 1:size(pxi,2) 120 | hold on 121 | voronoi(pxi(:,i),pyi(:,i),'b.') 122 | plot(mu(1,1),mu(2,1),'r*') 123 | plot(mu(1,9),mu(2,9),'r*') 124 | title(strcat('t = ',num2str(tspan(i)))) 125 | xlabel('Consensus controller') 126 | xlim([0,1]) 127 | ylim([0,1]) 128 | hold off 129 | drawnow 130 | % Capture the plot as an image 131 | frame = getframe(cf); 132 | im = frame2im(frame); 133 | [imind,cm] = rgb2ind(im,256); 134 | % Write to the GIF File 135 | if i == 1 136 | imwrite(imind,cm,filename, 'gif', 'Loopcount',inf); 137 | else 138 | imwrite(imind,cm,filename,'gif','DelayTime',0.0001,'WriteMode','append'); 139 | end 140 | clf; 141 | end 142 | 143 | %% Comparing Position error (Basic non concesus vs concensus controller) 144 | load('output/z_130_0_err.mat') % non concensus g = 130; psi = 0 145 | est_pos_err_nc = est_pos_err; 146 | tru_pos_err_nc = tru_pos_err; 147 | load('output/z_65_15_err.mat') % concensus g = 65; psi = 15 148 | tx = tspan(:,1:end-1); 149 | ty = 1:size(tx,2); 150 | 151 | % Estimated position error 152 | figure 153 | subplot(1,2,1) 154 | plot(tx,est_pos_err_nc(ty), 'k') 155 | hold on; 156 | plot(tx,est_pos_err(ty),'r') 157 | title('Mean Estimated Position Error') 158 | legend('Basic', 'Consensus') 159 | ylabel('$$||\hat{C}_{V_i} - p_i||$$','interpreter','latex') 160 | xlabel('time') 161 | 162 | % True position error 163 | subplot(1,2,2) 164 | plot(tx,tru_pos_err_nc(ty),'k') 165 | hold on 166 | plot(tx,tru_pos_err(ty), 'r') 167 | title('Mean True Position Error') 168 | legend('Basic', 'Consensus') 169 | ylabel('$$||C_{V_i} - p_i||$$','interpreter','latex') 170 | xlabel('time') -------------------------------------------------------------------------------- /mfiles/reshape_state.m: -------------------------------------------------------------------------------- 1 | % ----------------------------------------------------------------------- % 2 | % ----------------------------------------------------------------------- % 3 | % ----------------------------------------------------------------------- % 4 | 5 | function [px_rs,py_rs,ai_rs,li_rs,Li_rs] = reshape_state(z) 6 | global n; 7 | sz = [n,n,9*n,9*n,9*9*n]; 8 | en = 0; 9 | 10 | st = en+1; en = sum(sz(1)); 11 | px_rs = z(st:en,1); 12 | 13 | st = en+1; en = sum(sz(1:2)); 14 | py_rs = z(st:en,1); 15 | 16 | st = en+1; en = sum(sz(1:3)); 17 | ai_rs = reshape(z(st:en,1),9,n); 18 | 19 | st = en+1; en = sum(sz(1:4)); 20 | li_rs = reshape(z(st:en,1),9,n); 21 | 22 | st = en+1; en = sum(sz(1:5)); 23 | Li_rs = reshape(z(st:en,1),9,9,n); 24 | 25 | % all(x0==px_rs) 26 | % all(y0==py_rs) 27 | % all(ai==ai_rs) 28 | % all(li==li_rs) 29 | % all(Li==Li_rs) 30 | end 31 | 32 | -------------------------------------------------------------------------------- /plots/base_controller.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ravipipaliya/coverage-control-networked-robots/935f14c86f72e167ce78706362441628f668b219/plots/base_controller.png -------------------------------------------------------------------------------- /plots/consensus_65_15.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ravipipaliya/coverage-control-networked-robots/935f14c86f72e167ce78706362441628f668b219/plots/consensus_65_15.png -------------------------------------------------------------------------------- /plots/non_consensus_130_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ravipipaliya/coverage-control-networked-robots/935f14c86f72e167ce78706362441628f668b219/plots/non_consensus_130_0.png -------------------------------------------------------------------------------- /plots/position_error_corrected.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ravipipaliya/coverage-control-networked-robots/935f14c86f72e167ce78706362441628f668b219/plots/position_error_corrected.png -------------------------------------------------------------------------------- /ref.bib: -------------------------------------------------------------------------------- 1 | @article{schwager2009decentralized, 2 | title={Decentralized, adaptive coverage control for networked robots}, 3 | author={Schwager, Mac and Rus, Daniela and Slotine, Jean-Jacques}, 4 | journal={The International Journal of Robotics Research}, 5 | volume={28}, 6 | number={3}, 7 | pages={357--375}, 8 | year={2009}, 9 | publisher={Sage Publications Sage UK: London, England} 10 | } --------------------------------------------------------------------------------