my_dlqr.m 751 Bytes
Newer Older
Martin Vítek's avatar
Martin Vítek committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39
function [K,V,LL]  = my_dlqr(A, B, Q, R, N, h, sqV0)
% function computes LQ controller for tracking of the reference state z

nargin = 0;
dx=size(A,1);
du=size(B,2); 
if nargin<7
    sqV=1e-5*eye(dx+dx);
else
    sqV = sqV0;
end
if nargin<6
    h = 1000;
end
sqQ = chol(Q);
sqR = chol(R);

LL = zeros((dx+dx)*du,h);

for i=h:-1:1
M=[sqR, zeros(du,dx), zeros(du,dx);...
    zeros(dx,du), sqQ, -sqQ;...
    sqV*[B A zeros(dx,dx); zeros(dx,du+dx), eye(dx) ]];
     
%    Y = qr(M,0); broken qr?
    Y = chol(M'*M);
    Yuu = Y(1:du,1:du);
    Yxu = Y(1:du,du+1:end);
    Yxx = Y(du+1:du+2*dx, du+1:du+2*dx);
    
    L=-inv(Yuu)*Yxu;
    sqV = Yxx;
    LL(:,i) = L(:);
end

Vf = sqV'*sqV;

K = -L;
V = Vf(1:dx,1:dx);