-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathbatch_predSIM.m
125 lines (88 loc) · 2.9 KB
/
batch_predSIM.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
function batch_predSIM
tic
% batch_predSIM runs batch simulations of predSIM. The initial position of
% the prey item is defined using ANG and RAD.
% angles to define initial prey position
ang = linspace(0,pi/2 - pi/12,20); % (rad)
% radial distances to define initial prey position
rad = linspace(0.01, 0.3, 20); % (m)
% Initial gain parameter
% kp = 3.0e-3;
% Get size of initial position vectors
N1 = length(rad);
N2 = length(ang);
% Set up data containers
distInit = zeros(N1,N2);
phiInit = zeros(N1,N2);
phiFinal = zeros(N1,N2);
captureInd = zeros(N1,N2);
totTurns = zeros(N1,N2);
gainParam = zeros(N1,N2);
preyAng = zeros(N1,N2);
% sol = cell(N1,N2);
% Outer loop
for j=1:N1
% Set current prey radial distance
currDist = rad(j);
% Inner loop
parfor k=1:N2
% Initial gain parameter
kp = 3e-3;
% Set current angle
currAng = ang(k);
% Prey initial position
preyX = currDist * cos(currAng); % (m)
preyY = currDist * sin(currAng); % (m)
% Run the simulation
sol = run_predSIM(preyX,preyY,kp);
% Store initial gain
gain_kp = kp;
% Store capture indicator
capture_ind = sol.capture;
while ~sol.capture && kp < 11e-3 %
% Adjust gain parameter
kp = kp * 1.05;
% Collect gain parameter
gain_kp = [gain_kp; kp];
% rerun simulation with new gain
sol = run_predSIM(preyX,preyY,kp);
% Collect capture indicator
capture_ind = [capture_ind; sol.capture];
end
% Store simulation results (of last run)
% Initial Distance
distInit(j,k) = sol.distInit;
% Initial bearing angle
phiInit(j,k) = sol.phiPre(1);
% Final bearing angle
phiFinal(j,k) = sol.phiPost(end);
% Capture inidicator
captureInd(j,k) = sol.capture;
% Total # of turns
totTurns(j,k) = sol.turns;
% Gain parameter
gainParam(j,k) = kp;
% Angle to define initial prey position
preyAng(j,k) = currAng;
% Full solution variables
solFull{j,k} = sol;
% All gain parameters
gainParamAll{j,k} = gain_kp;
% % Reset gain parameter
% kp = 3.0e-3;
% Clear results for next iteration
% clear sol preyX preyY
end
% disp(' Running next distance')
end
dOut.dist = distInit;
dOut.phiI = phiInit;
dOut.phiF = phiFinal;
dOut.cap = captureInd;
dOut.turns= totTurns;
dOut.gain = gainParam;
dOut.pAng = preyAng;
save('batch_predSim.mat','dOut','solFull','gainParamAll')
toc
end
%