// Clear initialisation
clf()

// Time vector
t =[0:0.5:100];
len_t = length(t);
dt = 100 / len_t;

// Real values x: position v: velocity a: acceleration
x_vrai = 0 * t;
v_vrai = 0 * t;
a_vrai = 0 * t;

// Meseared values.
x_mes = 0 * t;
v_mes = 0 * t;
a_mes = 0 * t;

// Initialisation
for i = 0.02 * len_t: 0.3 * len_t,
  a_vrai(i) = 1;
end

for i = 0.5 * len_t: 0.7 * len_t,
  a_vrai(i) = 2;
end

// State of kalman filter
etat_vrai = [ x_vrai(1);  v_vrai(1); a_vrai(1) ];

A = [ 1 dt dt*dt/2; 0 1 dt; 0 0 1];//transition matrice
for i = 2:length(t),
  etat_vrai = [ x_vrai(i-1);  v_vrai(i-1); a_vrai(i) ];
  etat_vrai = A * etat_vrai;
  x_vrai(i) = etat_vrai(1);  
  v_vrai(i) = etat_vrai(2);
end

max_brt_x = 200;
max_brt_v = 10;
max_brt_a = 0.3;

// Random noise
x_brt = grand(1, len_t, 'unf', -max_brt_x, max_brt_x);
v_brt = grand(1, len_t, 'unf', -max_brt_v, max_brt_v);
a_brt = grand(1, len_t, 'unf', -max_brt_a, max_brt_a);

// Compute meas
x_mes = x_vrai + x_brt;
v_mes = v_vrai + v_brt;
a_mes = a_vrai + a_brt;

// START
x_est = 0 * t;
v_est = 0 * t;
a_est = 0 * t;

etat_mes = [ 0; 0; 0 ];
etat_est = [ 0; 0; 0 ];
inn = [ 0; 0; 0 ];
// Matrix of covariance of the bruit.
// independant noise -> null except for diagonal,
// and on the diagonal == sigma^2.
// VAR(aX+b) = a^2 * VAR(X);
// VAR(X) = E[X*X] - E[X]*E[X]
R = [ max_brt_x*max_brt_x 0 0; 0 max_brt_v*max_brt_v 0; 0 0 max_brt_a*max_brt_a ];

Q = [1 0 0; 0 1 0; 0 0 1];
P = Q;
for i = 2:length(t),
  // Init : measured state
  etat_mes = [ x_mes(i);  v_mes(i); a_mes(i) ];
 
  // Innovation: measured state -  estimated state
  inn = etat_mes - etat_est;
 
  // Covariance of the innovation
  S = P + R;
 
  // Gain
  K = A * inv(S);
 
  // Estimated state
  etat_est = A * etat_est + K * inn;
 
  // Covariance of prediction error
  // C = identity
  P = A * P * A' + Q - A * P * inv(S) * P * A';
 
  // End: estimated state
  x_est(i) = etat_est(1);
  v_est(i) = etat_est(2);
  a_est(i) = etat_est(3);

end


// Blue : real
// Gree : noised
// Red: estimated
subplot(311)
plot(t, a_vrai, t, a_mes, t, a_est);  
subplot(312)
plot(t, v_vrai, t, v_mes, t, v_est);  
subplot(313)
plot(t, x_vrai, t, x_mes, t, x_est);