% Parameters --------------------------------------------------------------
params.R0 = polyZonotope(interval([9.5; -4.5; 2.1; 1.5], [9.55; -4.45; 2.11; 1.51]));
% Reachability Settings ---------------------------------------------------
options.zonotopeOrder = 100;
% Settings for NN evaluation ----------------------------------------------
options.nn.poly_method = "singh";
% System Dynamics ---------------------------------------------------------
f = @(x, u) [x(4) * cos(x(3)); x(4) * sin(x(3)); u(2) - 20; u(1) - 20 + u(3)];
% load neural network controller
load('controllerUnicycle.mat');
nnLinearLayer(W{1}, b{1}), ...
nnLinearLayer(W{2}, b{2}), ...
% construct neural network controlled system
sys = neurNetContrSys(sys, nn, 0.2);
% Specification -----------------------------------------------------------
goalSet = interval(-[0.6; 0.2; 0.06; 0.3], [0.6; 0.2; 0.06; 0.3]);
spec = specification(goalSet, 'safeSet', interval(params.tFinal));
% Verification ------------------------------------------------------------
[res, R, simRes] = verify(sys, spec, params, options, false);
% Visualization -----------------------------------------------------------
plot(specification(goalSet, 'safeSet'), [1, 2], 'DisplayName', 'Goal set');
useCORAcolors("CORA:contDynamics")
plot(R, [1, 2], 'DisplayName', 'Reachable set');
plot(R(1).R0, [1, 2], 'DisplayName', 'Initial set');
plot(simRes,[1, 2], 'DisplayName', 'Simulations');
xlabel('x'); ylabel('y');