% The unscented transform
% Example taken from:
% S. Julier and J. Uhlmann, "Unscented filtering and nonlinear estimation", Proc. of the IEEE, 92(3), pag. 401-422, 2004

clear; close all;

N=5000;
stdd=0.02;
stdalfa=15/180*pi;
d=1+stdd*randn(N,1);
alfa=pi/2+stdalfa*randn(N,1);

X=d.*cos(alfa); Y=d.*sin(alfa);
figure(1), plot(X,Y,'+');
v=[-0.8 0.8 0.5 1.2]; axis(v);
title('1000 points [d cos(\alpha), d sin(\alpha)]')
pause

% monte carlo (true)
mX=mean(X);mY=mean(Y); xc=[mX;mY];
P=1/N*([(X-mX)'; (Y-mY)']*[X-mX Y-mY]);
figure(2),
plotell(inv(P),-inv(P)*xc,xc'*inv(P)*xc-1,'--');
hold on
plot(mX,mY,'o')

%linearization 
mXlin=0;
mYlin=1;
xclin=[mXlin;mYlin];
Plin=diag([(15/180*pi)^2 0.02^2]);
plotell(inv(Plin),-inv(Plin)*xclin,xclin'*inv(Plin)*xclin-1,'r');
plot(mXlin,mYlin,'xr')
title('confidence ellipsoids: sampled (blue) linearized (red)')
pause

% sigma points (eq. 11 in Julier-Uhlmann)
xbar=[1; pi/2];
Sigmabar=diag([stdd^2 stdalfa^2]);
Nx=2;
M=sqrtm(Nx*Sigmabar);
Xs=[xbar+M(:,1) xbar+M(:,2) xbar-M(:,1) xbar-M(:,2)];
W=1/(2*Nx)*ones(1,4);

zv=[Xs(1,:).*cos(Xs(2,:)); Xs(1,:).*sin(Xs(2,:))];
figure(3), plot(zv(1,:),zv(2,:),'*')
title('transformed sigma points (eq. 11 in Julier-Uhlmann)')
 
mXu=W*zv(1,:)';mYu=W*zv(2,:)'; xcu=[mXu;mYu];
Pu=0; for i=1:2*Nx, Pu=Pu+W(i)*(zv(:,i)-xcu)*(zv(:,i)-xcu)'; end;
figure(4),
plotell(inv(P),-inv(P)*xc,xc'*inv(P)*xc-1,'--');
hold on
plot(mX,mY,'o')
plotell(inv(Plin),-inv(Plin)*xclin,xclin'*inv(Plin)*xclin-1,'r');
plot(mXlin,mYlin,'xr')
plotell(inv(Pu),-inv(Pu)*xcu,xcu'*inv(Pu)*xcu-1,'k-.');
plot(mXu,mYu,'sk')
title('confidence ellipsoids: sampled (blue) linearized (red) unscented base (black)')
pause

% new sigma points (eq. 12 in Julier-Uhlmann)
xbar=[1; pi/2];
Sigmabar=diag([stdd^2 stdalfa^2]);
Nx=2;
W0=1/3;
M=sqrtm(Nx/(1-W0)*Sigmabar);
Xsnew=[xbar xbar+M(:,1) xbar+M(:,2) xbar-M(:,1) xbar-M(:,2)];
Wnew=[W0 (1-W0)/(2*Nx) (1-W0)/(2*Nx) (1-W0)/(2*Nx) (1-W0)/(2*Nx)];

zvnew=[Xsnew(1,:).*cos(Xsnew(2,:)); Xsnew(1,:).*sin(Xsnew(2,:))];
figure(5), plot(zvnew(1,:),zvnew(2,:),'*')
title('extended transformed sigma points (eq. 12 in Julier-Uhlmann)')
 
mXu=Wnew*zvnew(1,:)';mYu=Wnew*zvnew(2,:)'; xcu=[mXu;mYu];
Pu=0; for i=1:2*Nx+1, Pu=Pu+Wnew(i)*(zvnew(:,i)-xcu)*(zvnew(:,i)-xcu)'; end;
figure(6),
plotell(inv(P),-inv(P)*xc,xc'*inv(P)*xc-1,'--');
hold on
plot(mX,mY,'o')
plotell(inv(Plin),-inv(Plin)*xclin,xclin'*inv(Plin)*xclin-1,'r');
plot(mXlin,mYlin,'xr')
plotell(inv(Pu),-inv(Pu)*xcu,xcu'*inv(Pu)*xcu-1,'k-.');
plot(mXu,mYu,'sk')
title('confidence ellipsoids: sampled (blue) linearized (red) unscented extended (black)')

%%

function [cen,ax,X]=plotell(A,b,c,color)
% PLOTELL Plots the ellipsoid X'AX+2b'X+c<0 in the plane
%
%         [cen,ax]=plotell(A,b,c,color)

n=2;

[V,D]=eig(A);
alfa=b'*V;
for i=1:n
 cen(i)=-alfa(i)/D(i,i);
 dime(i)=alfa(i)^2/D(i,i);
end

dimes=sum(dime)-c;
for i=1:n
 l(i)=sqrt(dimes/D(i,i));
end
ax=V*diag(l);

N=360;
X=zeros(n,N);

for j=1:N+1
th=(j-1)*2*pi/N;
X(:,j)=cen'+[l(1)*cos(th); l(2)*sin(th)];
end

X=V*X;     % back to the original coordinates
cen=cen*V';

hold on;
plot(X(1,:),X(2,:),color);
%if you need symmetry axes
%plot([X(1,1); X(1,N/2+1)]',[X(2,1); X(2,N/2+1)],color);
%plot([X(1,N/4+1); X(1,3*N/4+1)]',[X(2,N/4+1); X(2,3*N/4+1)],color);



