%% Unscented Transform: ejemplo función no lineal \mathbb R^2\mapsto\mathbb R
% © 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/ukf2ml.html
% Este código ejecutó correctamente con Matlab R2019a
% Objetivo: comprobar la exactitud de propagación de media y varianza de una función no lineal de dos entradas y una salida mediante la unscented transform, comparando con linealización y con simulación (partículas).
%% Modelo teórico de la distribución de entrada

n=2;%dimensión
Q=[1 1;0.1 0.6];%matriz arbitraria
x0=[2 1]; %valor central (media)
Sigma=Q'*Q %matriz varianzas-covarianzas

%% Generación de N partículas, para verificación por simulación

N=100000;
R=randn(N,2);
Particles=R*Q+x0; %cada punto en FILA
mean(Particles)
cov(Particles)

%% Unscented Transform: generación sigma-points

[V,D]=eig(Sigma) %direcciones y varianzas principales
kappa=1;
deltaX=[ [0;0] V*sqrt(D) -V*sqrt(D)]*sqrt(2+kappa); 
PuntosUT=deltaX+x0' %cada pto en COLUMNA
denom=2*(n+kappa);
PesosUT=[2*kappa/denom 1/denom*ones(1,2*n)]

% Media y varianza de los sigma-point

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

%% Representación gráfica

plot(Particles(:,1),Particles(:,2),'.','Color',[.7 .7 .75]), grid on
hold on
plot(PuntosUT(1,:),PuntosUT(2,:),'o','LineWidth',3)
hold off
axis equal

%% Evaluación de media y varianza de función no lineal (cuadrática)

J=[2 5]; hh=-1; H=[0 hh;hh 0];
f=@(x) 1.5+x*J'+2*hh*x(:,1).*x(:,2);%puntos en FILA
%predicción LINEALIZADO
meanLin=f(x0)
varLin=J*Sigma*J'
%predicción Partículas (Monte-Carlo simulation)
FParticles=f(Particles);
mean(FParticles)
var(FParticles)
%Predicción Unscented
FUnscented=f(PuntosUT')
MFU=sum(FUnscented.*PesosUT')
covUnscented=sum((FUnscented-MFU).^2.*PesosUT')
histogram(FParticles), grid on