MRPI by Schulze

[klix0]

Hello Rainer,

I have seen that you have worked a lot with the calculation of minimal RPI sets and I want to ask you if you can help me with a calculation of MRPI according to Schulze. I have written a code in Julia, but I need in Matlab. Can you offer me your help?

Best, Klinto

[worc4021]

Hello, I believe we used the MPT functions to compute them. But let me know if you need additional help.

Manuel

[klix0]

Hello,

yes I use MPT3 too. I have got the right result in Julia, but I have some errors in Matlab. Have you calculated it before? Can you have a quick look at my code if you have time? If you have the code in Matlab, it will be very helpful for me.

Best regards Klinto

[image: image.png]

On Sun 12. Jun 2022 at 11:27, Rainer Manuel Schaich < @.***> wrote:

Hello, I believe we used the MPT https://www.mpt3.org/ functions to compute them. But let me know if you need additional help.

Manuel

— Reply to this email directly, view it on GitHub https://github.com/worc4021/CDC2016/issues/1#issuecomment-1153112736, or unsubscribe https://github.com/notifications/unsubscribe-auth/AYPBACV6BLIJ2JFZCG2PYWTVOWUO5ANCNFSM5YROBOVA . You are receiving this because you authored the thread.Message ID: @.***>

using LazySets using LinearAlgebra using ControlSystems using Plots using InvariantSets using MathematicalSystems using Polyhedra % add https://github.com/ueliwechsler/InvariantSets.jl

% NOTE: is defined in the package InvariantSets.ρ_matrix see (https://github.com/ueliwechsler/InvariantSets.jl/blob/680cb87bd61cb26f3c1cfe97eaf27009d38ac016/src/helper/lazy_sets.jl#L65) % otherwise the code for the logic should be self contained.

% Functions needed function explicit_maximal_RPI_set(Hd, bd, 𝒲, S, k) Hr0 = Hd br0 = bd Hrk = Array{Array}(undef, k+1) brk = Array{Vector}(undef, k+1) supk = Array{Vector}(undef, k+1) supk[1] = zeros(length(br0)) Hrk[1] = Hr0 brk[1] = br0 for i=2:k+1 supk[i] = supk[i-1] + InvariantSets.ρ_matrix(HdS^(i-2), 𝒲) Hrk[i] = [Hrk[i-1]; HdS^k] brk[i] = [brk[i-1]; bd - supk[i]] end return Hrk[end], brk[end] end

""" 𝒟 = (𝕏,𝕌) = {x∈ℝⁿ | Hdx≤bd}""" function s_min_sufficient( S, 𝒲, Hd, bd; ε=0.1, verbose=false, k_max=1000 ) Hw, bw = tosimplehrep(𝒲) k = 0 while true k += 1 cond11 = (1 + ε)InvariantSets.ρ_matrix(HwS^k, 𝒲) .<= εbw bc = (1 + ε)sum(InvariantSets.ρ_matrix(HdS^j, 𝒲) for j in 0:k-1) cond12 = bc .<= bd (verbose) && @show k, cond11, cond12 if all(cond11) && all(cond12) return k, bc end k == k_max && error("Convergence not achieved for s = $k_max\n" "Check if the origin ∈ interior(𝒲)") end end

function minimal_RPI_set_schulze(S, 𝒲, 𝕏::LazySet, 𝕌::LazySet, K; ε=0.1, k_max=1000, remove_redundancy=true, avoid_equality_check=false, verbose=true ) Hx, bx = tosimplehrep(𝕏) Hu, bu = tosimplehrep(𝕌) Hd = [Hx; Hu*K] bd = [bx; bu] @time s, bc = s_min_sufficient(S, 𝒲, Hd, bd; ε=ε, k_max=k_max) @info "Darup-Schulze: maximal s=$s" Hc = Hd 𝒞 = HPolytope(Hc, bc) % maximal_RPI_set does converge in at most s-1 steps @time Hrk, brk = explicit_maximal_RPI_set(Hc, bc, 𝒲, S, s) 𝒫∞ = HPolytope(Hrk, brk) @show s return 𝒫∞, 𝒞, s end

%% Example 1 A = [1.0 1.0; 0.0 1.0] B = [0.5; 1.0] n = size(A,1) m = size(B,2) Q = I R = I K = dlqr(A,B,Q,R) S = A - B*K

Hx = [Matrix{Float64}(I,n,n); -Matrix{Float64}(I,n,n)] bx = [4.0; 2.0; 8.0; 4.0] 𝒳 = HPolytope(Hx, bx) Hu = [Matrix{Float64}(I,m,m); -Matrix{Float64}(I,m,m)] bu = [1.0; 1.0] 𝒰 = HPolytope(Hu, bu) #or Hyperrectangle? 𝒲 = BallInf(zeros(n), 0.1)

ε1 = 0.001 ε_mrpi1 = 0.1 Pinfty1, 𝒞1, k1 = minimal_RPI_set_schulze(S, 𝒲, 𝒳, 𝒰, K; ε=ε1);

plot(𝒳) plot!(𝒞1, color=:gray,alpha=1) plot!(Pinfty1, color=:yellow,alpha=1)

% ========================================================= % Example 2: % =========================================================

% make your own discrete linear system (plant) A = [1 1; 0 1] B = hcat([1; 1]) sys = @.** x⁺ = Ax + B*u

% constraints on state Xc and input Uc Xc_vertex = [2.0 -2.0; 2.0 2.0; -10.0 2.0; -10.0 -2.0] % Uc_vertex = hcat([1.0; -1.0]) % hcat is needed to create a matrix, since VPolytope needs a matrix as input % Xc = VPolytope(Xc_vertex') Xc = BallInf(zeros(2), 3.0) Uc = BallInf(zeros(1), 3.0) % Uc = VPolytope(Uc_vertex)

% construct a convex set of system noise (2dim here) % W_vertex = [0.25 0.25; 0.25 -0.25; -0.25 -0.25; -0.25 0.25] % W = VPolytope(W_vertex)

% set boundary of system noise which corresponds to W. w_max = 0.25 W = BallInf(zeros(2), 0.25) plot(W)

Q = diagm([1.0, 1.0]) R = 0.1 K = -ControlSystems.dlqr(A,B,Q,R)

% closed loop system Ak = A + B*K

ε1 = 0.001 Pinfty1, 𝒞1, k1 = minimal_RPI_set_schulze(Ak, W, Xc, Uc, -K; ε=ε1) # note the minus for -K

plot(Xc) plot!(𝒞1, color=:gray,alpha=1) plot!(Pinfty1, color=:yellow,alpha=1)

[klix0]

Hello Manuel,

thank you for your quick response. I have written you an email.

[worc4021]

Hello Klinto,

what you put is your Julia implementation. I cannot see what type of issues you're getting when calculating the sets in Matlab. When we worked on this back then we had been using exclusively matlab.

Cheers, Manuel

[klix0]

Hello Manuel,

yes I know. Do you have still the code for this calculation? I need it exclusively in Matlab with MPT too, but I have not a lot of experience in Matlab. Can you share the code with me, if it is possible for you?

Best Klinto

On Sun 12. Jun 2022 at 12:18, Rainer Manuel Schaich < @.***> wrote:

Hello Klinto,

what you put is your Julia implementation. I cannot see what type of issues you're getting when calculating the sets in Matlab. When we worked on this back then we had been using exclusively matlab.

Cheers, Manuel

— Reply to this email directly, view it on GitHub https://github.com/worc4021/CDC2016/issues/1#issuecomment-1153122075, or unsubscribe https://github.com/notifications/unsubscribe-auth/AYPBACRRFB3C5MMDBCLAQUTVOW2Q5ANCNFSM5YROBOVA . You are receiving this because you authored the thread.Message ID: @.***>

[worc4021]

Hi Klinto,

I cannot find the code anymore. But from memory I believe that it was rather simple to do the implementation with the MPT. What type of issue do you run into when you try implementing the computation in Matlab?

Cheers, Manuel

[klix0]

Hi Manuel,

I have implement the calculation of mRPI according to Rakovic before and it works properly with MPT. By implementation of Schulze I have many errors with inputs and syntax. I am going to share the Rakovic Implementation and a short function I am using to calculate the new implementation. If you can do something to fix my problem, please let me know (of course against a payment for your work and time).

On Sun, Jun 12, 2022 at 12:52 PM Rainer Manuel Schaich < @.***> wrote:

Hi Klinto,

I cannot find the code anymore. But from memory I believe that it was rather simple to do the implementation with the MPT. What type of issue do you run into when you try implementing the computation in Matlab?

Cheers, Manuel

— Reply to this email directly, view it on GitHub https://github.com/worc4021/CDC2016/issues/1#issuecomment-1153127523, or unsubscribe https://github.com/notifications/unsubscribe-auth/AYPBACUNTMI7BZI5GPBBNRDVOW6PRANCNFSM5YROBOVA . You are receiving this because you authored the thread.Message ID: @.***>

% example_mRPI.m

close all

%% make your own discrete linear system (plant) A = [1 1; 0 1]; B = [1; 1]; C = [0.5 0.2]; D= [0]; f=[0;0]; g=[0];

myopnlopsys = LTISystem('A', A, 'B', B, 'C', C, 'D', D, 'f', f, 'g', g);% discrete-time state-space model

% constraints on state Xc and input Uc Xc_vertex = [2, -2; 2 2; -10 2; -10 -2]; Uc_vertex = [1; -1]; Xc = Polyhedron(Xc_vertex); Uc = Polyhedron(Uc_vertex);

% construct a convex set of system noise (2dim here) W_vertex = [0.25, 0.25; 0.25, -0.25; -0.25, -0.25; -0.25, 0.25]; %low nois %W_vertex = [0.15, 0.15; 0.15, -0.15; -0.15, -0.15; -0.15, 0.15]; W = Polyhedron(W_vertex); % set boundary of system noise which corresponds to W. w_max = [0.25; 0.25]; w_min = [-0.25; -0.25];

figure(1) plot(W) title("noise of system");

%% make optimal controller Q = diag([1, 1]); R = 0.1; K = -dlqr(myopnlopsys.A, myopnlopsys.B, Q, R);

%% closed loop system

Ak= A+B*K; closedsys = LTISystem('A', Ak, 'B', B, 'C', C, 'D', D, 'f', f, 'g', g);% discrete-time state-space model

Q = diag([1, 1]); R = 0.1; [K_tmp, P] = dlqr(myopnlopsys.A, myopnlopsys.B, Q, R); K = -K_tmp; Ak = (myopnlopsys.A + myopnlopsys.B * K);

% compute minimal disturbance(robust) invariant set Z. Z_approx = approx_minRPIset(Ak, W, 3, 1.45); figure(20) title(" minimal disturbance invariant set for controller"); plot(Z_approx)

%The command lqr can be adapted to calculate an optimal observer gain in a dual way: L = lqr(A',C',Q,R)' Q = diag([1, 1]); R = 0.1; L = dlqr(myopnlopsys.A', myopnlopsys.C', Q, R)'; ob = LTIObserver(myopnlopsys,L);

% compute observer invariant set (Zob) Zob=ob.ApproxmRPIset(W,2,1.001);

%% find minimal Robust Positively Invariant (mRPI) Set for closed loop system

Zsys = Z_approx + Zob; figure(3) hold on Graphics.show_convex(Zsys, 'g', 'FaceAlpha', .4); % show Z Graphics.show_convex(Z_approx, 'r', 'FaceAlpha', .8); % show Z Graphics.show_convex(Zob, 'b', 'FaceAlpha', .3); % show Z

hold off legend(" Z system", "Z controller" ,"Z observer" )

%approximation of Z controller

myclosedloopsys.A=Ak; myclosedloopsys.f=[0;0]; myclosedloopsys.nx=size(Ak,1); alpha=.2; [Z_inner,Z_outer] = minrpiset(myclosedloopsys,W,alpha); figure(2) hold on Graphics.show_convex(Z_outer,'m' , 'FaceAlpha', .4); % show Z Graphics.show_convex(Z_inner,'r' , 'FaceAlpha', .7); % show Z hold off legend("Z outer approx.", "Z inner approx." ) title("approximation of minimal disturbance invariant set for controller"); %% simulation

% propagate particles many times following the discrete stochastic dynamics, % and we will see that particles never go outside of distervance invariant set Z. Nptcl = 100; x = zeros(2, Nptcl); % particles y = myopnlopsys.Cx; u=0; t=0; while true %% observer estimation Xhat= ob.EstimateCurrentState( y , u); Yhat = myopnlopsys.CXhat;

%% optimal control state feed back based on observer estimation
u = K * Xhat;

%% system response
sys_noise = rand(2, Nptcl).*repmat(w_max - w_min, 1, Nptcl) + repmat(w_min, 1, Nptcl);
x = myopnlopsys.A*x + myopnlopsys.B*u +sys_noise;
y = myopnlopsys.C*x;
%% show result
titl = sprintf('Running... Time: %f (press any key to exit)',t );

% Note that system state is placed inside the Z of controller.
h=figure(4);
clf;
Graphics.show_convex(Zsys,'g' , 'FaceAlpha', .3); % show Z
Graphics.show_convex(Z_approx, 'r', 'FaceAlpha', .7); % show Z    
scatter(x(1, :), x(2, :)); % show particles
legend("Z system(including observer)", "Z controller ")
title(titl); 
drawnow
isKeyPressed = ~isempty(get(h,'CurrentCharacter'));
if isKeyPressed
   break
end    
pause(0.01);
t=t+1;

end

% Functions needed function Z_max = explicit_maximal_RPI_set(Hd, bd, W, S, k) Hd = Hr0 ; bd = br0; Hrk = [undef, k+1] brk = [undef, k+1] supk = [undef, k+1] supk(1) = zeros(length(br0)) Hrk(1) = Hr0 brk(1) = br0

for i=2:k+1
     supk(i) = supk(i-1) + InvariantSets.p_matrix(Hd*S^(i-2), W)
     Hrk(i) = [Hrk(i-1);
               Hd*S^k]
     brk(i) = [brk(i-1);
               bd - supk(i)]
end

% Functions needed function Z_max = Schulze2(Hd, bd, W, S, k) Hr0 = Hd; br0 = bd; Hrk = [undef, k+1]; brk = [undef, k+1]; supk = [undef, k+1]; supk(1) = zeros(length(br0)); Hrk(1) = Hr0; brk(1) = br0;

for i=2:k+1
     supk(i) = supk(i-1) + InvariantSets.p_matrix(Hd*S^(i-2), W);
     Hrk(i) = [Hrk(i-1);
               Hd*S^k];
     brk(i) = [brk(i-1);
               bd - supk(i)];
end

% """ 𝒟 = (𝕏,𝕌) = {x∈ℝⁿ | Hdx≤bd}""" function S_min = s_min_sufficient(S, W, Hd, bd) e=0.1; verbose=false; k_max=1000;

Hw, bw = tosimplehrep(W);
k = 0;
while true
    k = 1;
    cond11 = (1 + e)*InvariantSets.p_matrix(Hw*S^k, W) <= e*bw;
    bc = (1 + e)*sum(InvariantSets.p_matrix(Hd*S^1i, W));
    cond12 = bc <= bd;

    if all(cond11) && all(cond12)

    end

end
end

function Y = minimal_RPI_set_schulze(S, W, X, U, K) e = 0.1; k_max=1000; remove_redundancy=true; avoid_equality_check=false; verbose=true;

Hx, bx = tosimplehrep(X);
Hu, bu = tosimplehrep(U);
Hd = [Hx; Hu*K];
bd = [bx; bu];
bc = s_min_sufficient(S, W, Hd, bd);

Hc = Hd;
C = HPolytope(Hc, bc);
% maximal_RPI_set does converge in at most `s-1` steps
brk = explicit_maximal_RPI_set(Hc, bc, W, S, s);
P_inf = HPolytope(Hrk, brk);

end

% ========================================================= % Example: % =========================================================

% make your own discrete linear system (plant)A = [1 1; 0 1]; A = [1 1; 0 1]; B = [1; 1];

% constraints on state Xc and input Uc Xc_vertex = [2, -2; 2 2; -10 2; -10 -2]; Uc_vertex = [1; -1]; Xc = Polyhedron(Xc_vertex); Uc = Polyhedron(Uc_vertex);

% construct a convex set of system noise (2dim here) W_vertex = [0.25, 0.25; 0.25, -0.25; -0.25, -0.25; -0.25, 0.25]; %low nois %W_vertex = [0.15, 0.15; 0.15, -0.15; -0.15, -0.15; -0.15, 0.15]; W = Polyhedron(W_vertex); % set boundary of system noise which corresponds to W. w_max = [0.25; 0.25]; w_min = [-0.25; -0.25]; plot(W)

Q = diag([1, 1]); R = 0.1; K = -dlqr(myopnlopsys.A, myopnlopsys.B, Q, R);

% closed loop system Ak= A+B*K;

e1 = 0.001; Z_approx = minimal_RPI_set_schulze(Ak, W, Xc, Uc, -K); % note the minus for -K plot(Z_approx)

plot(Xc) plot(C1) plot(Pinfty1)

end

[klix0]

Hi Manuel,

sorry for the inconvenience today! Just one last question. The code Simulations.m that you have upload in Github is the example of Schulze mRPI? Thank you for your help!

Best Klinto

On Sun, Jun 12, 2022 at 1:02 PM Klinto Xhemo @.***> wrote:

Hi Manuel,

I have implement the calculation of mRPI according to Rakovic before and it works properly with MPT. By implementation of Schulze I have many errors with inputs and syntax. I am going to share the Rakovic Implementation and a short function I am using to calculate the new implementation. If you can do something to fix my problem, please let me know (of course against a payment for your work and time).

On Sun, Jun 12, 2022 at 12:52 PM Rainer Manuel Schaich < @.***> wrote:

Hi Klinto,

I cannot find the code anymore. But from memory I believe that it was rather simple to do the implementation with the MPT. What type of issue do you run into when you try implementing the computation in Matlab?

Cheers, Manuel

— Reply to this email directly, view it on GitHub https://github.com/worc4021/CDC2016/issues/1#issuecomment-1153127523, or unsubscribe https://github.com/notifications/unsubscribe-auth/AYPBACUNTMI7BZI5GPBBNRDVOW6PRANCNFSM5YROBOVA . You are receiving this because you authored the thread.Message ID: @.***>

[worc4021]

Hello,

the code Simulations.m is indeed code to produce the sets. Unfortunately I no longer have access to a Gurobi license so I cannot run the code anymore. I also see that it's using my interface to the LRS for the vertex/facet enumeration. You can simply replace it with any other code that computes the H and V representation of polyhedra. It's probably easier than trying to make the LRS work on your system.

Hope this helps, Manuel

[klix0]

Hello Manuel,

thank you for your help! The code is unfortunately not working yet. Can you maybe help me to find a way how to calculate the mRpi according to Schulze and Trodden?

Best Klinto

[worc4021]

Hi Klinto,

I'll have a look on the weekend.

Manuel

[klix0]

Hi Manuel,

thank you very much! I appreciate your help.

Best Klinto

On Thu 16. Jun 2022 at 22:26, Rainer Manuel Schaich < @.***> wrote:

Hi Klinto,

I'll have a look on the weekend.

Manuel

— Reply to this email directly, view it on GitHub https://github.com/worc4021/CDC2016/issues/1#issuecomment-1158098648, or unsubscribe https://github.com/notifications/unsubscribe-auth/AYPBACWWDTKVF4CL2X4TMSDVPOEXPANCNFSM5YROBOVA . You are receiving this because you authored the thread.Message ID: @.***>

[klix0]

Hi Manuel,

have you had free time to have a look to the the code?

Have a nice Sunday!

Best Klinto

On Thu 16. Jun 2022 at 22:39, Klinto Xhemo @.***> wrote:

Hi Manuel,

thank you very much! I appreciate your help.

Best Klinto

On Thu 16. Jun 2022 at 22:26, Rainer Manuel Schaich < @.***> wrote:

Hi Klinto,

I'll have a look on the weekend.

Manuel

— Reply to this email directly, view it on GitHub https://github.com/worc4021/CDC2016/issues/1#issuecomment-1158098648, or unsubscribe https://github.com/notifications/unsubscribe-auth/AYPBACWWDTKVF4CL2X4TMSDVPOEXPANCNFSM5YROBOVA . You are receiving this because you authored the thread.Message ID: @.***>

[klix0]

Hi Manuel,

Sorry for the inconvenience! I just want to ask if you find the code or not?

Thank you!