kalman_filter.m

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

% Define parameters
measnoise  = 10; % position measurement noise (feet)
accelnoise = 0.2; % acceleration noise (feet/sec^2)
duration   = 60; 
dt         = 0.1;

% Define matrices
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 results
t = 0 : dt : duration;
figure(1)
plot(t,pos, t,posmeas, t,poshat)
grid
xlabel('Time (sec)')
ylabel('Position (feet)')
title('Vehicle Position (True, Measured, and Estimated)')

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

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

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

%% end