-
Notifications
You must be signed in to change notification settings - Fork 1
/
Network.m
253 lines (195 loc) · 10.2 KB
/
Network.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
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
classdef Network < handle
%UNTITLED4 Summary of this class goes here
% Detailed explanation goes here
properties
networkIndex
numOfPlatoons % N
numOfVehicles = [] % [n_1,n_2,...,n_N]
platoons = [] % all the platoons
% State variables
time
error
cost
% For plotting
graphics = [];
end
methods
function obj = Network(indexVal,numOfPlatoons,numOfVehicles,parameters,states,desiredSeparation,noiseMean,noiseStd)
obj.networkIndex = indexVal;
obj.numOfPlatoons = numOfPlatoons;
obj.numOfVehicles = numOfVehicles;
% create the platoons
platoons = [];
for k = 1:1:numOfPlatoons
platoon = Platoon(k,numOfVehicles(k),parameters{k},states{k},desiredSeparation{k},noiseMean{k},noiseStd{k});
platoons = [platoons, platoon];
end
obj.platoons = platoons;
obj.time = 0;
obj.error = [0;0;0];
obj.cost = 0;
end
function outputArg = drawNetwork(obj,figNum)
figure(figNum); hold on;
if ~isempty(obj.graphics)
delete(obj.graphics(1));
delete(obj.graphics(2));
delete(obj.graphics(3));
delete(obj.graphics(4));
delete(obj.graphics(5));
delete(obj.graphics(6));
end
% Draw platoons
for k = 1:1:obj.numOfPlatoons
obj.platoons(k).drawPlatoon(figNum);
end
% Coordinate of the boundary of the bounding box
posY1 = -5;
posY2 = 20;
lastPlatoon = obj.platoons(obj.numOfPlatoons);
posX1 = lastPlatoon.vehicles(lastPlatoon.numOfVehicles).states(1)-10;
posX2 = obj.platoons(1).vehicles(1).states(1)+10;
% Plot the 3 topics, i.e., time,error,cost, on the top of the simulator
obj.graphics(1) = text(posX1+5,posY2-5,['Time: ',num2str(obj.time)],'FontSize',12);
obj.graphics(2) = text(posX1+30,posY2-5,['Error: ',num2str(round(norm(obj.error),1))],'FontSize',12);
obj.graphics(3) = text(posX1+55,posY2-5,['Cost: ',num2str(round(obj.cost,1))],'FontSize',12);
obj.graphics(4) = text(posX1+5,posY2-8,['P-Error: ',num2str(round(obj.error(1),1))],'FontSize',12);
obj.graphics(5) = text(posX1+30,posY2-8,['V-Error: ',num2str(round(obj.error(2),1))],'FontSize',12);
obj.graphics(6) = text(posX1+55,posY2-8,['A-Error: ',num2str(round(obj.error(3),1))],'FontSize',12);
axis([posX1,posX2,posY1,posY2])
end
function outputArg = update(obj,t,dt)
% Do the necessary computations/generations to generate the signals to initiate the program
totalError = zeros(3,1);
for k = 1:1:obj.numOfPlatoons
% Generate the noises
obj.platoons(k).generateNoises();
% Error Dynamics - I : Computing Platooning Errors and Controls
% obj.platoons(k).computePlatooningErrors1();
% obj.platoons(k).computeControlInputs1(t);
% Error Dynamics - II : Computing Platooning Errors and Controls
obj.platoons(k).computePlatooningErrors2();
obj.platoons(k).computeControlInputs2(t);
% Update the states
platoonError = obj.platoons(k).update(t,dt);
totalError = totalError + platoonError;
end
% Update the time, error and cost
costSoFar = obj.cost^2*obj.time;
obj.time = obj.time + dt;
obj.error = totalError;
costSoFar = costSoFar + (norm(obj.error))^2*dt;
obj.cost = sqrt(costSoFar/obj.time);
end
function outputArg = redrawNetwork(obj,figNum)
% Redraw platoons
for k = 1:1:obj.numOfPlatoons
obj.platoons(k).redrawPlatoon(figNum); % Redraw the platoons by calling the redrawPlatoon method in platoon class
end
% Update the 3 topics of the figure
if ~isempty(obj.graphics)
delete(obj.graphics(1));
delete(obj.graphics(2));
delete(obj.graphics(3));
delete(obj.graphics(4));
delete(obj.graphics(5));
delete(obj.graphics(6));
% Coordinate of the boundary of the bounding box
posY1 = -5;
posY2 = 20;
lastPlatoon = obj.platoons(obj.numOfPlatoons);
posX1 = lastPlatoon.vehicles(lastPlatoon.numOfVehicles).states(1)-10;
posX2 = obj.platoons(1).vehicles(1).states(1)+10;
obj.graphics(1) = text(posX1+5,posY2-5,['Time: ',num2str(obj.time)],'FontSize',12);
obj.graphics(2) = text(posX1+30,posY2-5,['Error: ',num2str(round(norm(obj.error),1))],'FontSize',12);
obj.graphics(3) = text(posX1+55,posY2-5,['Cost: ',num2str(round(obj.cost,1))],'FontSize',12);
obj.graphics(4) = text(posX1+5,posY2-8,['P-Error: ',num2str(round(obj.error(1),1))],'FontSize',12);
obj.graphics(5) = text(posX1+30,posY2-8,['V-Error: ',num2str(round(obj.error(2),1))],'FontSize',12);
obj.graphics(6) = text(posX1+55,posY2-8,['A-Error: ',num2str(round(obj.error(3),1))],'FontSize',12);
axis([posX1,posX2,posY1,posY2])
% axis([min(posX1,posX2),max(posX1,posX2),posY1,posY2])
end
end
function outputArg = generateLeadersControlProfiles(obj,dt,tVals,vVals)
% Here we basically have to derive each leader's control trajectory
% aVals computation
aVals = [];
for k = 1:1:(length(tVals)-1)
v_1 = vVals(k);
t_1 = tVals(k);
v_2 = vVals(k+1);
t_2 = tVals(k+1);
a_1 = (v_2-v_1)/(t_2-t_1);
aVals = [aVals, a_1];
end
aVals = [aVals, 0];
% uVals computation
uVals= [];
a_0 = 0;
for k = 1:1:length(tVals)
a_1 = aVals(k);
u_1 = (a_1-a_0)/dt;
a_0 = a_1;
uVals = [uVals, u_1];
end
% Setting initial velocities
for k = 1:1:obj.numOfPlatoons
obj.platoons(k).vehicles(1).states(2) = vVals(1);
obj.platoons(k).vehicles(1).plannedControls = [tVals',uVals'];
end
end
function output = loadPlatoonControllers(obj,errorDynamicsType,isCentralized,isDSS,isOnlyStabilizing,gammaSqBar,nuBar,rhoBar,pVals)
for k = 1:1:obj.numOfPlatoons
%% Controller Types:
% There can be three factors that determines the controller
% type: (i) Centralized/Decentralized, (ii)
% Stabilizing/Robust, and (iii) Error Dynamics Type
if errorDynamicsType == 1 % Error dynamics formulation I
if isCentralized == 1 % Centralized
if isOnlyStabilizing == 1 % Only Stabilizing
status = obj.platoons(k).centralizedStabilizingControllerSynthesis1(nuBar,rhoBar);
else % Robust
status = obj.platoons(k).centralizedRobustControllerSynthesis1(nuBar,rhoBar,gammaSqBar);
end
else % Decentralized
if isOnlyStabilizing == 1 % Only Stabilizing
status = obj.platoons(k).decentralizedStabilizingControllerSynthesis1(nuBar,rhoBar);
else % Robust
status = obj.platoons(k).decentralizedRobustControllerSynthesis1(nuBar,rhoBar,gammaSqBar);
end
end
else % Error dynamics formulation II
if isCentralized == 1 && ~isDSS % Centralized & Not DSS
if isOnlyStabilizing == 1 % Only Stabilizing
status = obj.platoons(k).centralizedStabilizingControllerSynthesis2(nuBar,rhoBar);
else % Robust
status = obj.platoons(k).centralizedRobustControllerSynthesis2(pVals(k,:)); % nuBar,rhoBar,gammaSqBar are no longer needed
end
elseif ~isCentralized == 1 && ~isDSS % Decentralized & Not DSS
if isOnlyStabilizing == 1 % Only Stabilizing
status = obj.platoons(k).decentralizedStabilizingControllerSynthesis2(nuBar,rhoBar);
else % Robust
status = obj.platoons(k).decentralizedRobustControllerSynthesis2(pVals(k,:));
end
elseif ~isCentralized == 1 && isDSS % Decentralized & DSS
status = obj.platoons(k).decentralizedRobustControllerSynthesisDSS2(pVals(k,:));
% Robust
end
end
% Success or Failure
if status == 1
disp(['Synthesis Success at Platoon ',num2str(k),'.']);
else
disp(['Synthesis Failed at Platoon ',num2str(k),'.']);
end
end
end
function pVals = optimizeCodesignParameters(obj,isCentralized,isDSS)
pVals = [];
for k = 1:1:obj.numOfPlatoons
pVals_k = obj.platoons(k).optimizeCodesignParameters(isCentralized,isDSS);
pVals = [pVals; pVals_k];
end
end
end
end