derinier partie : 
donne_pb = load("valeurs_ex2.txt");

donne_pb
T=donne_pb(1);
sig_p = donne_pb(2);
sig_v = donne_pb(3);
sig_z = donne_pb(4);
sig_z_v = 10;
N = donne_pb(5);
#Mesures
Mes = load("mesures_2.txt");
Mes = Mes(:,4:6);

Mes
Comp1 = load("reality_2.txt");
Comp1

#Remplissage F
F = eye(6);
i=1;
for j = 4:6
   F(i,j) = T;
   i+=1
endfor
F
W = eye(6)
for i=1:3
  W(i,i)=T *sig_p^2;
endfor
for i=4:6
  W(i,i) = T *sig_v^2;
endfor

H = zeros(3,6);
for i=4:6
    H(i-3,i)=1;
endfor

V = (sig_z_v ^2) * eye(3);



k = 0;
x_0 = zeros(6,1)
P_0= 1000 * eye(6);
z_0 = Mes(1,:);

#Initialisation
x_k = x_0;
P_k = P_0;

trace_k= zeros(N,1);
X= zeros(N,1);
Y=zeros(N,1);
Z=zeros(N,1);


#Bande incertitude
X_minus_2sigma = zeros(N,1);
Y_plus_2sigma = zeros(N,1);
Y_minus_2sigma = zeros(N,1);
Z_plus_2sigma = zeros(N,1);
Z_minus_2sigma = zeros(N,1);

for k=1:N

  #Prediction 1ere etape :
  x_k = F*x_k;
  P_k = F*(P_k*F') +W;
  #Correction  2eme etape:
  K = (P_k * H')  * (inverse(H * (P_k *H') + V));
  x_k =   x_k+ K*(Mes(k,:)'- H*x_k);
  P_k = (eye(6) - K*H)*P_k;

  #
  trace_k(k) = trace(P_k);
  X(k)=x_k(1);
  Y(k) = x_k(2);
  Z(k) = x_k(3);

  #Incertiude :
  X_plus_2sigma(k) = X(k) + 2 * sqrt(P_k(1,1));
  X_minus_2sigma(k) = X(k) - 2 * sqrt(P_k(1,1));

  Y_plus_2sigma(k) = Y(k) + 2 * sqrt(P_k(2,2));
  Y_minus_2sigma(k) = Y(k) - 2 * sqrt(P_k(2,2));

  Z_plus_2sigma(k) = Z(k) + 2 * sqrt(P_k(3,3));
  Z_minus_2sigma(k) = Z(k) - 2 * sqrt(P_k(3,3));
endfor
step = 1:N;
#figure;
subplot(2,2,1);
color_estimation = [0.298, 0.447, 0.690];
color_real = [0.867, 0.518, 0.322];  # Coral/Red
color_sigma = [0.333, 0.658, 0.408];
plot(step,trace_k,'Color', color_estimation,'LineWidth', 2,'DisplayName',"Trace de la matrice de covariance");
legend show;
grid on;
title("Trace de la Matrice de Covariance");
#saveas(gcf, 'trace_mat_cov_v.png', 'png');
#plot X
subplot(2,2,2);
#figure;
plot(step,X,'Color', color_estimation,'LineWidth', 2,'DisplayName', "Estimation de x");
hold on;
plot(step,Comp1(1,:),'Color',color_real,'LineStyle','--','LineWidth', 1.5,'DisplayName',"Valeur reel de x");
plot(step,X_plus_2sigma,'Color',color_sigma,'LineStyle','--','LineWidth', 1,'DisplayName', "Estimation + 2*σ de x");
plot(step,X_minus_2sigma,'Color',color_sigma,'LineStyle','--','LineWidth', 1,'DisplayName', "Estimation - 2*σ de x");
legend show;
grid on;
#saveas(gcf, 'estimation_x_v.png', 'png');
title("Estimation de X avec ±2σ");
#plot Y
subplot(2,2,3);
#figure;
plot(step,Y,'Color', color_estimation,'LineWidth', 2,'DisplayName', "Estimation de y");
hold on;
plot(step,Comp1(2,:),'Color',color_real,'LineStyle','--','LineWidth', 1.5,'DisplayName',"Valeur reel de y");
plot(step,Y_plus_2sigma,'Color',color_sigma,'LineStyle','--','LineWidth', 1,'DisplayName', "Estimation + 2*σ de y");
plot(step,Y_minus_2sigma,'Color',color_sigma,'LineStyle','--','LineWidth', 1,'DisplayName', "Estimation - 2*σ de y");
legend show;
grid on;
#saveas(gcf, 'estimation_y_v.png', 'png');
title("Estimation de Y avec ±2σ");
subplot(2,2,4);
#figure;
plot(step,Z,'Color', color_estimation,'LineWidth', 2,'DisplayName', "Estimation de z");
hold on;
plot(step,Comp1(3,:),'Color',color_real,'LineStyle','--','LineWidth', 1.5,'DisplayName',"Valeur reel de z");
plot(step,Z_plus_2sigma,'Color',color_sigma,'LineStyle','--','LineWidth', 1,'DisplayName', "Estimation + 2*σ de z");
plot(step,Z_minus_2sigma,'Color',color_sigma,'LineStyle','--','LineWidth', 1,'DisplayName', "Estimation - 2*σ de z");
legend show;
grid on;
#saveas(gcf, 'estimation_z_v.png', 'png');
title("Estimation de Z avec ±2σ");
S = axes('visible', 'off','title',"Estimation avec vitesse sans position");

combinedMatrix = [X, Y, Z];
csvwrite('true_positions.csv', Mes(:,1:3));

csvwrite('estimated_positions.csv', combinedMatrix);


=> 
Reprendre à présent les équations du filtre mais dans le cas où on ne dispose que des mesures de
vitesse, soit H =


0 0 0 1 0 0
0 0 0 0 1 0
0 0 0 0 0 1

. Analyser la structure de la matrice de gain de Kalman
K =
0.097117 0 0
0 0.097117 0
0 0 0.097117
0.031240 0 0
0 0.031240 0
0 0 0.031240