FIX: rewrite compute_minRPI.m and debugging

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