Solve step for the computation of the dual gradient

mpcRicSolve.m

function [val, grad, e] = mpcRicSolve(y, x0, K, M, L, C, D, d, s, CholRbar, A, B, f, F_N, q_N)

grad = 0;

% sizes
n = size(A,1);
m = size(B,2);
nc_f = size(F_N, 1);


e = zeros(n, N);

if ~isempty(q_N),
    e(:, end) = q_N;
end

if ~isempty(F_N),
    e(:, end) = e(:, end) + F_N' * y(end, end-nc_f+1);
end

xstar=zeros(n, N+1);
ustar=zeros(m, N);

for k=N-1:-1:1,
    e(:,k) = L(:,:,k)*e(:,k+1) + C(:,:,k)*y;    % e_k = L e_{k+1} + C_k y_k + s_k
    if ~isempty(s),
        e(:,k) = e(:,k) + s(:,:,k);
    end    
end

xstar(:,1) = x0;

for k=1:N,
    ustar(:,k) = K(:,:,k)*xstar(:,:,k) + D(:,:,k)*y(:, k) + M(:,:,k)*e(:,:,k+1) + d(:,:,k);
    xstar(:,:,k+1) = A*xstar(:,:,k) + B*ustar(:,:,k) + f(:,:,k);
end

val = [vec(xstar)
    vec(ustar)];