function kalmanDemo_vehiclePosition(duration, dt)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Reference
% Kalman Filtering - by Dan Simon
% Link: https://academic.csuohio.edu/simond/courses/eec644/kalman.pdf
% (Edited/modified from the code provided in the article)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

measnoise_stdDev = 10; % (v_k) position measurement noise (feet)
accelnoise_stdDev = 10000.2; % acceleration noise (feet/sec^2)
A = [1 dt; 0 1]; % transition matrix 
B = [dt^2/2; dt]; % input matrix
H = [1 0]; % measurement matrix
x = [0; 0]; % initial state vector 
u = 1; %Used constant commanded acceleratiom
xhat = x; % initial state estimate
V = measnoise_stdDev^2; % measurement error covariance
W = accelnoise_stdDev^2 * [dt^4/4 dt^3/2; dt^3/2 dt^2]; % process noise 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
P = W; % initial estimation covariance

for t = 0 : dt: duration
    % Use a time varying 'commanded acceleration'
    % u = randn;
    
    %%%%%%%%%%%%%%%% SIMULATION %%%%%%%%%%%%%%%%
    % Simulate the linear system.
    ProcessNoise = accelnoise_stdDev * [(dt^2/2)*randn; dt*randn];
    x = A * x + B * u + ProcessNoise;    
    % Simulate the noisy measurement
    MeasNoise = measnoise_stdDev * randn;
    z = H * x + MeasNoise;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    %%%%%%%%%%% MEASUREMENT and ESTIMATION %%%%%%%%%%
    % Extrapolate the most recent state estimate to the present time.
    xhat = A * xhat + B * u;
    % Form the Innovation vector.
    Inn = z - H * xhat;
    % Compute the covariance of the Innovation.
    s = H * P * H' + V;
    % Form the Kalman Gain matrix,(Alternate formula: K = A * P * H' * inv(s);)
    K = P * H' * inv(s);    
    % Update the state estimate.
    xhat = xhat + K * Inn;
    % Compute the covariance of the estimation error.
    P = A * P * A' - A * P * H' * inv(s) * H * P * A' + W;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    %%%%%%%%%%% DATA LOGGING FOR PLOTS %%%%%%%%%%
    pos = [pos; x(1)];
    posmeas = [posmeas; z];
    poshat = [poshat; xhat(1)]; vel = [vel; x(2)];
    velhat = [velhat; xhat(2)];
end

% Plot the results close all;
t = 0 : dt : duration;
figure; hold on;
%plot(t,pos, t,posmeas, t,poshat);
plot(t,pos, 'r', 'linewidth', 2);
plot(t,posmeas, 'g', 'linewidth', 2);
plot(t,poshat, 'b', 'linewidth', 2);
grid;
xlabel('Time (sec)');
ylabel('Position (feet)');
title('Figure 1 - Vehicle Position (True, Measured, and Estimated)')
legend('true', 'measured', 'estimated');
set(gca,'FontSize',12);

figure; hold on;
plot(t,pos-posmeas, 'r', 'linewidth', 2);
plot(t,pos-poshat, 'g', 'linewidth', 2);
grid;
xlabel('Time (sec)');
ylabel('Position Error (feet)');
title('Figure 2 - Position Measurement Error and Position Estimation Error');
legend('measurement error', 'estimation error');
set(gca,'FontSize',12);

figure; hold on;
plot(t,vel, 'r', 'linewidth', 2);
plot(t,velhat, 'g', 'linewidth', 2);
grid;
xlabel('Time (sec)');
ylabel('Velocity (feet/sec)');
title('Figure 3 - Velocity (True and Estimated)');
legend('true', 'estimated');
set(gca,'FontSize',12);

figure; hold on;
plot(t,vel-velhat, 'r', 'linewidth', 2);
grid;
xlabel('Time (sec)');
ylabel('Velocity Error (feet/sec)'); title('Figure 4 - Velocity Estimation Error');
set(gca,'FontSize',12);