From cbe1e6241d95327dfc077080f9e5ef8466f4b232 Mon Sep 17 00:00:00 2001 From: trhoangdung Date: Fri, 1 Nov 2019 14:57:35 -0500 Subject: [PATCH] finish LinearNNCS reachLive, and reach method --- code/nnv/engine/nncs/DLinearNNCS.m | 4 +- code/nnv/engine/nncs/LinearNNCS.m | 245 ++++++++++++------ code/nnv/engine/set/Box.m | 6 + .../LinearNNCS/test_LinearNNCS_reachLive.m | 120 +++++++++ .../LinearNNCS/test_LinearNNCS_reach_exact.m | 125 +++++++++ 5 files changed, 421 insertions(+), 79 deletions(-) create mode 100644 code/nnv/tests/nncs/LinearNNCS/test_LinearNNCS_reachLive.m create mode 100644 code/nnv/tests/nncs/LinearNNCS/test_LinearNNCS_reach_exact.m diff --git a/code/nnv/engine/nncs/DLinearNNCS.m b/code/nnv/engine/nncs/DLinearNNCS.m index b858d081cd..76cd91d5c8 100644 --- a/code/nnv/engine/nncs/DLinearNNCS.m +++ b/code/nnv/engine/nncs/DLinearNNCS.m @@ -138,7 +138,7 @@ function start_pool(obj) init_set1 = varargin{2}; ref_input1 = varargin{3}; numOfSteps = varargin{4}; - method1 = 'exact-star'; + method1 = 'approx-star'; numCores1 = 1; case 5 @@ -261,7 +261,7 @@ function start_pool(obj) init_set1 = varargin{2}; ref_input1 = varargin{3}; numOfSteps = varargin{4}; - method1 = 'exact-star'; + method1 = 'approx-star'; numCores1 = 1; map_mat = zeros(1, obj.plant.dim); map_mat(1) = 1; % default setting, plot the reachable set of the first state versus time steps diff --git a/code/nnv/engine/nncs/LinearNNCS.m b/code/nnv/engine/nncs/LinearNNCS.m index 7f7a2eae21..285e39fb3b 100644 --- a/code/nnv/engine/nncs/LinearNNCS.m +++ b/code/nnv/engine/nncs/LinearNNCS.m @@ -49,7 +49,7 @@ plantReachSet = {}; plantIntermediateReachSet = {}; - plantNumOfSimSteps = 100; % default number of simulation steps for the plant between two control step k-1 and k + plantNumOfSimSteps = 20; % default number of simulation steps for the plant between two control step k-1 and k controlPeriod = 0.1; % default control period controllerReachSet = {}; numCores = 1; % default setting, using single core for computation @@ -136,6 +136,8 @@ function start_pool(obj) % @numOfSteps: number of steps % @method: 'exact-star' or 'approx-star' % @numCores: number of cores used in computation + % @numOfSimSteps: number of sim steps to compute reachable set + % for the plant % NOTE***: parallel computing may not help due to % comuninication overhead in the computation @@ -152,8 +154,9 @@ function start_pool(obj) init_set1 = varargin{2}; ref_input1 = varargin{3}; numOfSteps = varargin{4}; - method1 = 'exact-star'; + method1 = 'approx-star'; numCores1 = 1; + numOfSimSteps = 20; case 5 obj = varargin{1}; @@ -162,6 +165,8 @@ function start_pool(obj) numOfSteps = varargin{4}; method1 = varargin{5}; numCores1 = 1; + numOfSimSteps = 20; + case 6 obj = varargin{1}; @@ -170,7 +175,8 @@ function start_pool(obj) numOfSteps = varargin{4}; method1 = varargin{5}; numCores1 = varargin{6}; - + numOfSimSteps = 20; + case 7 obj = varargin{1}; init_set1 = varargin{2}; @@ -178,6 +184,8 @@ function start_pool(obj) numOfSteps = varargin{4}; method1 = varargin{5}; numCores1 = varargin{6}; + numOfSimSteps = varargin{7}; + otherwise error('Invalid number of inputs, should be 3, 4, 5, or 6'); @@ -199,6 +207,9 @@ function start_pool(obj) error('Invalid number of steps'); end + if numOfSimSteps < 1 + error('Invalid number of simulation steps for computing reachable set of the plant'); + end if ~isempty(ref_input1) && ~isa(ref_input1, 'Star') && size(ref_input1, 2) ~= 1 && size(ref_input1, 1) ~= obj.nI_ref error('Invalid reference input vector'); @@ -213,6 +224,7 @@ function start_pool(obj) obj.plantReachSet = cell(1, numOfSteps); obj.plantIntermediateReachSet = cell(1,numOfSteps); obj.controllerReachSet = cell(1, numOfSteps); + obj.plantNumOfSimSteps = numOfSimSteps; if obj.numCores > 1 obj.start_pool; @@ -276,12 +288,16 @@ function start_pool(obj) init_set1 = varargin{2}; ref_input1 = varargin{3}; numOfSteps = varargin{4}; - method1 = 'exact-star'; + method1 = 'approx-star'; numCores1 = 1; + numOfSimSteps = 20; % number of simulation steps used for platn reachability analysis map_mat = zeros(1, obj.plant.dim); map_mat(1) = 1; % default setting, plot the reachable set of the first state versus time steps map_vec = []; % default setting color = 'blue'; + boundary_mat = []; + boundary_vec = []; + case 5 obj = varargin{1}; @@ -290,10 +306,13 @@ function start_pool(obj) numOfSteps = varargin{4}; method1 = varargin{5}; numCores1 = 1; + numOfSimSteps = 20; % number of simulation steps used for platn reachability analysis map_mat = zeros(1, obj.plant.dim); map_mat(1) = 1; % default setting, plot the reachable set of the first state versus time steps map_vec = []; % default setting color = 'blue'; + boundary_mat = []; + boundary_vec = []; case 6 obj = varargin{1}; @@ -302,10 +321,13 @@ function start_pool(obj) numOfSteps = varargin{4}; method1 = varargin{5}; numCores1 = varargin{6}; + numOfSimSteps = 20; % number of simulation steps used for platn reachability analysis map_mat = zeros(1, obj.plant.dim); map_mat(1) = 1; % default setting, plot the reachable set of the first state versus time steps map_vec = []; % default setting color = 'blue'; + boundary_mat = []; + boundary_vec = []; case 7 obj = varargin{1}; @@ -314,31 +336,41 @@ function start_pool(obj) numOfSteps = varargin{4}; method1 = varargin{5}; numCores1 = varargin{6}; + numOfSimSteps = 20; % number of simulation steps used for platn reachability analysis map_mat = varargin{7}; map_vec = []; % default setting color = 'blue'; + boundary_mat = []; + boundary_vec = []; case 8 obj = varargin{1}; init_set1 = varargin{2}; ref_input1 = varargin{3}; numOfSteps = varargin{4}; + numOfSimSteps = 20; % number of simulation steps used for platn reachability analysis method1 = varargin{5}; numCores1 = varargin{6}; map_mat = varargin{7}; map_vec = varargin{8}; color = 'blue'; + boundary_mat = []; + boundary_vec = []; + case 9 obj = varargin{1}; init_set1 = varargin{2}; ref_input1 = varargin{3}; numOfSteps = varargin{4}; + numOfSimSteps = 20; % number of simulation steps used for platn reachability analysis method1 = varargin{5}; numCores1 = varargin{6}; map_mat = varargin{7}; map_vec = varargin{8}; color = varargin{9}; + boundary_mat = []; + boundary_vec = []; case 11 @@ -346,6 +378,7 @@ function start_pool(obj) init_set1 = varargin{2}; ref_input1 = varargin{3}; numOfSteps = varargin{4}; + numOfSimSteps = 20; % number of simulation steps used for platn reachability analysis method1 = varargin{5}; numCores1 = varargin{6}; map_mat = varargin{7}; @@ -369,6 +402,37 @@ function start_pool(obj) end + case 12 + + obj = varargin{1}; + init_set1 = varargin{2}; + ref_input1 = varargin{3}; + numOfSteps = varargin{4}; + method1 = varargin{5}; + numCores1 = varargin{6}; + map_mat = varargin{7}; + map_vec = varargin{8}; + color = varargin{9}; + boundary_mat = varargin{10}; + boundary_vec = varargin{11}; + numOfSimSteps = varargin{12}; % number of simulation steps used for platn reachability analysis + + % Indication of unsafe behavior + % The output: y = map_mat * x + map_vec + % The boundary of the output: y_b = boundary_mat * x + boundary_vec + % Used to indicate whether a output y reach its unsafe + % boundary, i.e. y == y_b? + + if (size(boundary_mat, 1) ~= size(map_mat, 1)) || (size(boundary_mat, 2) ~= size(map_mat, 2)) + error('The size of boundary matrix is not equal to the size of the map_mat, we require this for plotting unsafe boundary'); + end + + if (size(boundary_vec, 1) ~= size(boundary_mat, 1)) || (size(boundary_vec, 2) ~= 1) + error('Invalid boundary vector, it should have one column and have the same number of rows as the boundary matrix'); + end + + + otherwise error('Invalid number of inputs, should be 3, 4, 5, 6, 7, 8, or 9'); end @@ -400,8 +464,10 @@ function start_pool(obj) obj.method = method1; obj.init_set = init_set1; + obj.plantNumOfSimSteps = numOfSimSteps; obj.plantReachSet = cell(numOfSteps, 1); obj.controllerReachSet = cell(numOfSteps, 1); + if obj.numCores > 1 obj.start_pool; @@ -430,27 +496,16 @@ function start_pool(obj) if size(map_mat, 1) == 1 - times = 0:1:k+1; - - if nargin == 11 - obj.plotStepOutputReachSets('red', boundary_mat, boundary_vec, k); - hold on; - end - - obj.plotStepOutputReachSets(color, map_mat, map_vec, k); - pause(1.0); + times = 0: obj.controlPeriod: k*obj.controlPeriod; + obj.plotStepOutputReachSets(color, map_mat, map_vec, boundary_mat, boundary_vec, k); ax = gca; ax.XTick = times; hold on; elseif (size(map_mat, 1) == 2) || (size(map_mat, 1) == 3) - if nargin == 11 - obj.plotStepOutputReachSets('red', boundary_mat, boundary_vec, k); - hold on; - end - obj.plotStepOutputReachSets(color, map_mat, map_vec, k); - pause(1.0); - hold on; + + obj.plotStepOutputReachSets(color, map_mat, map_vec, boundary_mat, boundary_vec, k); + elseif size(indexes, 1) > 3 error('NNV plots only three-dimensional output reachable set, please limit number of rows in map_mat <= 3'); end @@ -488,12 +543,12 @@ function start_pool(obj) X_imd_trans = obj.transPlant.simReach(obj.plantReachMethod, trans_init_set, [], h, obj.plantNumOfSimSteps, []); X_imd = []; - for i=2:obj.plantNumOfSimSteps +1 + for i=1:obj.plantNumOfSimSteps +1 X_imd = [X_imd X_imd_trans(i).affineMap(obj.transPlant.C, [])]; end obj.plantIntermediateReachSet{k} = cell(1,1); - obj.plantIntermediateReachSet{k}{1,1} = X_imd; - X = X_imd(obj.plantNumOfSimSteps); + obj.plantIntermediateReachSet{k}{1,1} = {X_imd}; + X = X_imd(obj.plantNumOfSimSteps+1); end if k>1 @@ -506,24 +561,23 @@ function start_pool(obj) % compute exact reachable set of the continuous plant from t[k] to t[k+1] using star set X = []; - for i=1:nX - + obj.plantIntermediateReachSet{k} = cell(1, nX); + for i=1:nX U0_i = U0{i}; % control set corresponding to the initial set of state X0(i) mU = length(U0_i); % number of control sets corresponding to the initial set of state X0(i) - obj.plantIntermediateReachSet{k} = cell(nX, mU); + obj.plantIntermediateReachSet{k}{i} = cell(mU, 1); for j=1:mU new_V = [X0(i).V; U0_i(j).V]; trans_init_set = Star(new_V, U0_i(j).C, U0_i(j).d, U0_i(j).predicate_lb, U0_i(j).predicate_ub); X_imd = obj.transPlant.simReach(obj.plantReachMethod, trans_init_set, [], h, obj.plantNumOfSimSteps, []); X1 = []; - for l=2:obj.plantNumOfSimSteps + 1 + for l=1:obj.plantNumOfSimSteps + 1 X1 = [X1 X_imd(l).affineMap(obj.transPlant.C, [])]; end - X = [X X1(obj.plantNumOfSimSteps)]; % store plant reach set at t[k], which is the initial set for the next control period t[k+1] - obj.plantIntermediateReachSet{k}{i, j} = X1; % store intermediate plant reach set + X = [X X1(obj.plantNumOfSimSteps+1)]; % store plant reach set at t[k], which is the initial set for the next control period t[k+1] + obj.plantIntermediateReachSet{k}{i}{j} = X1; % store intermediate plant reach set end - - + end end @@ -658,25 +712,10 @@ function start_pool(obj) % get output reach sets n = length(obj.plantIntermediateReachSet); + Y = cell(1, n); for i=1:n - Xi = obj.plantIntermediateReachSet{i}; - [n1, m1] = size(Xi); - Y1 = cell(n1, m1); - for j=1:n1 - for l=1:m1 - X1 = Xi{j, l}; - m2 = length(X1); - X2 = []; - for g=1:m2 - X2 = [X2 X1(g).affineMap(map_mat, map_vec)]; - end - Y1{j, l} = X2; - end - end - - Y{i} = Y1; - + Y{i} = obj.getStepOutputReachSet(map_mat, map_vec, i); end end @@ -720,20 +759,24 @@ function start_pool(obj) X = obj.plantIntermediateReachSet{k}; - [n1, m1] = size(X); - Y = cell(n1, m1); - for j=1:n1 - for l=1:m1 - X1 = X{j, l}; - m2 = length(X1); - X2 = []; - for g=1:m2 - X2 = [X2 X1(g).affineMap(map_mat, map_vec)]; + nX = length(X); + + Y = cell(1, obj.plantNumOfSimSteps+1); + + for l=1:obj.plantNumOfSimSteps+1 + Y1 = []; + for i=1:nX + Xi = X{i}; + nXi = length(Xi); + for j=1:nXi + Xij = Xi{j}; + Yijl = Xij(l).affineMap(map_mat, map_vec); + Y1 = [Y1 Yijl]; end - Y{j, l} = X2; end + Y{l} = Y1; end - + end @@ -923,18 +966,18 @@ function plotOutputReachSets(obj, color, map_mat, map_vec) option = size(map_mat, 1); - G = obj.init_set.affineMap(map_mat, map_vec); + I0 = obj.init_set.affineMap(map_mat, map_vec); + G = I0.getBox; h = obj.controlPeriod / obj.plantNumOfSimSteps; if option == 1 % plot 1D, output versus time steps for i=1:n Y1 = Y{i}; - [~, m1] = size(Y1); % m1 is number of simulation steps for the plant in one control step of the controller - G1 = []; - for j=1:m1 - G1 = [G1 Star.get_hypercube_hull(Y1{:, j})]; + for j=1:obj.plantNumOfSimSteps+1 + B = Star.get_hypercube_hull(Y1{j}); + G1 = [G1 B.toStar]; end T1 = (i-1)*obj.controlPeriod:h:i*obj.controlPeriod; @@ -951,10 +994,18 @@ function plotOutputReachSets(obj, color, map_mat, map_vec) if option == 2 || option == 3 % plot 2D or 3D for i=1:n - G = [G Y{i}]; + Y1 = Y{i}; + for j=1:obj.plantNumOfSimSteps+1 + B = Star.get_hypercube_hull(Y1{j}); + G = [G B]; + end end - Star.plots(G, color); + if option == 2 + Box.plotBoxes_2D_noFill(G, 1, 2, color); + elseif option == 3 + Box.plotBoxes_3D_noFill(G, 1, 2, 3, color); + end end @@ -967,19 +1018,29 @@ function plotOutputReachSets(obj, color, map_mat, map_vec) % plot step output reachable sets - function plotStepOutputReachSets(obj, color, map_mat, map_vec, k) + function plotStepOutputReachSets(obj, color, map_mat, map_vec, unsafe_mat, unsafe_vec, k) % @map_mat: a mapping matrix % @map_vec: mapping vector % Y = map_mat * X + map_vec + % @unsafe_mat: unsafe region matrix + % @unsafe_vec: unsafe region vector % @color: color % @k: step index % author: Dung Tran % date: 10/2/2019 + % update: 11/2/2019 + + Y = obj.getStepOutputReachSet(map_mat, map_vec, k); % output set at step k - Y = obj.getStepOutputReachSet(map_mat, map_vec, k); + if ~isempty(unsafe_mat) && ~isempty(unsafe_vec) + US = obj.getStepOutputReachSet(unsafe_mat, unsafe_vec, k); % unsafe region output + else + US = []; + end + h = obj.controlPeriod / obj.plantNumOfSimSteps; n = length(Y); % plot output reach sets @@ -988,19 +1049,49 @@ function plotStepOutputReachSets(obj, color, map_mat, map_vec, k) if option == 1 % plot 1D, output versus time steps - - B = Star.get_hypercube_hull(Y); - ymin = B.lb; - ymax = B.ub; - y = [ymin ymin ymax ymax ymin]; - x = [k-1 k k k-1 k-1]; - plot(x, y, color) + + for i=2:obj.plantNumOfSimSteps + 1 + B = Star.get_hypercube_hull(Y{i}); + ymin = B.lb; + ymax = B.ub; + y = [ymin ymin ymax ymax ymin]; + xmin = (k-1)*obj.controlPeriod + (i-2)*h; + xmax = xmin + h; + x = [xmin xmax xmax xmin xmin]; + plot(x, y, color); + hold on; + + if ~isempty(US) + B = Star.get_hypercube_hull(US{i}); + ymin = B.lb; + ymax = B.ub; + y = [ymin ymin ymax ymax ymin]; + xmin = (k-1)*obj.controlPeriod + (i-2)*h; + xmax = xmin + h; + x = [xmin xmax xmax xmin xmin]; + plot(x, y, 'red'); + end + pause(0.25); + + end end if option == 2 || option == 3 % plot 2D or 3D - - Star.plots(Y, color); + + for i=2:obj.plantNumOfSimSteps + 1 + + B = Star.get_hypercube_hull(Y{i}); + Star.plots(B.toStar, color); + hold on; + + if ~isempty(US) + B = Star.get_hypercube_hull(US{i}); + Star.plots(B.toStar, 'red'); + hold on; + end + pause(0.25); + end end diff --git a/code/nnv/engine/set/Box.m b/code/nnv/engine/set/Box.m index 18bf3e8dbd..5ba3c71b9e 100644 --- a/code/nnv/engine/set/Box.m +++ b/code/nnv/engine/set/Box.m @@ -213,6 +213,12 @@ function plot(obj) Z = Zono(obj.center, obj.generators); end + % get Range + function [lb, ub] = getRange(obj) + lb = obj.lb; + ub = obj.ub; + end + % get all vertices of the box function V = getVertices(obj) % author: Dung Tran diff --git a/code/nnv/tests/nncs/LinearNNCS/test_LinearNNCS_reachLive.m b/code/nnv/tests/nncs/LinearNNCS/test_LinearNNCS_reachLive.m new file mode 100644 index 0000000000..3cdb4943f4 --- /dev/null +++ b/code/nnv/tests/nncs/LinearNNCS/test_LinearNNCS_reachLive.m @@ -0,0 +1,120 @@ +% Reachability analysis for Linear ACC model +% Dung Tran: 10/4/2019 + + + + +%% System model +% x1 = lead_car position +% x2 = lead_car velocity +% x3 = lead_car internal state + +% x4 = ego_car position +% x5 = ego_car velocity +% x6 = ego_car internal state + +% lead_car dynamics +%dx(1,1)=x(2); +%dx(2,1) = x(3); +%dx(3,1) = -2 * x(3) + 2 * a_lead - mu*x(2)^2; + +% ego car dynamics +%dx(4,1)= x(5); +%dx(5,1) = x(6); +%dx(6,1) = -2 * x(6) + 2 * a_ego - mu*x(5)^2; + + +% let x7 = -2*x(3) + 2 * a_lead -> x7(0) = -2*x(3)(0) + 2*alead +% -> dx7 = -2dx3 + + +A = [0 1 0 0 0 0 0; 0 0 1 0 0 0 0; 0 0 0 0 0 0 1; 0 0 0 0 1 0 0; 0 0 0 0 0 1 0; 0 0 0 0 0 -2 0; 0 0 -2 0 0 0 0]; +B = [0; 0; 0; 0; 0; 2; 0]; +C = [1 0 0 -1 0 0 0; 0 1 0 0 -1 0 0; 0 0 0 0 1 0 0]; % feedback relative distance, relative velocity, longitudinal velocity +D = [0; 0; 0]; + +plant = LinearODE(A, B, C, D); % continuous plant model + +% the neural network provides a_ego control input to the plant +% a_lead = -2 + + +%% Controller +load controller_3_20.mat; + +n = length(weights); +Layers = []; +for i=1:n - 1 + L = LayerS(weights{1, i}, bias{i, 1}, 'poslin'); + Layers = [Layers L]; +end +L = LayerS(weights{1, n}, bias{n, 1}, 'purelin'); +Layers = [Layers L]; +Controller = FFNNS(Layers); % feedforward neural network controller + + +%% NNCS + +ncs = LinearNNCS(Controller, plant); % a discrete linear neural network control system + +%% Initial Set of states and reference inputs + +% reference input for neural network controller +% t_gap = 1.4; v_set = 30; + +ref_input = [30; 1.4]; + +% initial condition of the plant + +% initial position of lead car x_lead +x_lead = [90 100]; +% initial condition of v_lead +v_lead = [32 35]; +% initial condition of x_internal_lead +internal_acc_lead = [0 0]; +% initial condition of x_ego +x_ego = [10 11]; +% initial condition of v_ego +v_ego = [30 30.2]; +% initial condition of x_internal_ego +internal_acc_ego = [0 0]; +% initial condition for new introduced variable +x7_0 = [-4 -4]; % x7 = -2*x(3) + 2 * a_lead -> x7(0) = -2*x(3)(0) + 2*alead = -2*0 + 2*-2 = -4 + + +x1 = x_lead; +lb = [x1(1); v_lead(1); internal_acc_lead(1); x_ego(1); v_ego(1); internal_acc_ego(1); x7_0(1)]; +ub = [x1(2); v_lead(2); internal_acc_lead(2); x_ego(2); v_ego(2); internal_acc_ego(2); x7_0(2)]; +init_set = Star(lb, ub); + + +%% Live Reachability Analysis 1 + +numSteps = 4; +method = 'approx-star'; +%method = 'exact-star'; +numCores = 1; +plantNumOfSimSteps = 10; +% plot on-the-fly the distance between two cars x1-x4 and the safe distance d_safe = D_default + t_gap * v_ego = 10 + 1.4 * x(5) +output_mat = [1 0 0 -1 0 0 0]; % plot on-the-fly the distance between two cars x1 - x4 +output_vec = []; +boundary_mat = [0 0 0 0 1.4 0 0]; % plot on-the-fly the safe distance +boundary_vec = [10]; +figure; +ncs.reachLive(init_set, ref_input, numSteps, method, numCores, output_mat, output_vec, 'blue', boundary_mat, boundary_vec, plantNumOfSimSteps); + +%% Live Reachability Analysis 2 + +numSteps = 10; +method = 'approx-star'; +%method = 'exact-star'; +numCores = 1; +% plot on-the-fly the distance between two cars x1-x4 and the safe distance d_safe = D_default + t_gap * v_ego = 10 + 1.4 * x(5) +output_mat = [1 0 0 -1 0 0 0; 0 0 0 0 1 0 0]; % plot on-the-fly the distance between two cars x1 - x4 versus ego car speed +output_vec = []; +boundary_mat = [0 0 0 0 1.4 0 0; 0 0 0 0 1 0 0]; % plot on-the-fly the safe distance vs. ego car speed +boundary_vec = [10;0]; +figure; +ncs.reachLive(init_set, ref_input, numSteps, method, numCores, output_mat, output_vec, 'blue', boundary_mat, boundary_vec); + +%% END \ No newline at end of file diff --git a/code/nnv/tests/nncs/LinearNNCS/test_LinearNNCS_reach_exact.m b/code/nnv/tests/nncs/LinearNNCS/test_LinearNNCS_reach_exact.m new file mode 100644 index 0000000000..99124d834d --- /dev/null +++ b/code/nnv/tests/nncs/LinearNNCS/test_LinearNNCS_reach_exact.m @@ -0,0 +1,125 @@ +% Reachability analysis for Linear ACC model +% Dung Tran: 10/4/2019 + + + + +%% System model +% x1 = lead_car position +% x2 = lead_car velocity +% x3 = lead_car internal state + +% x4 = ego_car position +% x5 = ego_car velocity +% x6 = ego_car internal state + +% lead_car dynamics +%dx(1,1)=x(2); +%dx(2,1) = x(3); +%dx(3,1) = -2 * x(3) + 2 * a_lead - mu*x(2)^2; + +% ego car dynamics +%dx(4,1)= x(5); +%dx(5,1) = x(6); +%dx(6,1) = -2 * x(6) + 2 * a_ego - mu*x(5)^2; + + +% let x7 = -2*x(3) + 2 * a_lead -> x7(0) = -2*x(3)(0) + 2*alead +% -> dx7 = -2dx3 + + +A = [0 1 0 0 0 0 0; 0 0 1 0 0 0 0; 0 0 0 0 0 0 1; 0 0 0 0 1 0 0; 0 0 0 0 0 1 0; 0 0 0 0 0 -2 0; 0 0 -2 0 0 0 0]; +B = [0; 0; 0; 0; 0; 2; 0]; +C = [1 0 0 -1 0 0 0; 0 1 0 0 -1 0 0; 0 0 0 0 1 0 0]; % feedback relative distance, relative velocity, longitudinal velocity +D = [0; 0; 0]; + +plant = LinearODE(A, B, C, D); % continuous plant model + +% the neural network provides a_ego control input to the plant +% a_lead = -2 + + +%% Controller +load controller_3_20.mat; + +n = length(weights); +Layers = []; +for i=1:n - 1 + L = LayerS(weights{1, i}, bias{i, 1}, 'poslin'); + Layers = [Layers L]; +end +L = LayerS(weights{1, n}, bias{n, 1}, 'purelin'); +Layers = [Layers L]; +Controller = FFNNS(Layers); % feedforward neural network controller + + +%% NNCS + +ncs = LinearNNCS(Controller, plant); % a discrete linear neural network control system + +%% Initial Set of states and reference inputs + +% reference input for neural network controller +% t_gap = 1.4; v_set = 30; + +ref_input = [30; 1.4]; + +% initial condition of the plant + +% initial position of lead car x_lead +x_lead = [90 100]; +% initial condition of v_lead +v_lead = [32 35]; +% initial condition of x_internal_lead +internal_acc_lead = [0 0]; +% initial condition of x_ego +x_ego = [10 11]; +% initial condition of v_ego +v_ego = [30 30.2]; +% initial condition of x_internal_ego +internal_acc_ego = [0 0]; +% initial condition for new introduced variable +x7_0 = [-4 -4]; % x7 = -2*x(3) + 2 * a_lead -> x7(0) = -2*x(3)(0) + 2*alead = -2*0 + 2*-2 = -4 + + +x1 = x_lead; +lb = [x1(1); v_lead(1); internal_acc_lead(1); x_ego(1); v_ego(1); internal_acc_ego(1); x7_0(1)]; +ub = [x1(2); v_lead(2); internal_acc_lead(2); x_ego(2); v_ego(2); internal_acc_ego(2); x7_0(2)]; +init_set = Star(lb, ub); + + +%% Reachability Analysis + +numSteps = 4; +method = 'approx-star'; +%method = 'exact-star'; +numCores = 1; +plantNumOfSimSteps = 10; +[R, reachTime] = ncs.reach(init_set, ref_input, numSteps, method, numCores, plantNumOfSimSteps); + +%% Plot output reach sets: actual distance vs. safe distance + +% plot reachable set of the distance between two cars d = x1 - x4 +figure; +map_mat = [1 0 0 -1 0 0 0]; +map_vec = []; +ncs.plotOutputReachSets('blue', map_mat, map_vec); + +hold on; + +% plot safe distance between two cars: d_safe = D_default + t_gap * v_ego; +% D_default = 10; t_gap = 1.4 +% d_safe = 10 + 1.4 * x5; + +map_mat = [0 0 0 0 1.4 0 0]; +map_vec = [10]; + +ncs.plotOutputReachSets('red', map_mat, map_vec); +title('Actual Distance versus. Safe Distance'); + +%% plot 2d output sets +figure; +map_mat = [1 0 0 -1 0 0 0; 0 0 0 0 1 0 0]; +map_vec = []; +ncs.plotOutputReachSets('blue', map_mat, map_vec); +title('Actual Distance versus. Ego car speed'); \ No newline at end of file