lqgsamp.m 1.72 KB
Newer Older
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 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57
function [sysd,Q1e,Q2e,Q12e,R1,Qconst] = lqgsamp(sysc,h,Q1c,Q2c,Q12c,R1,tau)
%
% [sysd,Q1,Q2,Q12,R1,Qconst] = lqgsamp(sysc,h,Q1c,Q2c,Q12c,R1c)
% [sysd,Q1,Q2,Q12,R1,Qconst] = lqgsamp(sysc,h,Q1c,Q2c,Q12c,R1c,tau)
%
% Sample a continous time LQ design

if nargin < 7
  tau = 0;
end

[A,B,C] = ssdata(sysc);

totsize = size(A,2)+size(B,2);
a1 = size(A,1);  % number of states
b2 = size(B,2);  % number of inputs

inttau = max(0,ceil(tau/h-1)); % whole samples extra delay
fractau = tau - inttau*h;      % fractional delay

% Sample the plant, the cost, and the noise
Q = [Q1c Q12c; Q12c' Q2c];
[Phi2,r,Q2] = calcc2d([A B;zeros(b2,a1+b2)],blkdiag(R1,zeros(b2)),Q,fractau);
Gamma2 = Phi2(1:a1,a1+1:a1+b2);
[X,r,Q1] = calcc2d([A B;zeros(b2,a1+b2)],blkdiag(R1,zeros(b2)),Q,h-fractau);
Gamma2 = X(1:a1,1:a1)*Gamma2;
Gamma1 = X(1:a1,a1+1:a1+b2);
[Phi,R1,Q,Qconst] = calcc2d([A B;zeros(b2,a1+b2)],blkdiag(R1,zeros(b2)),Q,h);
R1 = R1(1:a1,1:a1);
Phi = Phi(1:a1,1:a1);

% Build extended state-space model
Phie = [Phi Gamma2; zeros(b2,a1+b2)];
Gammae = [Gamma1; eye(b2)];
Ce = [C zeros(size(C,1),b2)];
Ge = [eye(a1);zeros(b2,a1)];
Q1e = Q2+Phi2(1:a1,:)'*Q1(1:a1,1:a1)*Phi2(1:a1,:);
Q2e = Q1(a1+1:end,a1+1:end);
Q12e = [Phi2(1:a1,:)'*Q1(1:a1,a1+1:end)];

% Add additional integer delays (if tau > h)
if inttau > 0
  Phie = blkdiag([Phie Gammae],eye((inttau-1)*b2));
  Gammae = zeros(size(Phie,1),b2);
  Phie = [Phie; zeros(b2,size(Phie,2))];
  Gammae = [Gammae; eye(b2)];
  Ce = [Ce zeros(size(Ce,1),inttau*b2)];
  Ge = [Ge; zeros(inttau*b2,size(Ge,2))];
  Q1e = [Q1e Q12e; Q12e' Q2e];
  Q1e = blkdiag(Q1e,zeros((inttau-1)*b2));
  Q2e = zeros(size(Q2e));
  Q12e = zeros(size(Q1e,1),size(Q2e,2));
end

R1 = blkdiag(R1,zeros(size(Phie)-size(A)));

sysd = ss(Phie,Gammae,Ce,0,h);