Exemple de filtre de Kalman

Introduction

Le filtre de Kalman est un outil mathématique couramment utilisé pour réaliser de la fusion de données provenant de différents capteurs. We allons illustré son utilisation sur une exemple simple permettant de comprendre comment le filtre fonctionne. COnsidérons un robot situé en face d’un mur qui ne peut se déplacer que dans une seule direction. Supposons que le robot est équipé de deux capteurs : un capteur de vitesse et un capteur de distance (range finder). Nous supposerons que les deux capteurs sont bruités.

Eemple de filtre de Kalman pour un vehicleavec deux capteurs (distance et vitesse)

Le but est ici d’estimer, le plus précisément possible, la position x du robot :

Real trajectory of the vehicle

Les entrées du système sont une mesure de distance bruitée et une mesure de vitesse, elle aussi buitée :

Mesure bruitée de la vitesse du véhicule

Mesure bruitée du capteur de distance

Resultats

Les résultats montrent que la fusion des données réduit grandement l'incertitude (lignes rouges) et l'estimation globale de la position est relativement proche de la trajectoire réelle :

Position estimée (sortie du filtre de Kalman)

Code source

close all;
clear all;
clc;

% Step time
dt=0.1;
% Variance of the range finder
SensorStd=0.03;
% Variance of the speed measurement
SpeedStd=0.2;

%% Compute the vehicle trajectory and simulate sensors
% Time
t=[0:dt:10];
n=size(t,2)
% Trajectory
x=0.05*t.*cos(2*t);
% Accelerometer
u=[0,diff(x)/dt] + SpeedStd*randn(1,n);
% Range finder
z=x-SensorStd*randn(1,n);

% Display position
figure;
plot (t,x);
grid on;
xlabel ('Time [s]');
ylabel ('Position [m]');
title ('vehicle real position versus time x=f(t)');

% Display accelation
figure;
plot (t,u,'m');
grid on;
hold on;
plot (t,[0,diff(x)/dt]+3*SpeedStd,'k-.');
plot (t,[0,diff(x)/dt]-3*SpeedStd,'k-.');
xlabel ('Time [s]');
ylabel ('Velocity [m/s]');
title ('Velocity versus time u=f(t)');

% Display range finder
figure;
plot (t,z,'r');
grid on;
hold on;
plot (t,x+3*SensorStd,'k-.');
plot (t,x-3*SensorStd,'k-.');
xlabel ('Time [s]');
ylabel ('Distance [m]');
title ('Distance versus time z=f(t)');

%% Kalman

B=dt;
F=1;
Q=SpeedStd^2;
H=1;
R=SensorStd^2;

x_hat=[0];
y_hat=[0];
P=[0];
S=[0];

for i=2:n

    %% Prediction

    %State
    x_hat(i)=F*x_hat(i-1) + B*u(i-1);

    % State uncertainty
    P(i)=F*P(i-1)*F' + Q;

    %% Update    

    % Innovation or measurement residual
    y_hat(i) = z(i)-H*x_hat(i);

    % Innovation (or residual) covariance
    S = H*P(i)*H'+R;

    K = P(i)*H'* inv(S);

    x_hat(i) = x_hat(i) + K*y_hat(i);

    P(i) = (1-K*H)*P(i);    
end

figure;
plot (t,x,'k');
hold on;
plot (t,x_hat);
plot (t,x_hat+sqrt(P),'r-.');
plot (t,x_hat-sqrt(P),'r-.');
legend('Real position','Estimated position','Uncertainty');

grid on;
xlabel ('Time [s]');
ylabel ('Distance [m]');
title ('Estimated position');

Téléchargement

Cet example a été inspiré par l’excellent tutorial de Bradley Hiebert-Treuer An Introduction to Robot SLAM (Simultaneous Localization And Mapping)

introduction-to-slam.pdf


Dernière mise à jour : 26/11/2018