function [Errf, Errb, Errs] = FixIntSmooth(duration, dt, measnoise)

% function FixIntSmooth(duration, dt)
%
% Fixed lag smoother simulation for a vehicle travelling along a road.
% INPUTS
%   duration = length of simulation (seconds)
%   dt = step size (seconds)
%   measnoise = standard deviation of measurement noise
% OUTPUTS
%   Errf = norm of state estimation error of the forward filter at the desired time of estimation
%   Errb = norm of state estimation error of the backward filter at the desired time of estimation
%   Errf = norm of state estimation error of the smoothed filter at the desired time of estimation

accelnoise = 10; % acceleration noise (feet/sec^2)

if ~exist('duration', 'var')
   duration = 10;
end
if ~exist('dt', 'var')
   dt = 0.1;
end
if ~exist('measnoise', 'var')
	measnoise = 10; % position measurement noise (feet)
end

% We want to obtain a smoothed estimate at the halfway point.
EstPoint = duration / 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

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

yArr = [];
% Simulate the system and collect the measurements.
% Use a constant commanded acceleration.
u = 10;
for t = 0 : dt: duration
    if t == EstPoint
        xTrue = x;
    end
    ProcessNoise = accelnoise * [(dt^2/2)*randn; dt*randn];
    x = a * x + b * u + ProcessNoise;
    y = c * x + measnoise * randn;
    yArr = [yArr y];
end

% Obtain the forward estimate.
Pf = [20 0; 0 20]; % initial forward estimation covariance
PfArr = [];
xhatf = x + sqrt(Pf) * ones(size(x)); % initial state estimate
index = 1;
for t = 0 : dt : EstPoint
   % Extrapolate the most recent state estimate to the present time.
   xhatf = a * xhatf + b * u;
   % Form the Innovation vector.
   Inn = yArr(index) - c * xhatf;
   % Compute the covariance of the Innovation.
   CovInn = c * Pf * c' + Sz;
   % Compute the covariance of the estimation error.
   Pf = a * Pf * a' - a * Pf * c' * inv(CovInn) * c * Pf * a' + Sw;
   % Form the Kalman Gain matrix.
   K = a * Pf * c' * inv(CovInn);
   % Update the state estimate.
   xhatf = xhatf + K * Inn;
   % Save some parameters for plotting later.
   PfArr = [PfArr trace(Pf)];
   % Increment the pointer to the measurement array.
   index = index + 1;
end
Errf = norm(xTrue - xhatf);

% Obtain the backward estimate.
% The initial backward information matrix Ibminus needs to be set to a
% small nonzero matrix so that the first calculated Ibplus is invertible.
Ibminus = [0 0; 0 1e-12]; 
PbArr = [];
s = zeros(size(x)); % initial backward modified estiamte
index = length(yArr);
Sw = Sw + [0 0; 0 0]; 
for t = duration : -dt : EstPoint + dt
    Ibplus = Ibminus + c' * inv(Sz) * c;
    s = s + c' * inv(Sz) * yArr(index);
    % The following line does not work unless Sw is invertible.
    %Ibminus = inv(Sw) - inv(Sw) * inv(a) * inv(Ibplus + inv(a)' * inv(Sw) * inv(a)) * inv(a)' * inv(Sw);
    Ibminus = inv(inv(a) * inv(Ibplus) * inv(a)' + Sw);
    s = Ibminus * inv(a) * inv(Ibplus) * s - Ibminus * inv(a) * b * u;
    % Save some parameters for plotting later.
    Pbminus = inv(Ibminus);
    PbArr = [PbArr trace(Pbminus)];
    % Decrement the pointer to the measurement array.
    index = index - 1;
end
xhatb = inv(Ibminus) * s;
Errb = norm(xTrue - xhatb);

% Obtain the smoothed estimate
Kf = Pbminus * inv(Pf + Pbminus);
xhat = Kf * xhatf + (eye(2) - Kf) * xhatb;
Errs = norm(xTrue - xhat);

disp(['forward error = ', num2str(Errf), ', backward error = ', num2str(Errb), ', smoothed error = ', num2str(Errs)]);

% Compute the smoothed estimation covariance.
P = inv(inv(Pf) + Ibminus);

close all;
set(gca,'FontSize',12); set(gcf,'Color','White');
plot(0:dt:EstPoint, PfArr, 'b', duration:-dt:EstPoint+dt, PbArr, 'r', EstPoint, trace(P), 'ko');
hold on;
plot(EstPoint, PfArr(end), 'bo', EstPoint+dt, PbArr(end), 'ro');
axis([0 duration 0 2*max(PfArr)]);
xlabel('Time step'); ylabel('Trace of estimation covariance');