diff options
| -rw-r--r-- | templates/compute_minRPI.m | 47 |
1 files changed, 30 insertions, 17 deletions
diff --git a/templates/compute_minRPI.m b/templates/compute_minRPI.m index 2ae7946..158cbad 100644 --- a/templates/compute_minRPI.m +++ b/templates/compute_minRPI.m @@ -8,27 +8,40 @@ function [H_tube,h_tube,n_iter] = compute_minRPI(K_tube,params) % YOUR CODE HERE - A = params.model.A+params.model.B*K_tube; - nx = params.model.nx; - omega = Polyhedron('A',[eye(nx);-eye(nx)],'b',zeros(1,2*nx)'); - % display(omega.A) - % display(omega.b) + fprintf("\n") + + % Compute closed loop, disturbance set and disturbance reachable sets + Ak = params.model.A + params.model.B*K_tube; + W = Polyhedron('A', params.constraints.DisturbanceMatrix, ... + 'b', params.constraints.DisturbanceRHS); + Fi = @(i) Ak^i * W; + + fprintf("Stable error dynamics? %d\n", all(eig(Ak) < 1)) + fprintf("Decreasing DRS? %d\n", Fi(3).volume() > Fi(4).volume()); + + % Start algorithm + omega = Polyhedron.emptySet(params.model.nx); n_iter = 0; - Hw = params.constraints.DisturbanceMatrix; - hw = params.constraints.DisturbanceRHS; - while true - omega_last = copy(omega); - % display(n_iter) - % A_last = omega.A; - % b_last = omega.b; - omega = omega.plus(Polyhedron('A',Hw*(A^n_iter),'b',hw)); - - if eq(omega.minHRep(),omega_last.minHRep()) - % if eq(omega,omega_last) - break + while n_iter < 200 + Fn_iter = Fi(n_iter); + omega_next = omega + Fn_iter; + fprintf("n_iter=%d: volume=%d, fulldim=%d, bounded=%d\n", ... + n_iter, Fn_iter.volume(), omega_next.isFullDim(), omega_next.isBounded()) + + % why does this happen? + if ~ omega_next.isFullDim() || ~ omega_next.isBounded() + fprintf("There is something off ...\n"); + break; + end + + if eq(omega_next.minHRep(), omega.minHRep()) + fprintf("converged!\n"); + break; end n_iter = n_iter+1; + omega = omega_next; end + H_tube = omega.A; h_tube = omega.b; end
\ No newline at end of file |