Example of using Kalman filter for estimates velocity and distance of the vehicle

kalman.m

function kalman(duration, dt)

% function kalman(duration, dt)
%
% Kalman filter simulation for a vehicle travelling along a road.
% INPUTS
% duration = length of simulation (seconds)
% dt = step size (seconds)

measnoise = 10; % position measurement noise (feet)
accelnoise = 0.2; % acceleration noise (feet/sec^2)

a = [1 dt; 0 1]; % transition matrix
b = [dt^2/2; dt]; % input matrix
c = [1 0]; % measurement matrix
x = [0; 0]; % initial state vector
xhat = x; % initial state estimate

Sz = measnoise^2; % measurement error covariance
Sw = accelnoise^2 * [dt^4/4 dt^3/2; dt^3/2 dt^2]; % process noise cov
P = Sw; % initial estimation covariance

% Initialize arrays for later plotting.
pos = []; % true position array
poshat = []; % estimated position array
posmeas = []; % measured position array
vel = []; % true velocity array
velhat = []; % estimated velocity array

for t = 0 : dt: duration,
% Use a constant commanded acceleration of 1 foot/sec^2.
    u = 1;
    % Simulate the linear system.
    ProcessNoise = accelnoise * [(dt^2/2)*randn; dt*randn];
    x = a * x + b * u + ProcessNoise;
    % Simulate the noisy measurement
    MeasNoise = measnoise * randn;
    y = c * x + MeasNoise;
    % Extrapolate the most recent state estimate to the present time.
    xhat = a * xhat + b * u;
    % Form the Innovation vector.
    Inn = y - c * xhat;
    % Compute the covariance of the Innovation.
    s = c * P * c' + Sz;
    % Form the Kalman Gain matrix.
    K = a * P * c' * inv(s);
    % Update the state estimate.
    xhat = xhat + K * Inn;
    % Compute the covariance of the estimation error.
    P = a * P * a' - a * P * c' * inv(s) * c * P * a' + Sw;
    % Save some parameters for plotting later.
    pos = [pos; x(1)];
    posmeas = [posmeas; y];
    poshat = [poshat; xhat(1)];
    vel = [vel; x(2)];
    velhat = [velhat; xhat(2)];
end

% Plot the results
close all;
t = 0 : dt : duration;

figure;
plot(t,pos, t,posmeas, t,poshat);
grid;
xlabel('Time (sec)');
ylabel('Position (feet)');
title('Figure 1 - Vehicle Position (True, Measured, and Estimated)')

figure;
plot(t,pos-posmeas, t,pos-poshat);
grid;
xlabel('Time (sec)');
ylabel('Position Error (feet)');
title('Figure 2 - Position Measurement Error and Position Estimation Error');

figure;
plot(t,vel, t,velhat);
grid;
xlabel('Time (sec)');
ylabel('Velocity (feet/sec)');
title('Figure 3 - Velocity (True and Estimated)');

figure;
plot(t,vel-velhat);
grid;
xlabel('Time (sec)');
ylabel('Velocity Error (feet/sec)');
title('Figure 4 - Velocity Estimation Error');