% 15.8.02 Unscented transformation. 
% Needs not to be modified to adapt to other models

function [xhat,Pxx,K]=ex5ut(xhat,Pxx,y,fct,obsfct,dq,dx,dy,R);

  N=2*dx;

  xsigma=chol( dx*Pxx )'; % Pxx=root*root', but Pxx=chol'*chol
  Xa=xhat*ones(1,N)+[xsigma, -xsigma];
  X=feval(fct,dq,Xa);

  xtilde=sum(X')'/N;

  Pxx=zeros(dx,dx);
  for i=1:N;
    Pxx=Pxx+(X(:,i)-xtilde)*(X(:,i)-xtilde)'/N;
  end;

  Y=feval(obsfct,X);

  ytilde=sum(Y')'/N;
  Pyy=R;
  for i=1:N;
    Pyy=Pyy+(Y(:,i)-ytilde)*(Y(:,i)-ytilde)'/N;
  end;
  Pxy=zeros(dx,dy); 
  for i=1:N;
    Pxy=Pxy+(X(:,i)-xtilde)*(Y(:,i)-ytilde)'/N;
  end;
  
  K=Pxy*inv(Pyy);
  xhat=xtilde+K*(y-ytilde);
  Pxx=Pxx-K*Pxy';
