Ricatti-like factor step for the computation of the dual gradient of an MPC optimisation problem

mpcCaller.m

clear
clc
n=4;
m=2;
N=10;

Q=10*speye(n,n);
R=10*speye(m,m);

S=[];

f=rand(n,1);
r=rand(m,1);
q=rand(n,1);

A=speye(n);
A(1,2)=-0.2;
A(n,n)=-2;
B=rand(n,m);

[~, Q_N] = dlqr(full(A), full(B), full(Q), full(R));


[K, M, L, d, s, CholRbar, P] = mpcRicFac(Q, R, S, Q_N, q, r, A, B, f, N);

x0 = randn(n, 1);
y = rand((n+m)*N+n, 1);
q_N = randn(n,1);

[val, grad] = mpcRicSolve(y, x0, K, M, L, d, s, CholRbar, A, B, f,Q_N, q_N, Q, R, S, q, r, N);

F=[speye(n);sparse(m,n)];
G=[sparse(n,m);speye(m)];
FN = rand(2*n,n);

FG = [F G];

T = blkdiag(kron(speye(N), FG), FN);
%%

fobj = mpcGenericCost(x0, Q, R, S, Q_N, q, q_N, r, A, B, f, N);
                    %(Q, R, S, Q_N, q, q_N, r, A, B, f, N)

                    
fstar = fobj.makefconj();   
[val, grad]=fstar(y)


xmin = -ones(n,1);
xmax = -xmin;
umin = -ones(m,1);
umax = ones(m,1);
lb = [repmat([xmin;umin],N,1);-inf(size(FN,1),1)];
ub = [repmat([xmax;umax],N,1);ones(size(FN,1),1)];
weights = [repmat([1e2*ones(n,1);inf*ones(m,1)],N,1);1e2*ones(n,1)];
g = distBox(lb,ub,weights);

mpcGenericCost.m

function obj = mpcGenericCost(x0, varargin)
    %
    % Only f conjugate is available.
    %
    if length(varargin) > 1
        % input args:
        % (Q, R, S, Q_N, q, q_N, r, A, B, f, N)
        obj.Q = varargin{1};        % Q
        obj.R = varargin{2};        % R
        obj.S = varargin{3};        % S
        obj.Q_N = varargin{4};      % Q_N
        obj.q = varargin{5};        % q
        obj.q_N = varargin{6};      % q_N
        obj.r = varargin{7};        % r
        obj.A = varargin{8};        % A
        obj.B = varargin{9};        % B
        obj.f = varargin{10};       % f
        obj.N = varargin{11};       % N
        [obj.K, obj.M, obj.L, obj.d, obj.s, obj.CholRbar, obj.P] = ...
        	mpcRicFac(obj.Q, obj.R, obj.S, obj.Q_N, obj.q, obj.r, obj.A, obj.B, obj.f, obj.N);
                     %(   Q,     R,     S,     Q_N,     q,     r,     A,     B,     f,     N)
        obj.makefconj = ...
        	@() make_mpcCost_fconj(x0, obj.K, obj.M, obj.L, obj.d, obj.s, obj.CholRbar, ...
        	  obj.A, obj.B, obj.f, obj.Q, obj.q, obj.Q_N, obj.q_N, obj.R, obj.S, obj.r, obj.N);
    else
        obj = varargin{1};
        obj.makefconj = ...
        	@() make_mpcCost_fconj(x0, obj.K, obj.M, obj.L, obj.d, obj.s, obj.CholRbar, ...
        	  obj.A, obj.B, obj.f, obj.Q, obj.q, obj.Q_N, obj.q_N, obj.R, obj.S, obj.r, obj.N);
    end
    obj.isConjQuadratic = 1;
end

function op = make_mpcCost_fconj(x0, K, M, L, d, s, CholRbar, A, B, f, Q, q, Q_N, q_N, R, S, r, N)
    op = @(y) mpcRicSolve(y, x0, K, M, L, d, s, ...
    CholRbar, A, B, f, Q_N, q_N,Q, R, S, q, r, N);
end

mpcRicFac.m

function [K, M, L, d, s, CholRbar, P] = mpcRicFac(Q, R, S, Q_f, q, r, A, B, f, N)

%Input arguments
%
% Q             : State weight matrix
% R             : Input weight matrix
% S             : State-input joint weight matrix 
% Q_f           : Terminal weight matrix
% q             : Linear weight on states (vector)
% r             : Linear weight on inputs (vector)
% A, B, f       : System dynamics x^+ = Ax + Bu + f
% N             : Prediction horizon
% 
%
%Output arguments:
%
% K, M, L, C, D, d, s : Matrices and vectors computed by the Factor step
% P                   : Matrices P
% CholRbar            : Cholesky decomposition of Rbar
% These outputs are 3D matrices
%
%See also:
%mpcRicSolve

n = size(A,1);  % # states
m = size(B,2);  % # inputs

is_S_empty = isempty(S);
is_r_empty = isempty(r);
is_f_empty = isempty(f);
is_q_empty = isempty(q);

if (is_r_empty && is_f_empty),
    d = [];
else
    d = zeros(m, 1, N);
end

if (is_r_empty && is_f_empty && is_q_empty),
    s = [];
else
    s = zeros(n, 1, N);
end

P    = zeros(n,n,N+1);
Rbar = zeros(m, m, N);
CholRbar = zeros(m, m, N);
K = zeros(m, n, N);
M = zeros(m, n, N);
L = zeros(n, n, N);
Sbar = zeros(m,n,N);


%
% Part I.
%
P(:,:,end) = Q_f;
for k=N : -1 : 1,
    Rbar(:,:,k) = R + B'*P(:,:,k+1)*B;              % Rbar_k = R + B' P_{k+1} B
    CholRbar(:,:,k) = chol(Rbar(:,:,k), 'lower');   % Find Cholesky of Rbar_k
    Sbar(:,:,k) = B'*P(:,:,k+1)*A;                  % Sbar_k = S + B' P_{k+1} A
    if (~is_S_empty),
        Sbar(:,:,k) = Sbar(:,:,k) + S;
    end
    % P_k = Q + A' P_{k+1} A - Sbar_k' Rbar_k \ Sbar_k
    P(:,:,k) = ...
        Q + A' * P(:,:,k+1) * A - ...
        Sbar(:,:,k)' * ( CholRbar(:,:,k)' \ ( CholRbar(:,:,k) \ Sbar(:,:,k) ) );
       
end


%
% Part II.
%
for k=1:N,
    K(:,:,k) = - ( CholRbar(:,:,k)' \ ( CholRbar(:,:,k) \ Sbar(:,:,k) ) );  % K_k = -Rbar_k \ Sbar_k
    M(:,:,k) = - ( CholRbar(:,:,k)' \ ( CholRbar(:,:,k) \ B' ) );           % M_k = -Rbar_k \ B'
    if (~isempty(d)),                                                       % d _k = -Rbar_k \ dt_k
        dtemp = zeros(m, 1);                                                % where:
        if ~is_r_empty,                                                     % dt_k = r + B' P_{k+1} f
            dtemp = r;
        end
        if ~is_f_empty,
            dtemp = dtemp + B'*P(:,:,k+1)*f;
        end
        d(:,:,k) = - ( CholRbar(:,:,k)' \ ( CholRbar(:,:,k) \ dtemp ) );
    end
    L(:,:,k) = (A + B * K(:,:,k))';             % L_k = (A + B K_k)' 
    if ~isempty(s),                             % s_k = K_k' r + L_k P_{k+1} f + q
        if ~is_r_empty, 
            s(:,:,k) = K(:,:,k)'*r;
        end
        if ~is_f_empty,
            s(:,:,k) = s(:,:,k) + L(:,:,k) * P(:,:,k+1) * f;
        end
        if ~is_q_empty,
            s(:,:,k) = s(:,:,k) + q;
        end
    end
end % end for


end

mpcRicSolve.m

function [val, grad, e] = mpcRicSolve(y, x0, K, M, L, d, s, ...
    CholRbar, A, B, f, Q_N, q_N,Q, R, S, q, r, N)


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

%% Intro
YN = y(end-n+1:end);
Y = reshape(y(1:end-n), n+m, N);

e = zeros(n, N + 1);

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

%% e_N = q_N - y_N
if ~isempty(q_N),
    e(:, end) = q_N;
end
e(:, end) = e(:, end) - YN;

%% Construct e(:,k)
for k=N:-1:2,
    e(:,k) = L(:,:,k)*e(:,k+1);             % e_k = L e_{k+1} + C_k y_k + s_k
    if ~isempty(s),
        e(:,k) = e(:,k) + s(:,:,k);
    end
    e(:,k) = e(1:n,k) - Y(1:n, k)- K(:,:,k)'*Y(n+1:n+m, k);
    if ~isempty(s),
        e(:,k) = e(:,k) + s(:,:,k);
    end
end

%% Initial conditions
xstar(:, 1) = x0;
grad = x0;
val = 0;
if isempty(S),
    S = sparse(m,n);
end
PP = [Q S'; S R];
for k = 1:N,
    ustar(:,k) = K(:,:,k)*xstar(:,k) ...
        + d(:,:,k) + M(:,:,k)*e(:,k+1) ...
        + ( CholRbar(:,:,k)' \ ( CholRbar(:,:,k) \ Y(n+1:n+m,k) ) );
    xstar(:,k+1) = A*xstar(:,k) + B*ustar(:,k) + f;
    val = val - 0.5*[xstar(:,k);ustar(:,k)]'*PP*[xstar(:,k);ustar(:,k)] - ...
        q'*xstar(:,k) - r'*ustar(:,k) + Y(:,k)'*[xstar(:,k);ustar(:,k)];
    grad = [grad;ustar(:,k);xstar(:,k+1)];
end
val = val - 0.5*xstar(:, N+1)'*Q_N*xstar(:, N+1) - q_N'*xstar(:, N+1) + YN'*xstar(:, N+1);