% CONSTANTES

dt = 1e-2;   % Pas de temps
t = 1;      % Duree de l'action
n = t/dt;    % Nombre d'iterations
tau = 4e-2;  % Constante de temps
m = 0.3;     % Masse
SD = 0.0035;  % Uncertainty initiale de la main
sigmaXpre=0.0035;% uncertainty of the hand position
sigmaT1_offset=0.05;% initial uncertainty about target

wr = 1;
ws = 1;
wp = 1;

Ad = [1 dt 0 0 0 0;
      0 1 0 0 dt/m 0;
      0 0 1 dt 0 0;
      0 0 0 1 0 dt/m;
      0 0 0 0 1-dt/tau 0;
      0 0 0 0 0 1-dt/tau];
A = [Ad zeros(6,4);
     zeros(4,6) eye(4)] ;
B = [zeros(4,2);
     dt/tau 0;
     0 dt/tau;
     zeros(4,2)];
Ak = [A zeros(10,110);
      eye(110) zeros(110,10)];
Bk = [B;
      zeros(110,2)];
Hh = [1 0 0 0 0 0 0 0 0 0;
      0 0 1 0 0 0 0 0 0 0;
      0 0 0 0 0 0 1 0 0 0;
      0 0 0 0 0 0 0 1 0 0];
H = [zeros(4,110) Hh];

L = [wr 0;
     0 wr];
Sv = [wp 0 0 0 0 0 -wp 0 0 0;
     0 ws 0 0 0 0 0 0 -ws 0;
     0 0 wp 0 0 0 0 -wp 0 0;
     0 0 0 ws 0 0 0 0 0 -ws;
     0 0 0 0 0 0 0 0 0 0;
     0 0 0 0 0 0 0 0 0 0;
     -wp 0 0 0 0 0 wp 0 0 0;
     0 0 -wp 0 0 0 0 wp 0 0;
     0 -ws 0 0 0 0 0 0 ws 0;
     0 0 0 -ws 0 0 0 0 0 ws];



% STATE NOISE
sigmaU = 1e-3;          % Bruit de la commande
sigmaTU = 1.5e-3; %OK   % Bruit sur la position de la cible
sigmaTU2 = 1.5e-3;      % Bruit sur la vitesse de la cible

OmEps = [0 0 0 0 sigmaU^2 sigmaU^2 sigmaTU^2 sigmaTU^2 sigmaTU2^2 sigmaTU2^2];
Q = diag([OmEps zeros(1,110)]);

% MEASUREMENT NOISE
sigmaP = 1e-2; %OK      % Bruit sur l estimation de position de la main
sigmaT = 1e-3;          % Bruit sur l estimation de position de la cible
Rv = diag([sigmaP^2 sigmaP^2 sigmaT^2 sigmaT^2]);

%INITIALISATION DES MATRICES
W = zeros(120,120,n);
K = zeros(120,4,n);
P = zeros(120,120,n);
Y = zeros(4,1,n);
Xk = zeros(120,1,n);
U = zeros(2,1,n);
S = zeros(120,120,n);
R = zeros(4,4,n);
omega = ones(4,1);
for i=1:n
  S(1:10,1:10,i) = Sv;
  R(1:4,1:4,i) = Rv;
end

% CONDITIONS INITIALES
Xn = [0 0 0 0 0 0 1 1 0 0];
Xkn = [Xn zeros(1,110)];
Un = [0 0];

Pv=zeros(10,10);Pv(1,1)=sigmaXpre^2;Pv(2,2)=0;Pv(3,3)=sigmaXpre;Pv(4,4)=0;Pv(5,5)=0;Pv(6,6)=0;
Pv(7,7)=(sigmaT1_offset)^2;Pv(8,8)=(sigmaT1_offset)^2;Pv(9,9)=0;Pv(10,10)=0;
P(1:10,1:10,1)=Pv;
P(:,:,1)=blkdiag(Pv,Pv,Pv,Pv,Pv,Pv,Pv,Pv,Pv,Pv,Pv,Pv);
P_b(:,:,1)=P(:,:,1);
P_b(:,:,2) = P(:,:,1);

Xk(:,:,1) = Xkn;

W(:,:,n) = S(:,:,1);
% BOUCLE INVERSE
for i=1:n-1
   W(:,:,n-i) = Ak.'*(eye(120)+W(:,:,n-i+1)*Bk*((L)\Bk.'))*W(:,:,n-i+1)*Ak; %+S(:,:,n-i);
end
% RECONSTRUCTION DES ETATS
for i=2:n-1
    Y(:,:,i) = H*Xk(:,:,i) + omega;
    K(:,:,i) = P(:,:,i)*H.'/(H*P(:,:,i)*H.'+R(:,:,i));
    P(:,:,i) = (eye(120)-K(:,:,i)*H)*P(:,:,i-1);
    Xk(:,:,i) = Xk(:,:,i-1) + K(:,:,i)*(Y(:,:,i)-H*Xk(:,:,i));
    U(:,:,i) = (L+Bk.'*W(:,:,i+1)*Bk) \ Bk.'*W(:,:,i+1)*Ak*Xk(:,:,i);
    Xk(:,:,i+1) = Ak*Xk(:,:,i) + Bk*U(:,:,i);
    P(:,:,i+1) = Ak*P(:,:,i)*Ak.'+Q;
end