summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
-rw-r--r--templates/compute_minRPI.m47
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