% Exemples de propagations non-linéaire
% Cours de robotique mobile GLO-4001/7021
% (c) 2014 Philippe Giguere
% Université Laval
clearvars
clf
myFontSize = 14;

X = [2.5; 3.9];
P = [0.04 0.03; 0.03 0.08];

for iSample =1:2000
    Samples(:,iSample) = mvnrnd(X,P);
end

% ====================================================
% Calcul via série de Taylor (linéarisation comme EKF)
% ====================================================
subplot(2,1,1);
plot(Samples(1,:),Samples(2,:),'.');
xlabel('x','FontSize',myFontSize);
ylabel('y','FontSize',myFontSize);
set(gca,'FontSize',myFontSize);
hold on;
h = plotError(X,P,'r');
set(h,'LineWidth',4)
plot(X(1),X(2),'r+','MarkerSize',14,'LineWidth',3);
axis equal

subplot(2,1,2);
x = Samples(1,:); y = Samples(2,:);
Propagated(1,:) = 2*cos(x) + 3*sin(y) + x.*y;
Propagated(2,:) = exp(-x) + y./(x+2);
PropagatedX(1) = 2*cos(X(1)) + 3*sin(X(2)) + X(1)*X(2);
PropagatedX(2) = exp(-X(1)) + X(2)./(X(1)+2);
% Calcul de la jacobienne Phi
x = X(1); y=X(2);
Phi = [(y-2*sin(x))        (3*cos(y)+x); ...
       -y/(x^2)-exp(-x)        1/x];
PropagatedP = Phi*P*Phi';

% Tracé des résultats
plot(Propagated(1,:),Propagated(2,:),'.');
xlabel('x','FontSize',myFontSize);
ylabel('y','FontSize',myFontSize);
set(gca,'FontSize',myFontSize);
hold on;
h = plotError(PropagatedX,PropagatedP,'r'); set(h,'LineWidth',4)
plot(PropagatedX(1),PropagatedX(2),'r+','MarkerSize',14,'LineWidth',3);
axis equal

% ====================================================
%                  Unscented Transform
% ====================================================
% Calcul des Sigma Points
n = 2;
alpha = 0.001;
beta = 2;
k = 1;
lambda = alpha^2*(n+k-n);
SX(:,1) = X;
wm(1) = lambda/(lambda+n);
wc(1) = lambda/(lambda+n) + (1-alpha^2+beta);
sP = sqrtm(P*(n+lambda));
for index=1:2
    SX(:, index+1) = X + sP(:,index);
end
for index=1:2
    SX(:, index+3) = X - sP(:,index);
end
for index = 1:4
    wm(index+1) = 1/(2*(n+lambda));
    wc(index+1) = wm(index+1);
end
x = SX(1,:); y = SX(2,:);
FX(1,:) = 2*cos(x) + 3*sin(y) + x.*y;
FX(2,:) = exp(-x) + y./(x+2);

PropXUT(1,:) = sum(wm.*FX(1,:));
PropXUT(2,:) = sum(wm.*FX(2,:));
PropPUT = zeros(2,2);
for index = 1:5
    PropPUT = PropPUT + wc(index)*(FX(:,index)-PropXUT)*(FX(:,index)-PropXUT)';
end

% Calcul des erreurs, à partir des échantillons
TrueX = mean(Propagated,2);
TrueCov = cov(Propagated');

clf
plot(Samples(1,:),Samples(2,:),'.');
xlabel('x','FontSize',myFontSize);
ylabel('y','FontSize',myFontSize);
set(gca,'FontSize',myFontSize);
hold on;
h = plotError(X,P,'g');
set(h,'LineWidth',4)
%plot(X(1),X(2),'g+','MarkerSize',14,'LineWidth',3);
plot(SX(1,:),SX(2,:),'go','LineWidth',4);
axis equal

% Faire la propagation
clf
plot(Propagated(1,:),Propagated(2,:),'.','MarkerSize',2);
xlabel('x','FontSize',myFontSize);
ylabel('y','FontSize',myFontSize);
set(gca,'FontSize',myFontSize);
hold on;
h = plotError(PropXUT,PropPUT,'g'); set(h,'LineWidth',3)
h = plotError(PropagatedX,PropagatedP,'r'); set(h,'LineWidth',3,'LineStyle','--')
h = plotError(TrueX,TrueCov,'k'); set(h,'LineWidth',3,'LineStyle','--')
hl(1) = plot(TrueX(1),TrueX(2),'ko','MarkerSize',14,'LineWidth',3,'MarkerFaceColor','w');
hl(2) = plot(PropXUT(1),PropXUT(2),'gx','MarkerSize',14,'LineWidth',3);
hl(3) = plot(PropagatedX(1),PropagatedX(2),'r+','MarkerSize',14,'LineWidth',3);
legend(hl,{'Réel' 'Non-parfumé (UT)' 'Linéarisation (EKF)'},'Location', 'Northeast');

axis equal










