%% Generación de Sigma-points para transformación Unscented (Uhlmann-Julier)
% © 2019, Antonio Sala Piqueras, Universitat Politècnica de València. Todos los derechos reservados.
% Presentación en vídeo: http://personales.upv.es/asala/YT/V/sigptml.html
% Este código funcionó correctamente en Matlab R2019a
%% Ejemplo unidimensional
% Generemos muestras de una distribución normal de media -4 y desv. típ 2 (vza 4):

n=1;
desvtip = 2;  x0 = -4; 
R=randn(8000,n)*desvtip+x0;
mean(R)
cov(R)
skewness(R)
kurtosis(R)
histogram(R), grid on %para comparar con unscented debajo

% Unscented Transform: generemos ahora una distribución discreta de TRES puntos (2n+1), con n=1:

kappa=2;
X=x0+sqrt(n+kappa)*[0 desvtip -desvtip]
xline(X(1),'Color',[.7 0.1 0.1],'LineWidth',3);
xline(X(2),'LineWidth',3);
xline(X(3),'LineWidth',3);
den=2*(n+kappa);
W=[2*kappa 1 1]/den
meanUT=sum(X.*W)
varUT=sum((X-meanUT).^2.*W)
stdUT=sqrt(varUT)
skewUT=sum((X-meanUT).^3.*W)/stdUT^3
kurtUT=sum((X-meanUT).^4.*W)/stdUT^4

%
%% Ejemplo bidimensional

n=2;
x0=[2;1]; Matriz=[1 1;0.1 0.6];
Nparticulas=1e4;
R=randn(Nparticulas,2);X=R*Matriz+x0';
Sigma=Matriz'*Matriz %teórica
mx=mean(X) %casi como x0, estimado muestral
%vzas-covarianzas muestral
SigmaMuestral=(X-mx)'*(X-mx)/(Nparticulas-1)
[V,D]=eig(Sigma) %direcciones y varianzas principales
kappa=0; 
deltaX=[ [0;0] V*sqrt(D) -V*sqrt(D)]*sqrt(2+kappa);
puntosUT=deltaX+x0
denom=2*(n+kappa);
PesosUT=[2*kappa/denom 1/denom*ones(1,2*n)]
plot(X(:,1),X(:,2),'.','Color',[.7 .7 .75]), grid on
hold on
plot(puntosUT(1,:),puntosUT(2,:),'o','LineWidth',3)
hold off
axis equal

% Media y varianza de los cinco sigma-point

sum(puntosUT'.*PesosUT') %coincide con x0
deltaX*diag(PesosUT)*deltaX' %coincide con Sigma teórica