xinit('wf.1.ps')
//test wiener filter macro

//define system macro which generates the next 
//observation given the old state

   deff('[x1,y]=system(x0,f,g,h,q,r)',...
        'rand(''normal'');...
         q2=chol(q);...
         r2=chol(r);...
         u=q2''*rand(ones(x0));...
         v=r2''*rand(ones(x0));...
         x1=f*x0+g*u;...
         y=h*x1+v;')

//initialize state statistics (mean and er. variance)

   m0=[10 10]';
   p0=[100 0;0 100];

//create system

   f=[1.15 .1;0 .8];
   g=[1 0;0 1];
   h=[1 0;0 1];
   [hi,hj]=size(h);
   
//noise statistics

   q=[.01 0;0 .01];
   r=20*eye(2,2);

//initialize system process

   rand('seed',66),
   rand('normal'),
   p0c=chol(p0);
   x0=m0+p0c'*rand(ones(m0));
   y=h*x0+chol(r)'*rand(ones(1:hi))';
   yt=y;

//initialize plotted variables

   x=x0;

//loop

   ft=[f];
   gt=[g];
   ht=[h];
   qt=[q];
   rt=[r];
   
   n=10;
   for k=1:n,

//generate the state and observation at time k (i.e. xk and yk)

      [x1,y]=system(x0,f,g,h,q,r);
      x=[x x1];
      yt=[yt y];
      x0=x1;
      ft=[ft f];
      gt=[gt g];
      ht=[ht h];
      qt=[qt q];
      rt=[rt r];

//end loop

   end,

//get the wiener filter estimate

   [xs,ps,xf,pf]=wiener(yt,m0,p0,ft,gt,ht,qt,rt);

//plot result

   a=mini([x(1,:)-2*sqrt(ps(1,1:2:2*(n+1))),xf(1,:),xs(1,:)]);
   b=maxi([x(1,:)+2*sqrt(ps(1,1:2:2*(n+1))),xf(1,:),xs(1,:)]);
   c=mini([x(2,:)-2*sqrt(ps(2,2:2:2*(n+1))),xf(2,:),xs(2,:)]);
   d=maxi([x(2,:)+2*sqrt(ps(2,2:2:2*(n+1))),xf(2,:),xs(2,:)]);
   xmargin=maxi([abs(a),abs(b)]);
   ymargin=maxi([abs(c),abs(d)]);
   a=-.1*xmargin+a;
   b=.1*xmargin+b;
   c=-.1*ymargin+c;
   d=.1*ymargin+d;


//plot frame, real state (x), and estimates (xf, and xs)

   plot([a a b],[d c c]);
   plot2d(x(1,:)',x(2,:)',[-2],"000"),
   plot2d(xf(1,:)',xf(2,:)',[-2],"000"),
   plot2d(xs(1,:)',xs(2,:)',[-2],"000"),

//mark data points (* for real data, o for estimates)

   plot2d(xs(1,:)',xs(2,:)',[2],"000"),
   plot2d(xf(1,:)',xf(2,:)',[3],"000"),
   plot2d(x(1,:)',x(2,:)',[4],"000"),
   xend(),
