The Kalman filter is a very useful mathematical tool for merging multi-sensor data. We’ll consider a very simple example for understanding how the filter works. Let’s consider a robot that move in a single direction in front of a wall. Assume that the robot is equipped with two sensors : a speed measurement sensor and a distance measurement sensor (range finder). We’ll consider in the following that both sensors are noisy.
Our goal is to estimate, as accurately as possible, the position x of the robot:
Input of the system are a noisy distance measurement and a noisy velocity measurement:
The results show that merging data from both sensors highly reduce incertainty (red lines) and global estimation is quite close from real position:
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);
% Speedometer
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 velocity
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*dt)^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');
This example has been inspired by the very good tutorial of Bradley Hiebert-Treuer “An Introduction to Robot SLAM (Simultaneous Localization And Mapping)”