initial test - totally unfeasible calculation times

.gitignore

@@ -1,5 +1,5 @@
 *.asv
-tests**
+tests-old**
 results**
 *.mlx
 *.zip

control_act.m

@@ -18,21 +18,29 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
     tc = sim_data.tc;
 
     u_corr = zeros(2,1);
-    U_corr_history = zeros(2,1,sim_data.PREDICTION_HORIZON);
+    U_corr_history = zeros(2,1);
     q_act = q;
-    u_track_pred=zeros(2,1, pred_hor);
-    T_inv_pred=zeros(2,2, pred_hor);
-
     q_pred = [];
 
     if eq(pred_hor, 0)
         return
     end
+
+    U_corr_history = optimvar('ucorr', 2, pred_hor); %zeros(2,1,sim_data.PREDICTION_HORIZON);
     
-    if pred_hor > 1
-        % move the horizon over 1 step and add trailing zeroes
-        U_corr_history = cat(3, sim_data.U_corr_history(:,:, 2:end), zeros(2,1));
+    % prepare objective function. Sum of squared norms
+    obj = 0
+    for k = 1:pred_hor
+        % squared norm
+        obj = obj + ones(1, 2) * (U_corr_history(:, k).^2);
     end
+    prob = optimproblem('Objective', obj);
+    cons = []
+
+    %if pred_hor > 1
+    %    % move the horizon over 1 step and add trailing zeroes
+    %    U_corr_history = cat(3, sim_data.U_corr_history(:,:, 2:end), zeros(2,1));
+    %end
 
     %disp('start of simulation')
     % for each step in the prediction horizon, integrate the system to
@@ -44,7 +52,7 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
         % compute the inputs, based on the old state
 
         % u_corr is the prediction done at some time in the past, as found in U_corr_history
-        u_corr_ = U_corr_history(:, :, k);
+        u_corr_ = U_corr_history(:, k);
         % u_track can be computed from q
         t_ = t + tc * (k-1);
         u_track_ = utrack(t_, q_act, sim_data);
@@ -67,77 +75,34 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
         q_new = [x_new; y_new; theta_new];
 
         % save history
-        q_pred = [q_pred; q_new'];
-        u_track_pred(:,:,k) = u_track_;
-        T_inv_pred(:,:,k) = T_inv;
+        % this thing is not allowed with optimization variables, so build
+        % the problem while predicting the behaviour
+        %q_pred = [q_pred; q_new'];
+        %u_track_pred(:,:,k) = u_track_;
+        %T_inv_pred(:,:,k) = T_inv;
 
         % Prepare old state for next iteration
         q_act = q_new;
-    end
 
-    %{
-        Now setup the qp problem
-        It needs:
-        - Unknowns, u_corr at each timestep. Will be encoded as a vector of
-        vectors, in which every two elements is a u_j
-        i.e. (u_1; u_2; u_3; ...; u_C) = (v_1; w_1; v_2, w_2; v_3, w_3; ...
-        ; v_C, w_C)
-        It is essential that the vector stays a column, so that u'u is the
-        sum of the squared norms of each u_j
+        % since saving history is not possible, create box constraints
+        % while simulating
 
-        - Box constraints: a constraint for each timestep in the horizon.
-        Calculated using the predicted state and inputs. They need to be
-        put in matrix (Ax <= b) form
-    %}
+        s_ = SATURATION - ones(2,1)*PREDICTION_SATURATION_TOLERANCE;
+        d = T_inv*u_track_;
 
-    % box constrains
-    % A becomes sort of block-diagonal
-    % A will be at most PREDICTION_HORIZON * 2 * 2 (2: size of T_inv; 2:
-    % accounting for T_inv and -T_inv) by PREDICTION_HORIZON (number of
-    % vectors in u_corr times the number of elements [2] in each vector)
-    A_max_elems = pred_hor * 2 * 2;
-    A_deq = [];
-    b_deq = [];
+        c1 = T_inv * u_corr_ <= s_-d;
+        c2 = -T_inv * u_corr_ <= s_ + d;
 
-    s_ = SATURATION - ones(2,1)*PREDICTION_SATURATION_TOLERANCE;
-    for k=1:pred_hor
-        T_inv = T_inv_pred(:,:,k);
-        u_track = u_track_pred(:,:,k);
-
-        % [T_inv; -T_inv] is a 4x2 matrix
-        n_zeros_before = (k-1) * 4;
-        n_zeros_after = A_max_elems - n_zeros_before - 4;
-        zeros_before = zeros(n_zeros_before, 2);
-        zeros_after = zeros(n_zeros_after, 2);
-        column = [zeros_before; T_inv; -T_inv; zeros_after];
-        A_deq = [A_deq, column];
-
-        d = T_inv*u_track;
-        b_deq = [b_deq; s_ - d; s_ + d];
+        cons = [cons; c1; c2];
     end
     
-    %A_deq
-    %b_deq
-    % unknowns
-   
-    % squared norm of u_corr. H must be identity,
-    % PREDICTION_HORIZON*size(u_corr)
-    H = eye(pred_hor*2)*2;
-    % no linear terms
-    f = zeros(pred_hor*2, 1);
-
-    % solve qp problem
-    options = optimoptions('quadprog', 'Display', 'off');
-    U_corr = quadprog(H, f, A_deq, b_deq, [],[],[],[],[],options);
-
-    % reshape the vector of vectors to be an array, each element being
-    % u_corr_j as a 2x1 vector
-    % and add the prediction at t_k+C
-    U_corr_history = reshape(U_corr, [2,1,pred_hor]);
-    %sim_data.U_corr_history = U_corr_history;
-    % first result is what to do now
-    u_corr=U_corr_history(:,:, 1);
+    prob.Constraints.cons = cons;
+    x0.ucorr = zeros(2,1,pred_hor);
+    show(prob)
     
+    [sol,fval,exitflag,output] = solve(prob,x0)
+    U_corr_history=reshape(sol.ucorr, [2,1,pred_hor]);
+    u_corr=U_corr_history(:,:,1);
 end
 
 function u_track = utrack(t, q, sim_data)