function [C,AA,K]=hsQRW(x,p);
d=p(1); h1=p(2); h2=p(3); theta=p(4); aa=p(5);
rho0=p(7); rho1=p(8); rho2=p(9); delta=p(10);
lambda=p(6); sigma=p(11); beta=p(12);
varm=p(13:17);
A=[1     0    0   0      0;
   rho0 rho1 rho2 0      0;
   0     1    0   0      0;
   0     0    0 (1-delta)/lambda  0;
   0     0    0   0      0];
B=[0 0 0;0 0 0;0 0 0;0 0 1/lambda;1 0 0];
D=[0 1 0 0 0;
   0 0 0 1 0;
   0 0 0 0 1;
   0 0 0 0 0;
   0 0 0 0 0;
   0 0 0 0 0];
E=[0 0 0;
   0 0 0;
   0 0 0;
   1 0 0;
   0 1 0;
   0 0 1];

ct=1; ns=5; nc=3;
[ux,b,q]=hsu4(x,p(1:5));
b=b(:);

% construct Q,R,W
Q0=zeros(ns);
Q0(ct,ct)=ux-b'*x+x'*q*x/2;
d=(b'-x'*q)';
Q1=zeros(ns);
Q1(ct,:)=d'*D;
Q1(:,ct)=(d'*D)';
Q1=Q1/2;
Q1(ct,ct)=2*Q1(ct,ct);
Q=Q0+Q1+D'*q*D/2;
R=E'*q*E/2;
Q2=zeros(ns,nc);
Q2(ct,:)=(d'*E)/2;
W=Q2+D'*q*E/2;

Qn=(Q-W*(R\W'));
An=sqrt(beta)*(A-B*(R\W'));
Bn=sqrt(beta)*B;
Fn=double(An',Bn',Qn,R)';
F=Fn+R\W';
Fp=F';
Ao=A-B*F;
[tem1,tem2]=hsf4(x,p(1:4));
C=[0,0,0,1,0;
   -F;
   tem2'*(D-E*F)+[tem1-tem2'*x,zeros(1,ns-1)]];
Sigma=zeros(5); Sigma(2,2)=sigma^2;
Omega=diag(varm);
[K,S]=kfilter(Ao,C,Sigma,Omega);
AA=Ao-K*C;
Sigmaz=Omega+C*S*C';
Sigmazi=inv(Sigmaz);

load /home/erm/data/hsdata3
T=length(zxvar);
tem=lambda.^([1:T]');
Y=[zxvar(:,1)./tem,zxvar(:,2),zxvar(:,3),zxvar(:,5)./tem,zxvar(:,4)./tem];
X0=[1 x(1) x(1) x(2) x(3)]';

t0=3;
Xt(t0,:)=X0';
innov(t0,:)=Y(t0,:)-X0'*C';
sum1=innov(t0,:)'*innov(t0,:);
sum2=Xt(t0,:)'*innov(t0,:);
for i=t0+1:T;
  Xt(i,:)=Xt(i-1,:)*Ao'+innov(i-1,:)*K';
  innov(i,:)=Y(i,:)-Xt(i,:)*C';
  sum1=sum1+innov(i,:)'*innov(i,:);
  sum2=sum2+Xt(i,:)'*innov(i,:);
end;  
sum1=sum1/(T-t0+1);

L=(T-t0+1)*(log(det(Sigmaz))+trace(Sigmazi*sum1));
