function [u0,Sig0,g,C,ss,Minv]=lds2plg( A,Q,H,R,x0,P0 )
% 
% Convert a linear dynamical system (LDS) to a
% Predictive Linear Gaussian model (PLG).
%
%   (see the paper 'Predictive Linear Gaussian Models of Stochastic
%   Dynamical Systems' by Rudary, Singh and Wingate)
%
% These are the parameters of the LDS:
%   A,Q (State model. x_t+1 ~ N( A*x_t, Q) )
%   H,R (Observation model. H is row, R is scalar. y_t+1 ~ N( H*x_t+1, R ))
%   x0  (Initial state)
%   P0  (Initial covariance)
%
% We need to compute the PLG parameters:
%   u0   (Initial predictions)
%   Sig0 (Initial covariance)
%   g    (Linear trend)
%   C    (Covariance of noise with prediction vector)
%   ss   (Variance of noise)
%

%
% Check parameters
%

n = length(x0);

if ( ~all(size(A)==[n,n]) )
  error( 'A must be nxn!' );
end;

if ( ~all(size(Q)==[n,n]) )
  error( 'Q must be nxn!' );
end;

if ( ~all(size(P0)==[n,n]) )
  error( 'P0 must be nxn!' );
end;

if ( ~all(size(R)==[1,1] ) )
  error( 'R must be a scalar!' );
end;

if ( ~all(size(H)==[1,n] ) )
  error( 'H is invalid!' );
end;


%
% Compute some of the supporting matrices
%

% compute S's
Ss = {};
S = zeros(n,n);
Ss{0+1} = S;
for i=1:n
  S = S + (A^(i-1)) * Q * (A^(i-1))';
  Ss{i+1} = S;
end;

% compute M
for i=1:n
  M(i,:) = H*A^(i-1);
end;
Minv = M^-1;

% compute psi
for i=1:n
  for j=1:n
    if j <= i
      psi(i,j) = H*A^(i-j+1)*Ss{j-1+1}*H';
    else
      psi(i,j) = H*A^(j-i-1)*Ss{i+1}*H';
    end;
  end;
end;
psi = psi';
psi_n = psi(:,n);
psi = [ zeros(n,1), psi(:,1:n-1) ];

%
% Compute actual parameters
%

% compute u0
for i=1:n
  u0(i,1) = H*A^(i-1)*x0;
end;

% compute Sig0
for i=1:n
  for j=i:n
    Sig0(i,j) = H*A^(j-1) *P0* ((H*A^(i-1))') + H*A^(j-i)*Ss{i-1+1}*(H*A^(i-j))';
  end;
end;
for i=1:n
  for j=1:i-1
    Sig0(i,j) = Sig0(j,i);
  end;
end;
Sig0 = Sig0 + R*eye(n);

% compute g
g = H * (A^n) * (Minv);
g = g';

% compute C
C = psi_n - psi*g - R*g;

% compute ss
ss = H*Ss{n+1}*H' + R - g'*psi_n - C'*g;

return;