├── DKNS ├── appendConfigDKNS.m ├── dkns.m ├── kalmanFilter.m ├── maxConsensus.m ├── sensingRB.m └── sensingStandard.m ├── LICENSE ├── MASTT_main.m ├── README.md ├── TC ├── appendConfigTC.m ├── bidirectionalLink.m ├── createGraph.m ├── lmst.m └── topologyControl.m ├── _core ├── CDMTT.m ├── agent.m ├── createAgents.m ├── createTargets.m ├── target.m └── updateTargets.m ├── _gui ├── initialization.m └── simulate.m ├── _tools ├── dijkstra.m └── getNetDiameter.m ├── _viz ├── bearingControl.m ├── plotgauss2d.m ├── process_options.m ├── vizAgents.m ├── vizClean.m ├── vizConn.m ├── vizCovEll.m ├── vizEnv.m ├── vizEstimate.m └── vizTargets.m ├── gui.fig ├── gui.m └── images └── gui-example.png /DKNS/appendConfigDKNS.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % 3 | % Multi Agent Simulator for Target Tracking (MASTT) 4 | % 5 | % appendConfigDKNS.m 6 | % 7 | % Update the agents structure 8 | % 9 | %-------------------------------------------------------------------------% 10 | % 11 | % (c) 2009-2013 12 | % 13 | % A. Petitti 14 | % D. Di Paola 15 | % S. Giannini 16 | % 17 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 18 | 19 | function agents_dkns = appendConfigDKNS(agents, params, dkns_params) 20 | % 21 | % INPUTS: 22 | % agents = Agents structure 23 | % params = Global Parameters of the agent network structure 24 | % dkns_params = DKNS Parameters of the agent network structure 25 | % 26 | % OUTPUTS: 27 | % agents_dkns = updated agents structure 28 | % 29 | 30 | 31 | %-------------------------------------------------% 32 | % Update Agent structure 33 | % 34 | 35 | % Copy the agents structures 36 | agents_dkns = agents; 37 | 38 | 39 | for i = 1 : length(agents_dkns) 40 | 41 | %-------------------------------------------------% 42 | % Update Agent structure 43 | % 44 | agents_dkns(i).dkns.zone.x = agents_dkns(i).state.x; 45 | agents_dkns(i).dkns.zone.y = agents_dkns(i).state.y; 46 | 47 | agents_dkns(i).sens.isSensing = 0; 48 | 49 | 50 | %------------------------------------------------% 51 | % Kalman Filter (KF) Parameters 52 | % 53 | 54 | sigma_w = params.dt*dkns_params.sigma_0^2; 55 | Qi = [(params.dt^2/4)*sigma_w^2 (params.dt/2)*sigma_w^2; (params.dt/2)*sigma_w^2 sigma_w^2]; 56 | zero = zeros(2); 57 | Q = [Qi zero; zero Qi]; 58 | 59 | %Q = dkns_params.sigma_0^2 * eye(4); 60 | 61 | A = [1 params.dt 0 0;0 1 0 0;0 0 1 params.dt;0 0 0 1]; 62 | 63 | H = [1 0 0 0;0 0 1 0]; 64 | 65 | agents_dkns(i).dkns.KF.A = A; 66 | agents_dkns(i).dkns.KF.H = H; 67 | 68 | agents_dkns(i).dkns.KF.Q = Q; 69 | agents_dkns(i).dkns.KF.R = (agents_dkns(i).id)^(1/2)*eye(2); 70 | agents_dkns(i).dkns.KF.P = 10*(dkns_params.kw)^2*eye(4); 71 | 72 | agents_dkns(i).dkns.KF.x = [0.1 0 0.1 0]';%[randn() 0 randn() 0]'; 73 | agents_dkns(i).dkns.KF.z = [0 0]'; 74 | 75 | agents_dkns(i).dkns.KF.x_fusion = [0 0 0 0]'; 76 | 77 | 78 | end 79 | 80 | return -------------------------------------------------------------------------------- /DKNS/dkns.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % 3 | % Multi Agent Simulator for Target Tracking (MASTT) 4 | % 5 | % dkns.m 6 | % 7 | % Run all the phases of the distributed estimation algorithm for the 8 | % robotic network 9 | % 10 | %-------------------------------------------------------------------------% 11 | % 12 | % (c) 2009-2013 13 | % 14 | % A. Petitti 15 | % D. Di Paola 16 | % S. Giannini 17 | % 18 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 19 | 20 | function agents = dkns(agents, Nt, filterFlag) 21 | % 22 | % INPUTS: 23 | % agents = agents' structure 24 | % Nt = number of targets 25 | % filterFlag = filter flag 26 | % 27 | % OUTPUTS: 28 | % agents = updated agents' structure 29 | % 30 | 31 | 32 | %% Phase 1 : Estimation 33 | if strcmp(filterFlag,'kf') 34 | 35 | for i = 1 : length(agents) 36 | for j = 1 : Nt 37 | 38 | [x_estim, P_estim] = kalmanFilter(agents(i).dkns.KF(j), agents(i).sens.isSensing(j)); 39 | 40 | agents(i).dkns.KF(j).x = x_estim; 41 | agents(i).dkns.KF(j).P = P_estim; 42 | end 43 | end 44 | 45 | else 46 | error('MASF/_CORE :: CDMTT.m - No valid Filter Flag value'); 47 | end 48 | 49 | 50 | %% Phase 2 : Node Selection 51 | 52 | netDiameter = length(agents)-1;%getNetDiameter(); % Todo decentralized function 53 | agents = maxConsensus(agents, Nt, netDiameter); 54 | 55 | 56 | return -------------------------------------------------------------------------------- /DKNS/kalmanFilter.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % 3 | % Multi Agent Simulator for Target Tracking (MASTT) 4 | % 5 | % kalmanFilter.m 6 | % 7 | % Kalman Filter (with sensing/no sensing option) 8 | % 9 | %-------------------------------------------------------------------------% 10 | % 11 | % (c) 2009-2013 12 | % 13 | % A. Petitti 14 | % D. Di Paola 15 | % S. Giannini 16 | % 17 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 18 | 19 | function [x_estim, P_estim] = kalmanFilter(kf, isSensing) 20 | % 21 | % INPUTS: 22 | % kf = Kalman Filter Structure 23 | % isSensing = Sensing Flag 24 | % 25 | % OUTPUTS: 26 | % x_estim = estimation of the process state 27 | % P_estim = error covariance matrix 28 | % 29 | 30 | %Predicted (a priori) state estimate 31 | x_estim = kf.A * kf.x; 32 | 33 | %Predicted (a priori) estimate covariance 34 | P_estim = kf.A * kf.P * kf.A' + kf.Q; 35 | 36 | 37 | if isSensing 38 | % Optimal Kalman gain 39 | K = P_estim * kf.H' * (kf.H * P_estim * kf.H' + kf.R)^-1; 40 | 41 | % Updated (a posteriori) state estimate 42 | x_estim = x_estim + K * (kf.z - kf.H * x_estim); 43 | 44 | % Updated (a posteriori) estimate covariance 45 | I = eye(size(K,1), size(kf.H,2)); 46 | P_estim = (I - K * kf.H) * P_estim; 47 | end 48 | 49 | return -------------------------------------------------------------------------------- /DKNS/maxConsensus.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % 3 | % Multi Agent Simulator for Target Tracking (MASTT) 4 | % 5 | % ----------------------------------------------------------------------- 6 | % 7 | % 8 | % DKNS/maxConsensus.m 9 | % 10 | % MAx-consensus protocol 11 | % 12 | %-------------------------------------------------------------------------% 13 | % 14 | % (c) 2009-2013 15 | % 16 | % A. Petitti 17 | % D. Di Paola 18 | % S. Giannini 19 | % 20 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 21 | 22 | function agents = maxConsensus(agents, Nt, diam) 23 | % 24 | % INPUTS: 25 | % agents = agents' structure 26 | % Nt = number of targets 27 | % diam = length of the diameter of the network 28 | % 29 | % OUTPUTS: 30 | % agents = updated agents' structure 31 | % 32 | 33 | for t = 1 : diam 34 | 35 | % Communication 36 | [gamma_, x_, P_] = communication(agents, Nt); 37 | 38 | % Consesus 39 | for i = 1 : length(agents) 40 | % 41 | % MAX 42 | 43 | % for each Target 44 | for k=1:Nt 45 | 46 | gammaMax = gamma_(i,k); 47 | indMax = agents(i).id; 48 | 49 | % for each neighbour 50 | for j = 1:length(agents) 51 | if agents(i).tc.G(j) 52 | 53 | if gamma_(j,k) >= gammaMax 54 | 55 | if gamma_(j,k) == gammaMax 56 | if agents(j).id > indMax 57 | indMax = agents(j).id; 58 | agents(i).dkns.KF(k).x = x_{j,k}; 59 | agents(i).dkns.KF(k).P = P_{j,k}; 60 | end 61 | else 62 | gammaMax = gamma_(j,k); 63 | indMax = agents(j).id; 64 | agents(i).dkns.KF(k).x = x_{j,k}; 65 | agents(i).dkns.KF(k).P = P_{j,k}; 66 | end 67 | 68 | end 69 | end 70 | end 71 | end 72 | 73 | end 74 | end 75 | 76 | return 77 | 78 | %% Communication function 79 | function [gamma_, x_, P_] = communication(A, Nt) 80 | 81 | Na = length(A); 82 | 83 | % BUILD gamma_ from all agents 84 | gamma_ = zeros(Na,Nt); 85 | for i = 1 : Na 86 | for j = 1 : Nt 87 | gamma_(i,j) = 1/trace(A(i).dkns.KF(j).P); 88 | end 89 | end 90 | 91 | 92 | % BUILD x_ from all agents 93 | x_ = cell(Na,Nt); 94 | for i = 1 : Na 95 | for j = 1 : Nt 96 | x_{i,j} = A(i).dkns.KF(j).x; 97 | end 98 | end 99 | 100 | 101 | % BUILD P_ from all pNode 102 | P_ = cell(Na,Nt); 103 | for i = 1 : Na 104 | for j = 1 : Nt 105 | P_{i,j} = A(i).dkns.KF(j).P; 106 | end 107 | end 108 | 109 | 110 | return 111 | 112 | 113 | 114 | 115 | 116 | -------------------------------------------------------------------------------- /DKNS/sensingRB.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % 3 | % Multi Agent Simulator for Target Tracking (MASTT) 4 | % 5 | % sensingRB.m 6 | % 7 | % Sensing phase with Range Bearing Sensor model 8 | % 9 | %-------------------------------------------------------------------------% 10 | % 11 | % (c) 2009-2013 12 | % 13 | % A. Petitti 14 | % D. Di Paola 15 | % S. Giannini 16 | % 17 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 18 | 19 | function agents = sensingRB(agents,targets,dkns_params) 20 | % 21 | % INPUTS: 22 | % agents = agents' structure 23 | % targets = targets' structure 24 | % dkns_params = DKNS parameters of the agents' network structure 25 | % 26 | % OUTPUTS: 27 | % agents = updated agents' structure 28 | % 29 | 30 | for i = 1 : length(agents) 31 | 32 | theta = agents(i).state.theta; 33 | orientation = agents(i).sens.orientation; 34 | angle = agents(i).sens.angle; 35 | 36 | for j = 1 : length(targets) 37 | 38 | % Questo IF ? da RIVEDERE!!! 39 | beta = atan2(targets(j).state.y-agents(i).state.y, targets(j).state.x-agents(i).state.x); 40 | dist = sqrt( (targets(j).state.x-agents(i).state.x)^2 + (targets(j).state.y-agents(i).state.y)^2 ); 41 | 42 | if sign(beta)>sign(theta) 43 | theta = theta + 2*pi; 44 | elseif sign(beta)= (theta + orientation - angle/2) ) 52 | 53 | 54 | 55 | % Update isSensing variable 56 | agents(i).sens.isSensing(j) = true; 57 | 58 | % Update variance value 59 | sigma_ro = dkns_params.RB.k_d*( 1 + ( exp(dkns_params.RB.k_rho*( dist-agents(i).sens.range)/agents(i).sens.range ))); 60 | sigma_beta = dkns_params.RB.k_theta*dist/agents(i).sens.range; 61 | 62 | % RB measures 63 | dist = dist + sigma_ro*(-.5+rand); 64 | beta = beta + sigma_beta*(-.5+rand); 65 | 66 | 67 | % Update the measurement 68 | agents(i).dkns.KF(j).z(1) = dist*cos(beta) + agents(i).state.x; 69 | agents(i).dkns.KF(j).z(2) = dist*sin(beta) + agents(i).state.y; 70 | 71 | 72 | % Update the covariance measurement matrix 73 | T = [cos(beta) -sin(beta); sin(beta) cos(beta)]; 74 | agents(i).dkns.KF(j).R = T*[sigma_ro^2 0; 0 dist^2*sigma_beta^2]*T'; 75 | 76 | 77 | else 78 | % Update isSensing variable 79 | agents(i).sens.isSensing(j) = false; 80 | 81 | end 82 | 83 | 84 | 85 | else 86 | % Update isSensing variable 87 | agents(i).sens.isSensing(j) = false; 88 | end 89 | end 90 | end -------------------------------------------------------------------------------- /DKNS/sensingStandard.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % 3 | % Multi Agent Simulator for Target Tracking (MASTT) 4 | % 5 | % sensingStandard.m 6 | % 7 | % Sensing phase with gaussian sensing error 8 | % 9 | %-------------------------------------------------------------------------% 10 | % 11 | % (c) 2009-2013 12 | % 13 | % A. Petitti 14 | % D. Di Paola 15 | % S. Giannini 16 | % 17 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 18 | 19 | function agents = sensingStandard(agents,targets,dkns_params) 20 | % 21 | % INPUTS: 22 | % agents = agents' structure 23 | % targets = targets' structure 24 | % dkns_params = DKNS parameters of the agents' network structure 25 | % 26 | % OUTPUTS: 27 | % agents = updated agents' structure 28 | % 29 | 30 | for i = 1:length(agents) 31 | 32 | theta = agents(i).state.theta; 33 | orientation = agents(i).sens.orientation; 34 | angle = agents(i).sens.angle; 35 | 36 | for j = 1 : length(targets) 37 | 38 | beta = atan2(targets(j).state.y-agents(i).state.y, targets(j).state.x-agents(i).state.x); 39 | dist = sqrt( (targets(j).state.x-agents(i).state.x)^2 + (targets(j).state.y-agents(i).state.y)^2 ); 40 | 41 | if sign(beta)>sign(theta) 42 | theta = theta + 2*pi; 43 | elseif sign(beta)= (theta + orientation - angle/2) ) 50 | 51 | 52 | 53 | % Update isSensing variable 54 | agents(i).sens.isSensing(j) = true; 55 | 56 | % Update the measurement 57 | agents(i).dkns.KF(j).z(1) = targets(j).state.x + dkns_params.ST.sigma*(-.5+rand); 58 | agents(i).dkns.KF(j).z(2) = targets(j).state.y + dkns_params.ST.sigma*(-.5+rand); 59 | 60 | % Update the covariance measurement matrix 61 | agents(i).dkns.KF(j).R(1,1) = dkns_params.ST.sigma^2; 62 | agents(i).dkns.KF(j).R(2,2) = dkns_params.ST.sigma^2; 63 | agents(i).dkns.KF(j).R(2,1) = 0; 64 | agents(i).dkns.KF(j).R(1, 2) = 0; 65 | 66 | else 67 | % Update isSensing variable 68 | agents(i).sens.isSensing(j) = false; 69 | end 70 | 71 | else 72 | % Update isSensing variable 73 | agents(i).sens.isSensing(j) = false; 74 | end 75 | 76 | end 77 | end -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2016 Antonio Petitti 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /MASTT_main.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % 3 | % Multi Agent Simulator for Target Tracking 4 | % 5 | % Part of the code is related to: 6 | % D.Di Paola, A.Petitti, A.Rizzo, Distributed Kalman Filtering via Node Selection in Heterogeneous Sensor Networks, 7 | % International Journal of Systems Science, pages 2572-2583, Volume 46, Issue 14, 2015. 8 | % 9 | % 10 | % (c) 2009-2013 11 | % 12 | % A. Petitti 13 | % D. Di Paola 14 | % S. Giannini 15 | % 16 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 17 | 18 | clear all; 19 | close all; 20 | clc; 21 | 22 | disp('--------------------------------------------------------------'); 23 | disp('| Multi Agent Simulator for Target Tracking(MASTT) |'); 24 | disp('--------------------------------------------------------------'); 25 | disp(' '); 26 | 27 | 28 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 29 | %% DKNS-User Parameters Initialization 30 | % 31 | 32 | %-----------------------------------% 33 | % Setup Paths 34 | % 35 | addpath('./_tools'); 36 | addpath('./DKNS'); 37 | addpath('./TC'); 38 | 39 | % 40 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 41 | 42 | 43 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 44 | %% MASF-Parameters initialization 45 | % 46 | 47 | 48 | %-----------------------------------% 49 | % VISUALIZATIONS 50 | % 51 | 52 | addpath('./_viz'); 53 | 54 | 55 | % 56 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 57 | 58 | 59 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 60 | %% MASF-Core setup 61 | % 62 | 63 | addpath('./_core'); 64 | 65 | disp('- Initialization core parameters ... '); 66 | 67 | 68 | %-----------------------------------% 69 | % Init Structures 70 | % 71 | 72 | ENV = []; 73 | OBS = []; 74 | PARAMS = []; 75 | AGENTS = []; 76 | VIZ_PARAMS = []; 77 | 78 | 79 | %-----------------------------------% 80 | % ENVIROMENT PARAMETERS 81 | % 82 | ENV.W = 90; % Environment dimension 83 | ENV.H = 90; 84 | ENV.L = max([ENV.W ENV.H]); 85 | ENV.mode = 'closed'; % closed / toroid 86 | 87 | 88 | %-----------------------------------% 89 | % AGENTS PARAMETERS 90 | % 91 | 92 | PARAMS.Nu = 18; % Number of Sensing Units 93 | 94 | % Agents Positioning Mode 95 | % random = random free position in the environment 96 | % grid = grid-like pattern position in the environment 97 | % 98 | PARAMS.POS_MODE = 'random'; 99 | 100 | % Communication System Mode 101 | % default = isotropic communication area defined by a range 102 | % os = Olfati-Saber paper range definition 103 | % 104 | PARAMS.COMM_MODE = 'default'; 105 | 106 | % Sensing System Mode 107 | % iso = isotropic sensing area (FOV = 360??) 108 | % sector = sector sensing area (FOV defined by range and angle) 109 | % 110 | PARAMS.SENS_MODE = 'sector'; 111 | 112 | % Sensing System Type 113 | % homo = homogeneous angles and ranges 114 | % hetero = heterogeneous angles and ranges 115 | % 116 | PARAMS.SENS_TYPE = 'hetero'; 117 | 118 | % Sensing System Model 119 | % rb = range bearing sensor model 120 | % standard = standard KF-model 121 | % 122 | PARAMS.SENS_MODEL = 'rb'; 123 | 124 | disp('- Create agent structures ... '); 125 | 126 | AGENTS = createAgents(PARAMS.Nu,ENV,PARAMS); 127 | 128 | 129 | %-----------------------------------% 130 | % GENERAL PARAMETERS 131 | % 132 | 133 | % Simulation Params 134 | PARAMS.Nt = 1; % Number of Targets 135 | PARAMS.n_iters = 300; % Iterations for each experiment 136 | PARAMS.dt = 0.04; % Time Step 137 | 138 | 139 | %-----------------------------------% 140 | % VISUALIZATION PARAMETERS 141 | % 142 | VIZ_PARAMS.plotSensors = 'on'; 143 | 144 | % 145 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 146 | 147 | 148 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 149 | %% DKNS-Specific setup 150 | % 151 | 152 | disp('- Initialization specific DKNS parameters ... '); 153 | 154 | %-----------------------------------% 155 | % DKNS Params 156 | % 157 | DKNS_PARAMS.Nt = 1;%PARAMS.Nt; 158 | 159 | % Target Moving Model Parameters 160 | DKNS_PARAMS.c1 = 0.75; 161 | DKNS_PARAMS.c2 = 1; 162 | DKNS_PARAMS.a = 33; 163 | % Kalman Filter Parameters 164 | DKNS_PARAMS.kw = 5; 165 | DKNS_PARAMS.kv = 3; 166 | DKNS_PARAMS.sigma_0 = 5; 167 | % Sensing Parameters 168 | DKNS_PARAMS.RB.k_theta = 1/10; 169 | DKNS_PARAMS.RB.k_d = 1.056; 170 | DKNS_PARAMS.RB.k_rho = 10.07; 171 | DKNS_PARAMS.ST.sigma = 5; 172 | 173 | % 174 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 175 | 176 | 177 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 178 | %% DKNS-MainLoop 179 | % 180 | 181 | %-----------------------------------% 182 | % Append Configurations to Agents strucutures 183 | % 184 | AGENTS = appendConfigTC(AGENTS); 185 | 186 | %-----------------------------------% 187 | % Simulation Core 188 | % 189 | vizMain = vizEnv(ENV); 190 | 191 | % Setup network for this experiment 192 | AGENTS_DKNS = appendConfigDKNS(AGENTS, PARAMS, DKNS_PARAMS); 193 | 194 | % Setup targets for this experiment 195 | TARGETS = createTargets(ENV, DKNS_PARAMS); 196 | 197 | reverseStr = ''; 198 | fprintf('\n'); 199 | disp('* Start Experiment *'); 200 | fprintf('\n'); 201 | for k_iters = 1 : PARAMS.n_iters % Single Experiment 202 | 203 | % Display the progress 204 | percentDone = 100 * k_iters / PARAMS.n_iters; 205 | msg = sprintf('Completed ... %3.1f', percentDone); 206 | fprintf([reverseStr, msg]); 207 | reverseStr = repmat(sprintf('\b'), 1, length(msg)); 208 | 209 | %% Distributed Estimation Algorithm 210 | AGENTS_DKNS = CDMTT(AGENTS_DKNS, TARGETS, PARAMS, DKNS_PARAMS); 211 | 212 | %% Update target positions 213 | TARGETS = updateTargets(PARAMS, DKNS_PARAMS, TARGETS); 214 | 215 | AGENTS_DKNS = bearingControl(AGENTS_DKNS); 216 | vizMain = vizClean(vizMain); 217 | vizMain = vizTargets(vizMain, TARGETS); 218 | vizMain = vizConn(vizMain,AGENTS_DKNS); 219 | vizMain = vizAgents(vizMain,AGENTS_DKNS,PARAMS,VIZ_PARAMS.plotSensors); 220 | vizMain = vizEstimate(vizMain, AGENTS_DKNS, ENV); 221 | vizMain = vizCovEll(vizMain, AGENTS_DKNS(1).dkns.KF(1).x, AGENTS_DKNS(1).dkns.KF(1).P); 222 | pause(.1); 223 | 224 | end 225 | 226 | fprintf('\n\n'); 227 | disp('* Stop Experiment *'); 228 | 229 | 230 | 231 | % 232 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 233 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MASTT 2 | Multi Agent Simulator for Target Tracking 3 | 4 | Part of the code is related to: 5 | D.Di Paola, A.Petitti, A.Rizzo, Distributed Kalman Filtering via Node Selection in Heterogeneous Sensor Networks, 6 | International Journal of Systems Science, pages 2572-2583, Volume 46, Issue 14, 2015. 7 | 8 | # Run 9 | ## From script 10 | Run MASTT_main.m to view a target tracking demo 11 | 12 | ## From GUI 13 | Run the script gui.m to open a graphical user interface 14 | ![alt tag](https://raw.githubusercontent.com/gitAnto/MASTT/master/images/gui-example.png) 15 | 16 | # MATLAB Version 17 | This software has been tested with MATLAB R2013a 18 | 19 | It should work also with versions greater than R2013a 20 | 21 | # Contacts 22 | Write at ing.petitti@gmail.com for any issues and/or feedback 23 | -------------------------------------------------------------------------------- /TC/appendConfigTC.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % 3 | % Multi Agent Simulator for Target Tracking (MASTT) 4 | % 5 | % ----------------------------------------------------------------------- 6 | % 7 | % 8 | % TC/appendConfigTC.m 9 | % 10 | % % Comment 11 | % 12 | %-------------------------------------------------------------------------% 13 | % 14 | % (c) 2009-2013 15 | % 16 | % A. Petitti 17 | % D. Di Paola 18 | % S. Giannini 19 | % 20 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 21 | 22 | function agents_tc = appendConfigTC(agents)%, tc_params) 23 | % 24 | % INPUTS: 25 | % agents = Agents structure 26 | % tc_params = TC Parameters of the agent network structure 27 | % 28 | % OUTPUTS: 29 | % agents_tc = updated agents structure 30 | % 31 | 32 | 33 | %-------------------------------------------------% 34 | % Update Agent structure 35 | % 36 | 37 | % Copy the agents structures 38 | agents_tc = agents; 39 | 40 | 41 | for i = 1 : length(agents_tc) 42 | 43 | agents_tc(i).tc.G = zeros(1,length(agents_tc)); % k-th row of the adjacency matrix 44 | agents_tc(i).tc.W = ones(1,length(agents_tc))*inf; % k-th row of the proximity graph's matrix 45 | agents_tc(i).tc.path = ones(length(agents_tc),2)*inf; % local tree 46 | agents_tc(i).tc.pos = ones(length(agents_tc),1)*inf; % neighbours' location 47 | agents_tc(i).tc.link = zeros(length(agents_tc)); % link to be preserved 48 | agents_tc(i).tc.neig.num = 0; % number of neighbours 49 | 50 | end 51 | 52 | 53 | for k = 1:length(agents_tc) 54 | agents_tc = createGraph(agents_tc,k); 55 | agents_tc = topologyControl(agents_tc,k); 56 | end 57 | 58 | 59 | 60 | 61 | return 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /TC/bidirectionalLink.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % 3 | % Multi Agent Simulator for Target Tracking (MASTT) 4 | % 5 | % ----------------------------------------------------------------------- 6 | % 7 | % 8 | % TC/bidirectionalLink.m 9 | % 10 | % % Noticing to my neighbours who's in my lmst 11 | % 12 | %-------------------------------------------------------------------------% 13 | % 14 | % (c) 2009-2013 15 | % 16 | % A. Petitti 17 | % D. Di Paola 18 | % S. Giannini 19 | % 20 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 21 | 22 | 23 | function Ag = bidirectionalLink(Ag,u) 24 | % 25 | % INPUTS: 26 | % Ag = Agents structure 27 | % u = agent's id 28 | % 29 | % OUTPUTS: 30 | % Ag = Agents structure 31 | % 32 | 33 | Nu = size(Ag(u).tc.path,1); 34 | for i = 1:Nu 35 | if(Ag(u).tc.G(1,i) && Ag(u).tc.path(i,1)~=inf && Ag(u).tc.path(i,1)~=0) 36 | Ag(i).tc.link(Ag(u).tc.path(i,1),i)=1; 37 | Ag(i).tc.link(i,Ag(u).tc.path(i,1))=1; 38 | Ag(Ag(u).tc.path(i,1)).link(Ag(u).tc.path(i,1),i)=1; 39 | Ag(Ag(u).tc.path(i,1)).link(i,Ag(u).tc.path(i,1))=1; 40 | end 41 | end 42 | -------------------------------------------------------------------------------- /TC/createGraph.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % 3 | % Multi Agent Simulator for Target Tracking (MASTT) 4 | % 5 | % ----------------------------------------------------------------------- 6 | % 7 | % 8 | % TC/createGraph.m 9 | % 10 | % % Communication graph's creation. 11 | % 12 | %-------------------------------------------------------------------------% 13 | % 14 | % (c) 2009-2013 15 | % 16 | % A. Petitti 17 | % D. Di Paola 18 | % S. Giannini 19 | % 20 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 21 | 22 | function Ag = createGraph(Ag,a) 23 | % 24 | % INPUTS: 25 | % Ag = Agents structure 26 | % a = agent's id 27 | % 28 | % OUTPUTS: 29 | % Ag = Agents structure 30 | % 31 | 32 | %-----------------------------------% 33 | % Adjacency matrix 34 | % 35 | Nu = size(Ag(a).tc.G,2); 36 | 37 | p = zeros(1,Nu); 38 | temp = zeros(Nu,1); 39 | m = 5; 40 | 41 | for k = 1 : Nu 42 | p(k) = Ag(k).state.x + 1i * Ag(k).state.y; 43 | end 44 | 45 | 46 | dp = p - p(a); 47 | dist = sqrt(dp .* conj(dp)); 48 | vedo = dist <= Ag(a).comm.range; % if 1, it can communicate with agent a 49 | 50 | for k = 1 : Nu 51 | if(vedo(k)) 52 | temp(k) = p(k); % who are my neighbours? 53 | end 54 | end 55 | 56 | root=a; 57 | 58 | Ag(a).tc.G(1,:) = vedo; 59 | Ag(a).tc.G(1,a) = 0; 60 | Ag(a).tc.neig.num= sum((Ag(a).tc.G)); 61 | 62 | 63 | %-----------------------------------% 64 | % r-disk proximity graph computation 65 | % weight function: f(p)=||p||*(r-||p||)^(-m) 66 | % 67 | for i=1:Nu 68 | if(Ag(a).tc.G(1,i)||i==a) 69 | Ag(a).tc.W(1,i) = dist(1,i)/(Ag(a).sens.range-dist(1,i))^m; 70 | end 71 | end 72 | 73 | %-----------------------------------% 74 | % a is the root of its local tree 75 | % 76 | Ag(a).tc.path(root,1) = 0; 77 | Ag(a).tc.path(root,2) = 0; 78 | Ag(a).tc.pos(:,1) = temp(:,1); % neighbours' location 79 | 80 | return 81 | -------------------------------------------------------------------------------- /TC/lmst.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % 3 | % Multi Agent Simulator for Target Tracking (MASTT) 4 | % 5 | % ----------------------------------------------------------------------- 6 | % 7 | % 8 | % TC/lmst.m 9 | % 10 | % % Finding the candidate link to be added to the local minimum spanning tree. 11 | % 12 | %-------------------------------------------------------------------------% 13 | % 14 | % (c) 2009-2013 15 | % 16 | % A. Petitti 17 | % D. Di Paola 18 | % S. Giannini 19 | % 20 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 21 | 22 | function linked = lmst(edges) 23 | % 24 | % INPUTS: 25 | % edges = edges' structure 26 | % 27 | % OUTPUTS: 28 | % linked = candidate edge 29 | % 30 | 31 | %-----------------------------------% 32 | % edges sorted by cost 33 | % 34 | [costs,I] = sort(edges(3,:)); 35 | linked=[]; 36 | k=0; 37 | for i = 1:size(I,2) 38 | if(edges(4,I(i))==1 && edges(5,I(i))~=0) 39 | k=k+1; 40 | linked(1,k)=edges(1,I(i)); 41 | linked(2,k)=edges(2,I(i)); 42 | linked(3,k)=edges(3,I(i)); 43 | linked(4,k)=edges(4,I(i)); 44 | linked(5,k)=edges(5,I(i)); 45 | end 46 | end 47 | 48 | if(size(linked,2)~=0) 49 | 50 | costo_min = linked(3,1); 51 | %-----------------------------------% 52 | % edges with the same cost, sorted by index 53 | % 54 | k=0; 55 | while(k1) 59 | [ind_max,II] = sort(linked(2,1:k)); 60 | link1(2,1:k)=ind_max; 61 | for i = 1:k 62 | link1(1,i)=linked(1,II(i)); 63 | link1(3,i)=linked(3,II(i)); 64 | link1(4,i)=linked(4,II(i)); 65 | link1(5,i)=linked(5,II(i)); 66 | end 67 | linked(:,1:k)=link1;% 68 | i_max = linked(2,1); 69 | m=0; 70 | while(m1) 74 | [ind_min,III] = sort(link1(1,1:m)); 75 | link2(1,1:m)=ind_min; 76 | for i = 1:m 77 | link2(2,i)=link1(2,III(i)); 78 | link2(3,i)=link1(3,III(i)); 79 | link2(4,i)=link1(4,III(i)); 80 | link2(5,i)=link1(5,III(i)); 81 | end 82 | linked(:,1:m)=link2; 83 | link1=[]; 84 | link2=[]; 85 | 86 | end 87 | 88 | end 89 | 90 | 91 | end 92 | -------------------------------------------------------------------------------- /TC/topologyControl.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % 3 | % Multi Agent Simulator for Target Tracking (MASTT) 4 | % 5 | % ----------------------------------------------------------------------- 6 | % 7 | % 8 | % TC/topologyControl.m 9 | % 10 | % % Control of network connectivity. 11 | % 12 | %-------------------------------------------------------------------------% 13 | % 14 | % (c) 2009-2013 15 | % 16 | % A. Petitti 17 | % D. Di Paola 18 | % S. Giannini 19 | % 20 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 21 | 22 | function Ag = topologyControl(Ag,u) 23 | % 24 | % INPUTS: 25 | % Ag = Agents structure 26 | % u = agent's id 27 | % 28 | % OUTPUTS: 29 | % Ag = Agents structure 30 | % 31 | Nu = size(Ag(u).tc.G,2); 32 | in = zeros(Nu,1); %1-> i link che partono dall'agente in(i,1) sono stati aggiunti all'insieme degli archi 33 | in(u,1)=1; 34 | edges=[]; 35 | m=5; 36 | 37 | 38 | %-----------------------------------% 39 | % save all the neighbours of the root node 40 | % 41 | l=0; 42 | for i = 1:Nu 43 | if(Ag(u).tc.G(1,i)) 44 | 45 | l=l+1; 46 | 47 | if(u edge to be visited 57 | edges(5,l)=inf; % 0 -> edge in a cycle 58 | end 59 | end 60 | 61 | if(size(edges,2)>0) 62 | 63 | tree=1; 64 | while(tree) 65 | 66 | linked=[]; 67 | linked=lmst(edges); 68 | 69 | z = size(linked,2); 70 | q = 0; 71 | 72 | if(z~=0) 73 | 74 | p=linked(1,1); 75 | s=linked(2,1); 76 | %-----------------------------------% 77 | % if there's a new node's neighbour, 78 | % links of the new node's neighbour are added 79 | % 80 | if(in(p,1)==0) 81 | in(p,1)=1; 82 | 83 | for i =1:Nu 84 | if(Ag(u).tc.G(1,i)) 85 | if(i~=p) 86 | if(in(i,1)==0) 87 | dp = Ag(u).tc.pos(p,1) - Ag(u).tc.pos(i,1); 88 | 89 | dist = sqrt(dp .* conj(dp)); 90 | if(dist<=Ag(u).comm.range) 91 | q=q+1; 92 | if(p number of agents set to %d \n', n_ag); 36 | 37 | end 38 | 39 | %% MASF Populating agents structure 40 | % 41 | for i = 1 : n_ag 42 | 43 | agents(i) = agent('id',i); % Create a new agent with a unique identifier 44 | agents(i).status = 'FIXED'; 45 | 46 | %-----------------------------------% 47 | % DraW Parameter 48 | % 49 | agents(i).draw.color = [1 1 0]; 50 | 51 | %-----------------------------------% 52 | % Physical Parameter 53 | % 54 | agents(i).state.vel = 0; % agent velocity 55 | agents(i).state.vel_x = 0; % agent velocity x component 56 | agents(i).state.vel_y = 0; % agent velocity y component 57 | 58 | agents(i).state.theta = pi*rand(); % agent orientation (radiant) 59 | agents(i).state.theta_t = 0; % number of iteration with the same theta 60 | 61 | agents(i).state.r = 0; % agent radius (only for circular agents) 62 | agents(i).state.d = 2; % agent occupied space 63 | 64 | agents(i).state.x = 0; % agent x coordinate 65 | agents(i).state.y = 0; % agent y coordinate 66 | agents(i).state.dr = 0; 67 | 68 | % Agents Positioning Function 69 | if( strcmp(params.POS_MODE,'random') || strcmp(params.POS_MODE,'grid') ) 70 | [agents(i).state.x, agents(i).state.y] = agentPositions(n, env, agents, params.POS_MODE); 71 | else 72 | error('MASF/_CORE :: createAgents.m - No valid Positioning Mode [POS_MODE] value'); 73 | end 74 | 75 | 76 | %-----------------------------------% 77 | % Communication System 78 | % 79 | if ( strcmp(params.COMM_MODE,'default') || strcmp(params.COMM_MODE,'os')) 80 | agents(i).comm.range = (env.L + 12*agents(i).state.d)/(round(sqrt(n))+1) + 2; 81 | else 82 | error('MASF/_CORE :: createAgents.m - No valid Communication Mode [COMM_MODE] value'); 83 | end 84 | 85 | 86 | %-----------------------------------% 87 | % Sensing System 88 | % 89 | 90 | % Common Parameters for omogeneous robots 91 | agents(i).sens.range = agents(i).state.d*4; 92 | agents(i).sens.angle = pi; % Sensor's field of view 93 | agents(i).sens.orientation = 0; % FOV = [orientation - angle/2; orientation + angle/2] w.r.t. agent's frame 94 | 95 | 96 | if( strcmp(params.SENS_MODE,'iso') ) 97 | agents(i).sens.angle = 2*pi; 98 | elseif( strcmp(params.SENS_MODE,'sector') ) 99 | % do nothing - maintain common parameters 100 | else 101 | error('MASF/_CORE :: createAgents.m - No valid Sensing Mode [SENS_MODE] value'); 102 | end 103 | 104 | if( strcmp(params.SENS_TYPE,'homo') ) 105 | % do nothing 106 | elseif( strcmp(params.SENS_TYPE,'hetero') ) 107 | agents(i).sens.range = agents(i).sens.range + agents(i).sens.range*.5*rand; 108 | if( ~strcmp(params.SENS_MODE,'iso') ) 109 | agents(i).sens.angle = agents(i).sens.angle + pi/4*randn; 110 | end 111 | agents(i).sens.orientation = 0; %agents(i).sens.orientation + pi/4*randn; 112 | else 113 | error('MASF/_CORE :: createAgents.m - No valid Sensing Type [SENS_TYPE] value'); 114 | end 115 | 116 | end 117 | 118 | return 119 | 120 | 121 | function [x_coord, y_coord] = agentPositions(n, env, agents, mode) 122 | %% Agents positioning function 123 | % 124 | agent_id = numel(agents); 125 | agent_diam = agents(agent_id).state.d; 126 | area_perc_red = 12; % percentage of reduction of the positioning area 127 | 128 | L_min = -(env.L/2) + (env.L/100*area_perc_red); 129 | L_max = env.L/2 - (env.L/100*area_perc_red); 130 | 131 | if( strcmp(mode,'random') ) 132 | % Random positioning 133 | x_coord = L_min + (L_max - L_min)*rand; 134 | y_coord = L_min + (L_max - L_min)*rand; 135 | 136 | if( agent_id > 1 ) 137 | free_position = false; 138 | while(free_position == false); 139 | x_coord = L_min + (L_max - L_min)*rand; 140 | y_coord = L_min + (L_max - L_min)*rand; 141 | 142 | for i = 1 : (agent_id-1) 143 | dist = sqrt((x_coord - agents(i).state.x)^2 + (y_coord - agents(i).state.y)^2); 144 | if dist > 7 * agent_diam 145 | free_position = true; 146 | else 147 | free_position = false; 148 | break; 149 | end 150 | end 151 | end 152 | end 153 | elseif( strcmp(mode,'grid') ) 154 | % Grid-like positioning 155 | envL_augm = env.L + 10*agent_diam; 156 | n_L = round(sqrt(n)); 157 | agent_l = envL_augm/(n_L +1); 158 | 159 | if( mod(agent_id, n_L) == 0 ) 160 | x_row=n_L; 161 | y_row=agent_id/n_L; 162 | else 163 | x_row=mod(agent_id, n_L); 164 | y_row=ceil(agent_id/n_L); 165 | end 166 | 167 | x_coord = x_row*agent_l - envL_augm/2; 168 | y_coord = y_row*agent_l - envL_augm/2; 169 | 170 | % vet_pos(k).lim_inf_x = (i-1)*x_g; 171 | % vet_pos(k).lim_sup_x = (i+1)*x_g; 172 | 173 | 174 | % vet_pos(k).x_centroid = i*x_g - W/2; 175 | % vet_pos(k).y_centroid = j*y_g - H/2;% 176 | end 177 | 178 | return 179 | -------------------------------------------------------------------------------- /_core/createTargets.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % 3 | % Multi Agent Simulator for Target Tracking (MASTT) 4 | % 5 | % createTargets.m 6 | % 7 | % Create the basic targets' structure 8 | % 9 | %-------------------------------------------------------------------------% 10 | % 11 | % (c) 2009-2013 12 | % 13 | % A. Petitti 14 | % D. Di Paola 15 | % S. Giannini 16 | % 17 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 18 | 19 | function targets = createTargets(env, dkns_params) 20 | % 21 | % INPUTS: 22 | % env = Enviroment structure 23 | % dkns_params = dkns structure 24 | % 25 | % OUTPUTS: 26 | % targets = Targets structure 27 | % 28 | 29 | 30 | %% MASF Populating targets structure 31 | % 32 | if dkns_params.Nt>0 33 | for i = 1:dkns_params.Nt 34 | 35 | targets(i) = target('id',i); % Create a new target with an identifier 36 | targets(i).status = 'ACTIVE'; 37 | 38 | targets(i).draw.color = [rand rand rand]; 39 | 40 | %-----------------------------------% 41 | % Physical Parameter 42 | % 43 | 44 | [state_x, state_y] = targetPositions(env); 45 | 46 | targets(i).state.x = state_x; % target x coordinate 47 | targets(i).state.y = state_y; % target y coordinate 48 | targets(i).state.theta = pi*rand(); % target orientation (radiant) 49 | 50 | targets(i).state.vel = 10; % target velocity magnitude 51 | targets(i).state.vel_x = 7; % target velocity x component 52 | targets(i).state.vel_y = 20; % target velocity y component 53 | 54 | 55 | targets(i).state.d = .5; % target occupied space 56 | end 57 | else 58 | targets = []; 59 | end 60 | 61 | return 62 | 63 | 64 | 65 | function [x_coord,y_coord] = targetPositions(env) 66 | 67 | %% Targets positioning function 68 | % 69 | x_coord = -env.W/4+(env.W)/2.*rand; 70 | y_coord = -env.H/4+(env.H)/2.*rand; 71 | 72 | 73 | return -------------------------------------------------------------------------------- /_core/target.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % 3 | % Multi Agent Simulator for Target Tracking (MASTT) 4 | % 5 | % Agent.m 6 | % 7 | % Target struct declaration 8 | % 9 | %-------------------------------------------------------------------------% 10 | % 11 | % (c) 2009-2013 12 | % 13 | % A. Petitti 14 | % D. Di Paola 15 | % S. Giannini 16 | % 17 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 18 | 19 | function t = target(varargin) 20 | % 21 | % INPUTS: 22 | % varargin = Inizialization pairs (field, value) 23 | % 24 | % OUTPUTS: 25 | % t = target structure 26 | % 27 | 28 | %% Target struct 29 | % 30 | t = struct('id' , -1,... % Parameters 31 | 'type' , 'n/a',... 32 | 'status' , 'IDLE',... 33 | 'state' , [],... % Physical Parameters 34 | 'draw' , []); % Draw variables 35 | 36 | 37 | %% Inputs Handler 38 | % 39 | if mod(nargin,2) == 0 40 | % even # of inputs, given as ('param','value') pairs 41 | % e.g. 'name', '14QB', 'id', 11 ... 42 | ii = 1; 43 | while ii < nargin 44 | try 45 | t = setfield(t,varargin{ii},varargin{ii+1}); 46 | catch 47 | error(['MASF/_CORE :: target.m - Invalid pair: (' varargin{ii} ', ' varargin{ii+1} ')']); 48 | end 49 | ii = ii + 2; 50 | end 51 | else 52 | error('MASF/_CORE :: target.m - Inputs must be paired, e.g. t = target(''name'', ''apparel'', ''id'', 42)'); 53 | end 54 | 55 | return 56 | -------------------------------------------------------------------------------- /_core/updateTargets.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % 3 | % Multi Agent Simulator for Target Tracking (MASTT) 4 | % 5 | % updateTargets.m 6 | % 7 | % Update the targets structure 8 | % 9 | %-------------------------------------------------------------------------% 10 | % 11 | % (c) 2009-2013 12 | % 13 | % A. Petitti 14 | % D. Di Paola 15 | % S. Giannini 16 | % 17 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 18 | 19 | function targets = updateTargets(params, dkns_parameters, targets) 20 | % 21 | % INPUTS: 22 | % params = Global Parameters structure 23 | % dkns_params = dkns parameters 24 | % targets = targets structure to update 25 | % 26 | % OUTPUTS: 27 | % targets = updated targets structure 28 | % 29 | 30 | 31 | %% MASF Updateting targets structure 32 | % 33 | 34 | for k = 1:dkns_parameters.Nt 35 | 36 | 37 | w = dkns_parameters.sigma_0*randn(2,1); 38 | 39 | x = [targets(k).state.x; targets(k).state.vel_x; targets(k).state.y; targets(k).state.vel_y;]; 40 | 41 | M = [mu(x(1), dkns_parameters.a) 0; 0 mu(x(3), dkns_parameters.a)]; 42 | F1 = [1 params.dt; 0 1]; 43 | F2 = [1 params.dt; -params.dt*dkns_parameters.c1 1-params.dt*dkns_parameters.c2]; 44 | 45 | Gi = [ (params.dt^2*dkns_parameters.sigma_0)/2; params.dt*dkns_parameters.sigma_0]; 46 | zero = [0; 0]; 47 | 48 | A = [M(1,1)*F1 M(1,2)*F1; M(2,1)*F1 M(2,2)*F1] + [(1-M(1,1))*F2 (-M(1,2))*F2; (-M(2,1))*F2 (1-M(2,2))*F2]; 49 | B = [Gi zero; zero Gi]; 50 | 51 | x = A*x + B*w; 52 | 53 | targets(k).state.x = x(1); 54 | targets(k).state.y = x(3); 55 | targets(k).state.vel_x = x(2); 56 | targets(k).state.vel_y = x(4); 57 | targets(k).state.vel = sqrt(x(2)^2 + x(4)^2); 58 | targets(k).state.theta = atan2(targets(k).state.vel_y, targets(k).state.vel_x); 59 | 60 | end 61 | 62 | return 63 | 64 | 65 | 66 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 67 | %% Olfati-Saber Trajectory Functions 68 | % [from "Distributed Tracking in Sensor Networks with Limited Sensing Range"] 69 | % 70 | 71 | function y = mu(z,a) 72 | 73 | y = (sigma(a+z) + sigma(a-z)) / 2; 74 | 75 | return 76 | 77 | function y = sigma(z) 78 | 79 | if z >= 0 80 | 81 | y = 1; 82 | 83 | else 84 | 85 | y = -1; 86 | 87 | end 88 | 89 | return -------------------------------------------------------------------------------- /_gui/initialization.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % 3 | % Multi Agent Simulator for Target Tracking (MASTT) 4 | % 5 | % ----------------------------------------------------------------------- 6 | % 7 | % (c) 2009-2013 8 | % 9 | % A. Petitti 10 | % D. Di Paola 11 | % S. Giannini 12 | % 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | 15 | function handles = initialization(handles) 16 | 17 | %-----------------------------------% 18 | % Setup Paths 19 | % 20 | addpath('./_tools'); 21 | addpath('./DKNS'); 22 | addpath('./TC'); 23 | 24 | % 25 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 26 | 27 | 28 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 29 | %% MASF-Parameters initialization 30 | % 31 | 32 | 33 | %-----------------------------------% 34 | % VISUALIZATIONS 35 | % 36 | 37 | addpath('./_viz'); 38 | 39 | 40 | % 41 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 42 | 43 | 44 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 45 | %% MASF-Core setup 46 | % 47 | 48 | addpath('./_core'); 49 | 50 | 51 | %-----------------------------------% 52 | % Init Structures 53 | % 54 | 55 | ENV = []; 56 | OBS = []; 57 | PARAMS = []; 58 | AGENTS = []; 59 | VIZ_PARAMS = []; 60 | 61 | 62 | %-----------------------------------% 63 | % ENVIROMENT PARAMETERS 64 | % 65 | ENV.W = 90; % Environment dimension 66 | ENV.H = 90; 67 | ENV.L = max([ENV.W ENV.H]); 68 | ENV.mode = 'closed'; % closed / toroid 69 | 70 | 71 | %-----------------------------------% 72 | % AGENTS PARAMETERS 73 | % 74 | 75 | PARAMS.Nu = handles.Nu; % Number of Sensing Units 76 | 77 | % Agents Positioning Mode 78 | % random = random free position in the environment 79 | % grid = grid-like pattern position in the environment 80 | % 81 | PARAMS.POS_MODE = handles.pos; 82 | 83 | % Communication System Mode 84 | % default = isotropic communication area defined by a range 85 | % os = Olfati-Saber paper range definition 86 | % 87 | PARAMS.COMM_MODE = 'default'; 88 | 89 | % Sensing System Mode 90 | % iso = isotropic sensing area (FOV = 360??) 91 | % sector = sector sensing area (FOV defined by range and angle) 92 | % 93 | PARAMS.SENS_MODE = handles.sens.type; 94 | 95 | % Sensing System Type 96 | % homo = homogeneous angles and ranges 97 | % hetero = heterogeneous angles and ranges 98 | % 99 | PARAMS.SENS_TYPE = handles.sens.mode; 100 | 101 | % Sensing System Model 102 | % rb = range bearing sensor model 103 | % standard = standard KF-model 104 | % 105 | PARAMS.SENS_MODEL = handles.sens.model; 106 | 107 | 108 | AGENTS = createAgents(PARAMS.Nu,ENV,PARAMS); 109 | 110 | 111 | %-----------------------------------% 112 | % GENERAL PARAMETERS 113 | % 114 | 115 | % Simulation Params 116 | PARAMS.Nt = 1; % Number of Targets 117 | 118 | PARAMS.n_iters = 300; % Iterations for each experiment 119 | PARAMS.dt = 0.04; % Time Step 120 | 121 | 122 | %-----------------------------------% 123 | % VISUALIZATION PARAMETERS 124 | % 125 | VIZ_PARAMS.plotSensors = 'on'; 126 | 127 | % 128 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 129 | 130 | 131 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 132 | %% DKNS-Specific setup 133 | % 134 | 135 | 136 | %-----------------------------------% 137 | % DKNS Params 138 | % 139 | DKNS_PARAMS.Nt = PARAMS.Nt; 140 | 141 | % Target Moving Model Parameters 142 | DKNS_PARAMS.c1 = 0.75; 143 | DKNS_PARAMS.c2 = 1; 144 | DKNS_PARAMS.a = 33; 145 | % Kalman Filter Parameters 146 | DKNS_PARAMS.kw = 5; 147 | DKNS_PARAMS.kv = 3; 148 | DKNS_PARAMS.sigma_0 = 5; 149 | % Sensing Parameters 150 | DKNS_PARAMS.RB.k_theta = 1/10; 151 | DKNS_PARAMS.RB.k_d = 1.056; 152 | DKNS_PARAMS.RB.k_rho = 10.07; 153 | DKNS_PARAMS.ST.sigma = 5; 154 | 155 | %-----------------------------------% 156 | % TC Params 157 | % 158 | TC_PARAMS.m = 5; 159 | 160 | % 161 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 162 | 163 | 164 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 165 | %% DKNS-MainLoop 166 | % 167 | 168 | %-----------------------------------% 169 | % Append Configurations to Agents strucutures 170 | % 171 | AGENTS = appendConfigTC(AGENTS); 172 | 173 | %-----------------------------------% 174 | % Visualization parameters initialization 175 | % 176 | winPosX = 200; 177 | winPosY = 100; 178 | 179 | winSizeX = 700; 180 | winSizeY = 700; 181 | 182 | xmin = -ENV.L/2; 183 | xmax = ENV.L/2; 184 | ymin = -ENV.L/2; 185 | ymax = ENV.L/2; 186 | 187 | %viz.f = figure(); clf; 188 | %set(viz.f,'doublebuffer','on','position',[winPosX winPosY winSizeX winSizeY],'color',[1 1 1]); 189 | 190 | %viz.axs = axes('position',[.1 .1 .8 .8]); 191 | set(handles.axes1,'box','on','nextplot','add','xlim',[xmin xmax],'ylim',[ymin ymax],... 192 | 'xtick',[],'ytick',[],'plotboxaspectratio',[(xmax-xmin) (ymax-ymin) 1]); 193 | 194 | 195 | % Setup network for this experiment 196 | AGENTS_DKNS = appendConfigDKNS(AGENTS, PARAMS, DKNS_PARAMS); 197 | 198 | % Setup targets for this experiment 199 | TARGETS = createTargets(ENV, DKNS_PARAMS); 200 | 201 | % Add structures to handles 202 | handles.agents = AGENTS_DKNS; 203 | handles.targets = TARGETS; 204 | handles.env = ENV; 205 | handles.params = PARAMS; 206 | handles.viz_params = VIZ_PARAMS; 207 | handles.dkns_params = DKNS_PARAMS; 208 | handles.tc_params = TC_PARAMS; -------------------------------------------------------------------------------- /_gui/simulate.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % 3 | % Multi Agent Simulator for Target Tracking (MASTT) 4 | % 5 | % ----------------------------------------------------------------------- 6 | % 7 | % (c) 2009-2013 8 | % 9 | % A. Petitti 10 | % D. Di Paola 11 | % S. Giannini 12 | % 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | 15 | function handles = simulate(handles) 16 | 17 | global sim 18 | 19 | while true 20 | 21 | if sim 22 | %% Distributed Estimation Algorithm 23 | handles.agents = CDMTT(handles.agents, handles.targets, handles.params, handles.dkns_params); 24 | 25 | %% Update target positions 26 | handles.targets = updateTargets(handles.params, handles.dkns_params, handles.targets); 27 | 28 | handles.agents = bearingControl(handles.agents); 29 | 30 | %% Clean Axes 31 | cla(handles.axes1); 32 | 33 | %% Visualize Targets 34 | Nt = length(handles.targets); 35 | acc = 100; % plot accuracy of circles 36 | % 37 | % Targets as circles 38 | % 39 | for k= 1 : Nt 40 | x = handles.targets(k).state.x; 41 | y = handles.targets(k).state.y; 42 | 43 | q = x + 1i * y; 44 | r = handles.targets(k).state.d; 45 | h = q + r * exp(1i*(0:pi/acc:2*pi)); 46 | patch(real(h), imag(h), handles.targets(k).draw.color); 47 | end 48 | 49 | %% Visualize Connections 50 | n = length(handles.agents); 51 | % Connections 52 | % 53 | for i = 1 : n 54 | for k = 1 : n 55 | if (i~=k) && (handles.agents(i).tc.G(k) == 1) 56 | plot(handles.axes1,[handles.agents(i).state.x,handles.agents(k).state.x],... 57 | [handles.agents(i).state.y,handles.agents(k).state.y],'color',[0 1 0]','linewidth',1) 58 | end 59 | end 60 | end 61 | 62 | %% Visualize Agents 63 | n = length(handles.agents); 64 | acc = 100; % plot accuracy of circles 65 | % 66 | % Sensing Areas 67 | % 68 | if strcmp(handles.viz_params.plotSensors,'on') 69 | sens_area_color= [.8 .8 .8]; 70 | 71 | if( strcmp(handles.params.SENS_MODE,'iso') ) 72 | for k = 1 : n 73 | q = handles.agents(k).state.x + 1i * handles.agents(k).state.y; 74 | r = handles.agents(k).sens.range; 75 | h = q + r * exp(1i*(0:pi/acc:2*pi)); 76 | 77 | plot(handles.axes1,real(h), imag(h), 'color',sens_area_color,'linewidth',1); 78 | end 79 | elseif( strcmp(handles.params.SENS_MODE,'sector') ) 80 | 81 | for k = 1 : n 82 | 83 | theta_s = handles.agents(k).state.theta - (handles.agents(k).sens.angle/2) + handles.agents(k).sens.orientation; 84 | 85 | % Define the sector polygon 86 | th = 0: 2*pi/acc : handles.agents(k).sens.angle; 87 | xunit = handles.agents(k).sens.range * cos(th) + handles.agents(k).state.x; 88 | yunit = handles.agents(k).sens.range * sin(th) + handles.agents(k).state.y; 89 | P = [handles.agents(k).state.x xunit; handles.agents(k).state.y yunit]; 90 | 91 | % Rotation Matrix 92 | R = [ cos(theta_s) -sin(theta_s); ... 93 | sin(theta_s) cos(theta_s)]; 94 | % Apply the rotation to the polygon 95 | P = R*P; 96 | 97 | % Traslation Matrix 98 | T = [ones(1, size(P,2)) * handles.agents(k).state.x - P(1,1); ... 99 | ones(1, size(P,2)) * handles.agents(k).state.y - P(2,1)]; 100 | % Apply the traslation to the polygon 101 | P = P + T; 102 | 103 | p = patch(P(1,:), P(2,:),'w'); 104 | set(p,'FaceColor', [.95 .95 .95]); 105 | set(p,'FaceAlpha', .3); 106 | set(p,'EdgeColor',sens_area_color); 107 | 108 | end 109 | else 110 | error('MASF/_VIZ :: vizAgents.m - No valid Sensing Type [SENS_MODE] value'); 111 | end 112 | end 113 | % 114 | % Agents as triangular vehicles 115 | % 116 | for k = 1 : n 117 | 118 | % Define the vehicle polygon 119 | a = [-0.3536 -0.3536]'; 120 | b = [.5 0]'; 121 | c = [-0.3536 0.3536]'; 122 | d = [-0.2 0]'; 123 | 124 | % Scale the polygon 125 | P = handles.agents(k).state.d*[a b c d]; 126 | % Rotation Matrix 127 | R = [ cos(handles.agents(k).state.theta) -sin(handles.agents(k).state.theta); ... 128 | sin(handles.agents(k).state.theta) cos(handles.agents(k).state.theta)]; 129 | % Traslation Matrix 130 | T = [ones(1,size(P,2))*handles.agents(k).state.x; ones(1,size(P,2))*handles.agents(k).state.y]; 131 | 132 | % Apply the roto-traslation to the polygon 133 | P = R*P + T; 134 | 135 | patch(P(1,:), P(2,:), handles.agents(k).draw.color); 136 | end 137 | 138 | %% Visualize Estimate 139 | plot(handles.axes1,handles.agents(1).dkns.KF(1).x(1),handles.agents(1).dkns.KF(1).x(3),'*', 'LineWidth', 1, 'MarkerSize', 10); 140 | 141 | %% Visualize Covariance 142 | mu = [handles.agents(1).dkns.KF(1).x(1); handles.agents(1).dkns.KF(1).x(3)]; 143 | Sigma = [handles.agents(1).dkns.KF(1).P(1,1) handles.agents(1).dkns.KF(1).P(1,3); handles.agents(1).dkns.KF(1).P(3,1) handles.agents(1).dkns.KF(1).P(3,3)]; 144 | plotgauss2d(mu, Sigma); 145 | 146 | pause(.1); 147 | else 148 | break; 149 | end 150 | end 151 | return -------------------------------------------------------------------------------- /_tools/dijkstra.m: -------------------------------------------------------------------------------- 1 | function [r_path, r_cost] = dijkstra(pathS, pathE, transmat) 2 | % This version support detecting _cyclic-paths_ 3 | % 4 | % USAGE: 5 | % [path, cost]= dijkstra(pathStart, pathEnd, transMatrix) 6 | % 7 | % PARAMETERS: 8 | % pathS : the index of start node, indexing from 1 9 | % pathE : the index of end node, indexing from 1 10 | % transmat: the transition matrix, or adjacent matrix 11 | % 12 | 13 | % Ensure the transition matrix is square 14 | % 15 | if ( size(transmat,1) ~= size(transmat,2) ) 16 | error( 'detect_cycles:Dijkstra_SC', ... 17 | 'transmat has different width and heights' ); 18 | end 19 | 20 | 21 | % Initialization: 22 | % noOfNode : nodes in the graph 23 | % parent(i) : record the parent of node i 24 | % distance(i) : the shortest distance from i to pathS 25 | % queue : for width-first traveling of the graph 26 | noOfNode = size(transmat, 1); 27 | 28 | for i = 1:noOfNode 29 | parent(i) = 0; 30 | distance(i) = Inf; 31 | end 32 | 33 | queue = []; 34 | 35 | 36 | % Start from pathS 37 | % 38 | for i=1:noOfNode 39 | if transmat(pathS, i)~=Inf 40 | distance(i) = transmat(pathS, i); 41 | parent(i) = pathS; 42 | queue = [queue i]; 43 | end 44 | end 45 | 46 | 47 | 48 | % Width-first exploring the whole graph 49 | % 50 | while length(queue) ~= 0 51 | 52 | hopS = queue(1); 53 | queue = queue(2:end); 54 | 55 | for hopE = 1:noOfNode 56 | if distance(hopE) > distance(hopS) + transmat(hopS,hopE) 57 | distance(hopE) = distance(hopS) + transmat(hopS,hopE); 58 | parent(hopE) = hopS; 59 | queue = [queue hopE]; 60 | end 61 | end 62 | 63 | end 64 | 65 | 66 | % Back-trace the shortest-path 67 | % 68 | r_path = [pathE]; 69 | i = parent(pathE); 70 | 71 | while i~=pathS && i~=0 72 | r_path = [i r_path]; 73 | i = parent(i); 74 | end 75 | 76 | if i==pathS 77 | r_path = [i r_path]; 78 | else 79 | r_path = []; 80 | end 81 | 82 | 83 | % Return cost 84 | % 85 | r_cost = distance(pathE); 86 | 87 | return -------------------------------------------------------------------------------- /_tools/getNetDiameter.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % 3 | % Multi Agent Simulator for Target Tracking (MASTT) 4 | % 5 | % ----------------------------------------------------------------------- 6 | % 7 | % 8 | % getNetDiameter.m 9 | % 10 | % % Computing the graph diameter's length. 11 | % 12 | %-------------------------------------------------------------------------% 13 | % 14 | % (c) 2009-2013 15 | % 16 | % A. Petitti 17 | % D. Di Paola 18 | % S. Giannini 19 | % 20 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 21 | 22 | function longestShortestPath = getNetDiameter(G) 23 | % 24 | % INPUTS: 25 | % G = communication graph 26 | % 27 | % OUTPUTS: 28 | % longestShortestPath = longest shortest path length 29 | % 30 | 31 | N = size(G,2); 32 | 33 | % Convert PN Graph 34 | for i = 1:N 35 | for j = 1:N 36 | if G(i,j)==0; 37 | G(i,j)=Inf; 38 | end 39 | if ( i == j) 40 | G(i,j) = 1; 41 | end 42 | end 43 | end 44 | 45 | longestShortestPath = 0; 46 | for i = 1:N 47 | for j = 1:N 48 | if(i>=j) 49 | continue; 50 | end 51 | [sp, spcost] = dijkstra(i, j, G); 52 | temp = size(sp,2) - 1; 53 | if(temp > longestShortestPath) 54 | longestShortestPath = temp; 55 | end 56 | end 57 | end 58 | 59 | -------------------------------------------------------------------------------- /_viz/bearingControl.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % 3 | % Multi Agent Simulator for Target Tracking (MASTT) 4 | % 5 | % ----------------------------------------------------------------------- 6 | % Visualization (_viz) Toolbox 7 | % 8 | % bearingControl.m 9 | % 10 | % Setting the orientation of the agents according to the position 11 | % estimation of the target 12 | % 13 | %-------------------------------------------------------------------------% 14 | % 15 | % (c) 2009-2013 16 | % 17 | % A. Petitti 18 | % D. Di Paola 19 | % S. Giannini 20 | % 21 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 22 | 23 | function agents = bearingControl(agents) 24 | % 25 | % INPUTS: 26 | % agents = Agents structure 27 | % 28 | % OUTPUTS: 29 | % agents = Agents structure 30 | % 31 | 32 | for i = 1:length(agents) 33 | agents(i).state.theta = atan2(agents(i).dkns.KF(1).x(3) - agents(i).state.y, agents(i).dkns.KF(1).x(1) - agents(i).state.x); 34 | end -------------------------------------------------------------------------------- /_viz/plotgauss2d.m: -------------------------------------------------------------------------------- 1 | function h=plotgauss2d(mu, Sigma) 2 | % PLOTGAUSS2D Plot a 2D Gaussian as an ellipse with optional cross hairs 3 | % h=plotgauss2(mu, Sigma) 4 | % 5 | 6 | h = plotcov2(mu, Sigma); 7 | return; 8 | 9 | %%%%%%%%%%%%%%%%%%%%%%%% 10 | 11 | % PLOTCOV2 - Plots a covariance ellipse with major and minor axes 12 | % for a bivariate Gaussian distribution. 13 | % 14 | % Usage: 15 | % h = plotcov2(mu, Sigma[, OPTIONS]); 16 | % 17 | % Inputs: 18 | % mu - a 2 x 1 vector giving the mean of the distribution. 19 | % Sigma - a 2 x 2 symmetric positive semi-definite matrix giving 20 | % the covariance of the distribution (or the zero matrix). 21 | % 22 | % Options: 23 | % 'conf' - a scalar between 0 and 1 giving the confidence 24 | % interval (i.e., the fraction of probability mass to 25 | % be enclosed by the ellipse); default is 0.9. 26 | % 'num-pts' - the number of points to be used to plot the 27 | % ellipse; default is 100. 28 | % 29 | % This function also accepts options for PLOT. 30 | % 31 | % Outputs: 32 | % h - a vector of figure handles to the ellipse boundary and 33 | % its major and minor axes 34 | % 35 | % See also: PLOTCOV3 36 | 37 | % Copyright (C) 2002 Mark A. Paskin 38 | 39 | function h = plotcov2(mu, Sigma, varargin) 40 | 41 | if size(Sigma) ~= [2 2], error('Sigma must be a 2 by 2 matrix'); end 42 | if length(mu) ~= 2, error('mu must be a 2 by 1 vector'); end 43 | 44 | [p, ... 45 | n, ... 46 | plot_opts] = process_options(varargin, 'conf', 0.9, ... 47 | 'num-pts', 100); 48 | h = []; 49 | holding = ishold; 50 | if (Sigma == zeros(2, 2)) 51 | z = mu; 52 | else 53 | % Compute the Mahalanobis radius of the ellipsoid that encloses 54 | % the desired probability mass. 55 | k = conf2mahal(p, 2); 56 | % The major and minor axes of the covariance ellipse are given by 57 | % the eigenvectors of the covariance matrix. Their lengths (for 58 | % the ellipse with unit Mahalanobis radius) are given by the 59 | % square roots of the corresponding eigenvalues. 60 | if (issparse(Sigma)) 61 | [V, D] = eigs(Sigma); 62 | else 63 | [V, D] = eig(Sigma); 64 | end 65 | % Compute the points on the surface of the ellipse. 66 | t = linspace(0, 2*pi, n); 67 | u = [cos(t); sin(t)]; 68 | w = (k * V * sqrt(D)) * u; 69 | z = repmat(mu, [1 n]) + w; 70 | % Plot the major and minor axes. 71 | L = k * sqrt(diag(D)); 72 | h = plot([mu(1); mu(1) + L(1) * V(1, 1)], ... 73 | [mu(2); mu(2) + L(1) * V(2, 1)], plot_opts{:}); 74 | hold on; 75 | h = [h; plot([mu(1); mu(1) + L(2) * V(1, 2)], ... 76 | [mu(2); mu(2) + L(2) * V(2, 2)], plot_opts{:})]; 77 | end 78 | 79 | h = [h; plot(z(1, :), z(2, :), plot_opts{:})]; 80 | if (~holding) hold off; end 81 | 82 | %%%%%%%%%%%% 83 | 84 | % CONF2MAHAL - Translates a confidence interval to a Mahalanobis 85 | % distance. Consider a multivariate Gaussian 86 | % distribution of the form 87 | % 88 | % p(x) = 1/sqrt((2 * pi)^d * det(C)) * exp((-1/2) * MD(x, m, inv(C))) 89 | % 90 | % where MD(x, m, P) is the Mahalanobis distance from x 91 | % to m under P: 92 | % 93 | % MD(x, m, P) = (x - m) * P * (x - m)' 94 | % 95 | % A particular Mahalanobis distance k identifies an 96 | % ellipsoid centered at the mean of the distribution. 97 | % The confidence interval associated with this ellipsoid 98 | % is the probability mass enclosed by it. Similarly, 99 | % a particular confidence interval uniquely determines 100 | % an ellipsoid with a fixed Mahalanobis distance. 101 | % 102 | % If X is an d dimensional Gaussian-distributed vector, 103 | % then the Mahalanobis distance of X is distributed 104 | % according to the Chi-squared distribution with d 105 | % degrees of freedom. Thus, the Mahalanobis distance is 106 | % determined by evaluating the inverse cumulative 107 | % distribution function of the chi squared distribution 108 | % up to the confidence value. 109 | % 110 | % Usage: 111 | % 112 | % m = conf2mahal(c, d); 113 | % 114 | % Inputs: 115 | % 116 | % c - the confidence interval 117 | % d - the number of dimensions of the Gaussian distribution 118 | % 119 | % Outputs: 120 | % 121 | % m - the Mahalanobis radius of the ellipsoid enclosing the 122 | % fraction c of the distribution's probability mass 123 | % 124 | % See also: MAHAL2CONF 125 | 126 | % Copyright (C) 2002 Mark A. Paskin 127 | 128 | function m = conf2mahal(c, d) 129 | 130 | m = chi2inv(c, d); % matlab stats toolbox 131 | -------------------------------------------------------------------------------- /_viz/process_options.m: -------------------------------------------------------------------------------- 1 | % PROCESS_OPTIONS - Processes options passed to a Matlab function. 2 | % This function provides a simple means of 3 | % parsing attribute-value options. Each option is 4 | % named by a unique string and is given a default 5 | % value. 6 | % 7 | % Usage: [var1, var2, ..., varn[, unused]] = ... 8 | % process_options(args, ... 9 | % str1, def1, str2, def2, ..., strn, defn) 10 | % 11 | % Arguments: 12 | % args - a cell array of input arguments, such 13 | % as that provided by VARARGIN. Its contents 14 | % should alternate between strings and 15 | % values. 16 | % str1, ..., strn - Strings that are associated with a 17 | % particular variable 18 | % def1, ..., defn - Default values returned if no option 19 | % is supplied 20 | % 21 | % Returns: 22 | % var1, ..., varn - values to be assigned to variables 23 | % unused - an optional cell array of those 24 | % string-value pairs that were unused; 25 | % if this is not supplied, then a 26 | % warning will be issued for each 27 | % option in args that lacked a match. 28 | % 29 | % Examples: 30 | % 31 | % Suppose we wish to define a Matlab function 'func' that has 32 | % required parameters x and y, and optional arguments 'u' and 'v'. 33 | % With the definition 34 | % 35 | % function y = func(x, y, varargin) 36 | % 37 | % [u, v] = process_options(varargin, 'u', 0, 'v', 1); 38 | % 39 | % calling func(0, 1, 'v', 2) will assign 0 to x, 1 to y, 0 to u, and 2 40 | % to v. The parameter names are insensitive to case; calling 41 | % func(0, 1, 'V', 2) has the same effect. The function call 42 | % 43 | % func(0, 1, 'u', 5, 'z', 2); 44 | % 45 | % will result in u having the value 5 and v having value 1, but 46 | % will issue a warning that the 'z' option has not been used. On 47 | % the other hand, if func is defined as 48 | % 49 | % function y = func(x, y, varargin) 50 | % 51 | % [u, v, unused_args] = process_options(varargin, 'u', 0, 'v', 1); 52 | % 53 | % then the call func(0, 1, 'u', 5, 'z', 2) will yield no warning, 54 | % and unused_args will have the value {'z', 2}. This behaviour is 55 | % useful for functions with options that invoke other functions 56 | % with options; all options can be passed to the outer function and 57 | % its unprocessed arguments can be passed to the inner function. 58 | 59 | % Copyright (C) 2002 Mark A. Paskin 60 | % 61 | % This program is free software; you can redistribute it and/or modify 62 | % it under the terms of the GNU General Public License as published by 63 | % the Free Software Foundation; either version 2 of the License, or 64 | % (at your option) any later version. 65 | % 66 | % This program is distributed in the hope that it will be useful, but 67 | % WITHOUT ANY WARRANTY; without even the implied warranty of 68 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 69 | % General Public License for more details. 70 | % 71 | % You should have received a copy of the GNU General Public License 72 | % along with this program; if not, write to the Free Software 73 | % Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 74 | % USA. 75 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 76 | 77 | function [varargout] = process_options(args, varargin) 78 | 79 | % Check the number of input arguments 80 | n = length(varargin); 81 | if (mod(n, 2)) 82 | error('Each option must be a string/value pair.'); 83 | end 84 | 85 | % Check the number of supplied output arguments 86 | if (nargout < (n / 2)) 87 | error('Insufficient number of output arguments given'); 88 | elseif (nargout == (n / 2)) 89 | warn = 1; 90 | nout = n / 2; 91 | else 92 | warn = 0; 93 | nout = n / 2 + 1; 94 | end 95 | 96 | % Set outputs to be defaults 97 | varargout = cell(1, nout); 98 | for i=2:2:n 99 | varargout{i/2} = varargin{i}; 100 | end 101 | 102 | % Now process all arguments 103 | nunused = 0; 104 | for i=1:2:length(args) 105 | found = 0; 106 | for j=1:2:n 107 | if strcmpi(args{i}, varargin{j}) 108 | varargout{(j + 1)/2} = args{i + 1}; 109 | found = 1; 110 | break; 111 | end 112 | end 113 | if (~found) 114 | if (warn) 115 | warning(sprintf('Option ''%s'' not used.', args{i})); 116 | args{i} 117 | else 118 | nunused = nunused + 1; 119 | unused{2 * nunused - 1} = args{i}; 120 | unused{2 * nunused} = args{i + 1}; 121 | end 122 | end 123 | end 124 | 125 | % Assign the unused arguments 126 | if (~warn) 127 | if (nunused) 128 | varargout{nout} = unused; 129 | else 130 | varargout{nout} = cell(0); 131 | end 132 | end 133 | -------------------------------------------------------------------------------- /_viz/vizAgents.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % 3 | % Multi Agent Simulator for Target Tracking (MASTT) 4 | % 5 | % ----------------------------------------------------------------------- 6 | % Visualization (_viz) Toolbox 7 | % 8 | % 9 | % vizAgents.m 10 | % 11 | % Visualization of agents in the environment. 12 | % 13 | %-------------------------------------------------------------------------% 14 | % 15 | % (c) 2009-2013 16 | % 17 | % A. Petitti 18 | % D. Di Paola 19 | % S. Giannini 20 | % 21 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 22 | 23 | function viz = vizAgents(viz, agents, param, sensorFlag) 24 | % 25 | % INPUTS: 26 | % viz = visualition structure 27 | % agents = Agents structure 28 | % param = Global Parameters of the agent network structure 29 | % sensorFlag = sensor flag: on/off 30 | % 31 | % OUTPUTS: 32 | % viz = visualition structure 33 | % 34 | 35 | %% Parameters initialization 36 | % 37 | n = length(agents); 38 | acc = 100; % plot accuracy of circles 39 | 40 | 41 | %% Sensing Areas 42 | % 43 | if strcmp(sensorFlag,'on') 44 | sens_area_color= [.8 .8 .8]; 45 | 46 | if( strcmp(param.SENS_MODE,'iso') ) 47 | for k = 1 : n 48 | q = agents(k).state.x + 1i * agents(k).state.y; 49 | r = agents(k).sens.range; 50 | h = q + r * exp(1i*(0:pi/acc:2*pi)); 51 | 52 | plot(viz.axs,real(h), imag(h), 'color',sens_area_color,'linewidth',1); 53 | end 54 | elseif( strcmp(param.SENS_MODE,'sector') ) 55 | 56 | for k = 1 : n 57 | 58 | theta_s = agents(k).state.theta - (agents(k).sens.angle/2) + agents(k).sens.orientation; 59 | 60 | % Define the sector polygon 61 | th = 0: 2*pi/acc : agents(k).sens.angle; 62 | xunit = agents(k).sens.range * cos(th) + agents(k).state.x; 63 | yunit = agents(k).sens.range * sin(th) + agents(k).state.y; 64 | P = [agents(k).state.x xunit; agents(k).state.y yunit]; 65 | 66 | % Rotation Matrix 67 | R = [ cos(theta_s) -sin(theta_s); ... 68 | sin(theta_s) cos(theta_s)]; 69 | % Apply the rotation to the polygon 70 | P = R*P; 71 | 72 | % Traslation Matrix 73 | T = [ones(1, size(P,2)) * agents(k).state.x - P(1,1); ... 74 | ones(1, size(P,2)) * agents(k).state.y - P(2,1)]; 75 | % Apply the traslation to the polygon 76 | P = P + T; 77 | 78 | p = patch(P(1,:), P(2,:),'w'); 79 | set(p,'FaceColor', [.95 .95 .95]); 80 | set(p,'FaceAlpha', .3); 81 | set(p,'EdgeColor',sens_area_color); 82 | 83 | end 84 | else 85 | error('MASF/_VIZ :: vizAgents.m - No valid Sensing Type [SENS_MODE] value'); 86 | end 87 | end 88 | 89 | %% Agents as triangular vehicles 90 | % 91 | for k = 1 : n 92 | 93 | % Define the vehicle polygon 94 | a = [-0.3536 -0.3536]'; 95 | b = [.5 0]'; 96 | c = [-0.3536 0.3536]'; 97 | d = [-0.2 0]'; 98 | 99 | % Scale the polygon 100 | P = agents(k).state.d*[a b c d]; 101 | % Rotation Matrix 102 | R = [ cos(agents(k).state.theta) -sin(agents(k).state.theta); ... 103 | sin(agents(k).state.theta) cos(agents(k).state.theta)]; 104 | % Traslation Matrix 105 | T = [ones(1,size(P,2))*agents(k).state.x; ones(1,size(P,2))*agents(k).state.y]; 106 | 107 | % Apply the roto-traslation to the polygon 108 | P = R*P + T; 109 | 110 | patch(P(1,:), P(2,:), agents(k).draw.color); 111 | end 112 | 113 | return 114 | 115 | -------------------------------------------------------------------------------- /_viz/vizClean.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % 3 | % Multi Agent Simulator for Target Tracking (MASTT) 4 | % 5 | % ----------------------------------------------------------------------- 6 | % Visualization (_viz) Toolbox 7 | % 8 | % 9 | % vizClean.m 10 | % 11 | % Cleaning up the figure. 12 | % 13 | %-------------------------------------------------------------------------% 14 | % 15 | % (c) 2009-2013 16 | % 17 | % A. Petitti 18 | % D. Di Paola 19 | % S. Giannini 20 | % 21 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 22 | 23 | function viz = vizClean(viz) 24 | % 25 | % INPUTS: 26 | % viz = visualition structure 27 | % 28 | % OUTPUTS: 29 | % viz = visualition structure 30 | % 31 | 32 | %% Clean up the figure 33 | % 34 | cla(viz.axs); 35 | 36 | return 37 | 38 | -------------------------------------------------------------------------------- /_viz/vizConn.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % 3 | % Multi Agent Simulator for Target Tracking (MASTT) 4 | % 5 | % ----------------------------------------------------------------------- 6 | % Visualization (_viz) Toolbox 7 | % 8 | % 9 | % vizConn.m 10 | % 11 | % Visualization agents connections given the connection graph 12 | % 13 | %-------------------------------------------------------------------------% 14 | % 15 | % (c) 2009-2013 16 | % 17 | % A. Petitti 18 | % D. Di Paola 19 | % S. Giannini 20 | % 21 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 22 | 23 | function viz = vizConn(viz, agents) 24 | % 25 | % INPUTS: 26 | % viz = visualition structure 27 | % agents = Agents structure 28 | % 29 | % OUTPUTS: 30 | % viz = visualition structure 31 | % 32 | 33 | %% Parameters initialization 34 | % 35 | n = length(agents); 36 | 37 | %% Connections 38 | % 39 | for i = 1 : n 40 | for k = 1 : n 41 | if (i~=k) && (agents(i).tc.G(k) == 1) 42 | plot(viz.axs,[agents(i).state.x,agents(k).state.x],... 43 | [agents(i).state.y,agents(k).state.y],'color',[0 1 0]','linewidth',1) 44 | end 45 | end 46 | end 47 | 48 | return 49 | 50 | -------------------------------------------------------------------------------- /_viz/vizCovEll.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % 3 | % Multi Agent Simulator for Target Tracking (MASTT) 4 | % 5 | % ----------------------------------------------------------------------- 6 | % Visualization (_viz) Toolbox 7 | % 8 | % 9 | % vizCovEll.m 10 | % 11 | % Visualization of the covariance ellipse. 12 | % 13 | %-------------------------------------------------------------------------% 14 | % 15 | % (c) 2009-2013 16 | % 17 | % A. Petitti 18 | % D. Di Paola 19 | % S. Giannini 20 | % 21 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 22 | 23 | function viz = vizCovEll(viz, x, P) 24 | % 25 | % INPUTS: 26 | % viz = visualition structure 27 | % x = state estimation 28 | % P = covariance matrix 29 | % 30 | % OUTPUTS: 31 | % viz = visualition structure 32 | % 33 | 34 | %% Parameters initialization 35 | % 36 | %n = length(agents); 37 | acc = 100; % plot accuracy of circles 38 | 39 | mu = [x(1); x(3)]; 40 | Sigma = [P(1,1) P(1,3); P(3,1) P(3,3)]; 41 | 42 | %% Estimates as circles 43 | % 44 | %for k = 1 : n 45 | %hnew = plot(agents(1).cdtt.KF(1).x(1),agents(1).cdtt.KF(1).x(3),'*', 'LineWidth', 1, 'MarkerSize', 10); 46 | %end 47 | h = plotgauss2d(mu, Sigma); 48 | 49 | %legend(hnew, 'DENS', 'Location', 'NorthOutside'); 50 | return 51 | 52 | -------------------------------------------------------------------------------- /_viz/vizEnv.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % 3 | % Multi Agent Simulator for Target Tracking (MASTT) 4 | % 5 | % ----------------------------------------------------------------------- 6 | % Visualization (_viz) Toolbox 7 | % 8 | % 9 | % vizEnv.m 10 | % 11 | % Visualization of the environment (main viz object). 12 | % 13 | %-------------------------------------------------------------------------% 14 | % 15 | % (c) 2009-2013 16 | % 17 | % A. Petitti 18 | % D. Di Paola 19 | % S. Giannini 20 | % 21 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 22 | 23 | function viz = vizEnv(env) 24 | % 25 | % INPUTS: 26 | % env = Enviroment structure 27 | % 28 | % OUTPUTS: 29 | % viz = visualition structure 30 | % 31 | 32 | %% Parameters initialization 33 | % 34 | 35 | winPosX = 200; 36 | winPosY = 100; 37 | 38 | winSizeX = 700; 39 | winSizeY = 700; 40 | 41 | xmin = -env.L/2; 42 | xmax = env.L/2; 43 | ymin = -env.L/2; 44 | ymax = env.L/2; 45 | 46 | viz.f = figure(); clf; 47 | set(viz.f,'doublebuffer','on','position',[winPosX winPosY winSizeX winSizeY],'color',[1 1 1]); 48 | 49 | viz.axs = axes('position',[.1 .1 .8 .8]); 50 | set(viz.axs,'box','on','nextplot','add','xlim',[xmin xmax],'ylim',[ymin ymax],... 51 | 'xtick',[],'ytick',[],'plotboxaspectratio',[(xmax-xmin) (ymax-ymin) 1]); 52 | 53 | return 54 | 55 | -------------------------------------------------------------------------------- /_viz/vizEstimate.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % 3 | % Multi Agent Simulator for Target Tracking (MASTT) 4 | % 5 | % ----------------------------------------------------------------------- 6 | % Visualization (_viz) Toolbox 7 | % 8 | % 9 | % vizEstimate.m 10 | % 11 | % Visualization of the estimation of the target's position. 12 | % 13 | %-------------------------------------------------------------------------% 14 | % 15 | % (c) 2009-2013 16 | % 17 | % A. Petitti 18 | % D. Di Paola 19 | % S. Giannini 20 | % 21 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 22 | 23 | function viz = vizEstimate(viz, agents, env) 24 | % 25 | % INPUTS: 26 | % viz = visualition structure 27 | % agents = Agents structure 28 | % 29 | % OUTPUTS: 30 | % viz = visualition structure 31 | % 32 | 33 | 34 | %% Estimates as asterisks 35 | % 36 | 37 | if (abs(agents(1).dkns.KF(1).x(1)) < env.W/2 && abs(agents(1).dkns.KF(1).x(3)) < env.H/2) 38 | hnew = plot(viz.axs,agents(1).dkns.KF(1).x(1),agents(1).dkns.KF(1).x(3),'*', 'LineWidth', 1, 'MarkerSize', 10); 39 | else 40 | disp(''); 41 | end 42 | 43 | % legend(hnew, 'DKNS', 'Location', 'NorthOutside'); 44 | return 45 | 46 | -------------------------------------------------------------------------------- /_viz/vizTargets.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % 3 | % Multi Agent Simulator for Target Tracking (MASTT) 4 | % 5 | % ----------------------------------------------------------------------- 6 | % Visualization (_viz) Toolbox 7 | % 8 | % 9 | % vizTargets.m 10 | % 11 | % Visualization of targets in the environment. 12 | % 13 | %-------------------------------------------------------------------------% 14 | % 15 | % (c) 2009-2013 16 | % 17 | % A. Petitti 18 | % D. Di Paola 19 | % S. Giannini 20 | % 21 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 22 | 23 | function viz = vizTargets(viz, targets) 24 | % 25 | % INPUTS: 26 | % viz = visualition structure 27 | % targets = Targets structure 28 | % 29 | % OUTPUTS: 30 | % viz = visualition structure 31 | % 32 | 33 | %% Parameters initialization 34 | % 35 | Nt = length(targets); 36 | acc = 100; % plot accuracy of circles 37 | 38 | 39 | 40 | %% Targets as circles 41 | % 42 | 43 | for k= 1 : Nt 44 | 45 | x = targets(k).state.x; 46 | y = targets(k).state.y; 47 | 48 | q = x + 1i * y; 49 | r = targets(k).state.d; 50 | h = q + r * exp(1i*(0:pi/acc:2*pi)); 51 | patch(real(h), imag(h), targets(k).draw.color); 52 | end 53 | 54 | return 55 | 56 | -------------------------------------------------------------------------------- /gui.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gitAnto/MASTT/7e9f201aa02beacb88f18952180dffb8d8db19d1/gui.fig -------------------------------------------------------------------------------- /gui.m: -------------------------------------------------------------------------------- 1 | function varargout = gui(varargin) 2 | % GUI MATLAB code for gui.fig 3 | % GUI, by itself, creates a new GUI or raises the existing 4 | % singleton*. 5 | % 6 | % H = GUI returns the handle to a new GUI or the handle to 7 | % the existing singleton*. 8 | % 9 | % GUI('CALLBACK',hObject,eventData,handles,...) calls the local 10 | % function named CALLBACK in GUI.M with the given input arguments. 11 | % 12 | % GUI('Property','Value',...) creates a new GUI or raises the 13 | % existing singleton*. Starting from the left, property value pairs are 14 | % applied to the GUI before gui_OpeningFcn gets called. An 15 | % unrecognized property name or invalid value makes property application 16 | % stop. All inputs are passed to gui_OpeningFcn via varargin. 17 | % 18 | % *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only one 19 | % instance to run (singleton)". 20 | % 21 | % See also: GUIDE, GUIDATA, GUIHANDLES 22 | 23 | % Edit the above text to modify the response to help gui 24 | 25 | % Last Modified by GUIDE v2.5 04-Jan-2014 19:51:45 26 | 27 | addpath('./_gui'); 28 | 29 | % Begin initialization code - DO NOT EDIT 30 | gui_Singleton = 1; 31 | gui_State = struct('gui_Name', mfilename, ... 32 | 'gui_Singleton', gui_Singleton, ... 33 | 'gui_OpeningFcn', @gui_OpeningFcn, ... 34 | 'gui_OutputFcn', @gui_OutputFcn, ... 35 | 'gui_LayoutFcn', [] , ... 36 | 'gui_Callback', []); 37 | if nargin && ischar(varargin{1}) 38 | gui_State.gui_Callback = str2func(varargin{1}); 39 | end 40 | 41 | if nargout 42 | [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:}); 43 | else 44 | gui_mainfcn(gui_State, varargin{:}); 45 | end 46 | % End initialization code - DO NOT EDIT 47 | 48 | 49 | % --- Executes just before gui is made visible. 50 | function gui_OpeningFcn(hObject, eventdata, handles, varargin) 51 | % This function has no output args, see OutputFcn. 52 | % hObject handle to figure 53 | % eventdata reserved - to be defined in a future version of MATLAB 54 | % handles structure with handles and user data (see GUIDATA) 55 | % varargin command line arguments to gui (see VARARGIN) 56 | 57 | % Default Parameters 58 | handles.Nu = 4; 59 | handles.pos = 'random'; 60 | handles.sens.mode = 'hetero'; 61 | handles.sens.model = 'rb'; 62 | handles.sens.type = 'sector'; 63 | 64 | % Initialize Parameters 65 | handles = initialization(handles); 66 | 67 | % Choose default command line output for gui 68 | handles.output = hObject; 69 | 70 | % Update handles structure 71 | guidata(hObject, handles); 72 | 73 | % UIWAIT makes gui wait for user response (see UIRESUME) 74 | % uiwait(handles.figure1); 75 | 76 | 77 | % --- Outputs from this function are returned to the command line. 78 | function varargout = gui_OutputFcn(hObject, eventdata, handles) 79 | % varargout cell array for returning output args (see VARARGOUT); 80 | % hObject handle to figure 81 | % eventdata reserved - to be defined in a future version of MATLAB 82 | % handles structure with handles and user data (see GUIDATA) 83 | 84 | % Get default command line output from handles structure 85 | varargout{1} = handles.output; 86 | 87 | 88 | % --- Executes on selection change in popupmenu1. 89 | function popupmenu1_Callback(hObject, eventdata, handles) 90 | % hObject handle to popupmenu1 (see GCBO) 91 | % eventdata reserved - to be defined in a future version of MATLAB 92 | % handles structure with handles and user data (see GUIDATA) 93 | 94 | % Hints: contents = cellstr(get(hObject,'String')) returns popupmenu1 contents as cell array 95 | % contents{get(hObject,'Value')} returns selected item from popupmenu1 96 | 97 | val = get(hObject, 'Value'); 98 | str = get(hObject, 'String'); 99 | 100 | switch str{val} 101 | 102 | case 'homogeneous' 103 | handles.sens.mode = 'homo'; 104 | case 'heterogeneous' 105 | handles.sens.mode = 'hetero'; 106 | 107 | end 108 | 109 | % Update handles structure 110 | guidata(hObject, handles); 111 | 112 | 113 | % --- Executes during object creation, after setting all properties. 114 | function popupmenu1_CreateFcn(hObject, eventdata, handles) 115 | % hObject handle to popupmenu1 (see GCBO) 116 | % eventdata reserved - to be defined in a future version of MATLAB 117 | % handles empty - handles not created until after all CreateFcns called 118 | 119 | % Hint: popupmenu controls usually have a white background on Windows. 120 | % See ISPC and COMPUTER. 121 | if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) 122 | set(hObject,'BackgroundColor','white'); 123 | end 124 | 125 | 126 | % --- Executes on selection change in popupmenu2. 127 | function popupmenu2_Callback(hObject, eventdata, handles) 128 | % hObject handle to popupmenu2 (see GCBO) 129 | % eventdata reserved - to be defined in a future version of MATLAB 130 | % handles structure with handles and user data (see GUIDATA) 131 | 132 | % Hints: contents = cellstr(get(hObject,'String')) returns popupmenu2 contents as cell array 133 | % contents{get(hObject,'Value')} returns selected item from popupmenu2 134 | 135 | val = get(hObject, 'Value'); 136 | str = get(hObject, 'String'); 137 | 138 | switch str{val} 139 | 140 | case 'range-bearing' 141 | handles.sens.model = 'rb'; 142 | case 'standard' 143 | handles.sens.model = 'standard'; 144 | 145 | end 146 | 147 | % Update handles structure 148 | guidata(hObject, handles); 149 | 150 | 151 | % --- Executes during object creation, after setting all properties. 152 | function popupmenu2_CreateFcn(hObject, eventdata, handles) 153 | % hObject handle to popupmenu2 (see GCBO) 154 | % eventdata reserved - to be defined in a future version of MATLAB 155 | % handles empty - handles not created until after all CreateFcns called 156 | 157 | % Hint: popupmenu controls usually have a white background on Windows. 158 | % See ISPC and COMPUTER. 159 | if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) 160 | set(hObject,'BackgroundColor','white'); 161 | end 162 | 163 | 164 | % --- Executes on selection change in popupmenu3. 165 | function popupmenu3_Callback(hObject, eventdata, handles) 166 | % hObject handle to popupmenu3 (see GCBO) 167 | % eventdata reserved - to be defined in a future version of MATLAB 168 | % handles structure with handles and user data (see GUIDATA) 169 | 170 | % Hints: contents = cellstr(get(hObject,'String')) returns popupmenu3 contents as cell array 171 | % contents{get(hObject,'Value')} returns selected item from popupmenu3 172 | 173 | val = get(hObject, 'Value'); 174 | str = get(hObject, 'String'); 175 | 176 | switch str{val} 177 | 178 | case 'sector' 179 | handles.sens.type = 'sector'; 180 | case 'isotropic' 181 | handles.sens.type = 'iso'; 182 | 183 | end 184 | 185 | % Update handles structure 186 | guidata(hObject, handles); 187 | 188 | 189 | % --- Executes during object creation, after setting all properties. 190 | function popupmenu3_CreateFcn(hObject, eventdata, handles) 191 | % hObject handle to popupmenu3 (see GCBO) 192 | % eventdata reserved - to be defined in a future version of MATLAB 193 | % handles empty - handles not created until after all CreateFcns called 194 | 195 | % Hint: popupmenu controls usually have a white background on Windows. 196 | % See ISPC and COMPUTER. 197 | if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) 198 | set(hObject,'BackgroundColor','white'); 199 | end 200 | 201 | 202 | % --- Executes on selection change in popupmenu4. 203 | function popupmenu4_Callback(hObject, eventdata, handles) 204 | % hObject handle to popupmenu4 (see GCBO) 205 | % eventdata reserved - to be defined in a future version of MATLAB 206 | % handles structure with handles and user data (see GUIDATA) 207 | 208 | % Hints: contents = cellstr(get(hObject,'String')) returns popupmenu4 contents as cell array 209 | % contents{get(hObject,'Value')} returns selected item from popupmenu4 210 | 211 | val = get(hObject, 'Value'); 212 | str = get(hObject, 'String'); 213 | handles.pos = str{val}; 214 | 215 | % Update handles structure 216 | guidata(hObject, handles); 217 | 218 | 219 | % --- Executes during object creation, after setting all properties. 220 | function popupmenu4_CreateFcn(hObject, eventdata, handles) 221 | % hObject handle to popupmenu4 (see GCBO) 222 | % eventdata reserved - to be defined in a future version of MATLAB 223 | % handles empty - handles not created until after all CreateFcns called 224 | 225 | % Hint: popupmenu controls usually have a white background on Windows. 226 | % See ISPC and COMPUTER. 227 | if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) 228 | set(hObject,'BackgroundColor','white'); 229 | end 230 | 231 | 232 | % --- Executes on selection change in listbox1. 233 | function listbox1_Callback(hObject, eventdata, handles) 234 | % hObject handle to listbox1 (see GCBO) 235 | % eventdata reserved - to be defined in a future version of MATLAB 236 | % handles structure with handles and user data (see GUIDATA) 237 | 238 | % Hints: contents = cellstr(get(hObject,'String')) returns listbox1 contents as cell array 239 | % contents{get(hObject,'Value')} returns selected item from listbox1 240 | 241 | 242 | % --- Executes during object creation, after setting all properties. 243 | function listbox1_CreateFcn(hObject, eventdata, handles) 244 | % hObject handle to listbox1 (see GCBO) 245 | % eventdata reserved - to be defined in a future version of MATLAB 246 | % handles empty - handles not created until after all CreateFcns called 247 | 248 | % Hint: listbox controls usually have a white background on Windows. 249 | % See ISPC and COMPUTER. 250 | if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) 251 | set(hObject,'BackgroundColor','white'); 252 | end 253 | 254 | 255 | % --- Executes on selection change in popupmenu5. 256 | function popupmenu5_Callback(hObject, eventdata, handles) 257 | % hObject handle to popupmenu5 (see GCBO) 258 | % eventdata reserved - to be defined in a future version of MATLAB 259 | % handles structure with handles and user data (see GUIDATA) 260 | 261 | % Hints: contents = cellstr(get(hObject,'String')) returns popupmenu5 contents as cell array 262 | % contents{get(hObject,'Value')} returns selected item from popupmenu5 263 | 264 | val = get(hObject, 'Value'); 265 | str = get(hObject, 'String'); 266 | handles.Nu = str2num(str{val}); 267 | 268 | % Update handles structure 269 | guidata(hObject, handles); 270 | 271 | 272 | % --- Executes during object creation, after setting all properties. 273 | function popupmenu5_CreateFcn(hObject, eventdata, handles) 274 | % hObject handle to popupmenu5 (see GCBO) 275 | % eventdata reserved - to be defined in a future version of MATLAB 276 | % handles empty - handles not created until after all CreateFcns called 277 | 278 | % Hint: popupmenu controls usually have a white background on Windows. 279 | % See ISPC and COMPUTER. 280 | if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) 281 | set(hObject,'BackgroundColor','white'); 282 | end 283 | 284 | 285 | % --- Executes on button press in pushbutton1. 286 | function pushbutton1_Callback(hObject, eventdata, handles) 287 | % hObject handle to pushbutton1 (see GCBO) 288 | % eventdata reserved - to be defined in a future version of MATLAB 289 | % handles structure with handles and user data (see GUIDATA) 290 | 291 | global sim 292 | 293 | if get(handles.pushbutton1,'Value') 294 | 295 | if strcmp(get(handles.pushbutton1,'String'),'STOP') 296 | set(handles.pushbutton1,'String','START'); 297 | drawnow(); 298 | sim = false; 299 | else 300 | set(handles.pushbutton1,'String','STOP'); 301 | drawnow(); 302 | sim = true; 303 | set(handles.pushbutton1,'Value',0); 304 | % Reinitialize Parameters 305 | handles = initialization(handles); 306 | % Simulation loop 307 | handles = simulate(handles); 308 | end 309 | 310 | end 311 | 312 | % Update handles structure 313 | guidata(hObject, handles); 314 | -------------------------------------------------------------------------------- /images/gui-example.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gitAnto/MASTT/7e9f201aa02beacb88f18952180dffb8d8db19d1/images/gui-example.png --------------------------------------------------------------------------------