function [xsmooth] = FixPtSmooth(duration, dt, measnoise, PlotFlag)

% function FixPtSmooth(duration, dt)
%
% Fixed point smoother simulation for a two state navigation problem.
% INPUTS
%   duration = length of simulation (seconds)
%   dt = step size (seconds)
%   measnoise = standard deviation of position measurement noise
%   PlotFlag = true/false flag saying whether or not to generate plots
% OUTPUT
%   xsmooth = estimate of the initial state after all measurements have been processed

if ~exist('duration', 'var')
    duration = 10;
end
if ~exist('dt', 'var')
    dt = 0.1;
end
if ~exist('measnoise', 'var')
    measnoise = 1; 
end
if ~exist('PlotFlag', 'var')
    PlotFlag = true;
end

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

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

x = [0; 0]; % initial state vector
xhat = x + sqrt(P) * 2 * ones(size(x)); % initial the a posteriori state estimate (in error by 2 sigma)

u = 10; % Use a constant commanded acceleration

xhat = a * xhat + b * u; % initial the a priori state estimate
P = a * P * a' + Sw; % initial the a priori covariance

% Initialize the fixed point smoother
Sigma = P;
Pi = P;
xsmooth = xhat;

% Initialize arrays for later plotting.
pos = []; % true position array
poshat = []; % estimated position array
possmooth = []; % smoothed position array
posmeas = []; % measured position array
vel = []; % true velocity array
velhat = []; % estimated velocity array
velsmooth = []; % smoothed velocity array
TrPArray = []; % Trace of standard estimation covariance
TrPiArray = []; % Trace of smoothed estimation covariance

randn('state', sum(100*clock)); % initialize the random number generator

% Simulate the linear system and the noisy measurement.
ProcessNoise = accelnoise * [(dt^2/2)*randn; dt*randn];
x = a * x + b * u + ProcessNoise;
y = c * x + measnoise * randn;
% Form the Innovation vector.
Inn = y - c * xhat;

for t = 0 : dt: duration
    % Save some parameters for plotting later.
    pos = [pos; x(1)];
    posmeas = [posmeas; y];
    poshat = [poshat; xhat(1)];
    possmooth = [possmooth; xsmooth(1)];
    vel = [vel; x(2)];
    velhat = [velhat; xhat(2)];
    velsmooth = [velsmooth; xsmooth(2)];
    TrPArray = [TrPArray trace(P)];
    TrPiArray = [TrPiArray trace(Pi)];
    
    % Compute the covariance of the Innovation.
    s = c * P * c' + Sz;
    % Form the Kalman Gain matrix.
    K = a * P * c' * inv(s);
    
    % Compute the smoothed estimate.
    Lambda = Sigma * c' * inv(s); % smoother gain
    Pi = Pi - Sigma * c' * Lambda'; % covariance of smoothed estimate
    Sigma = Sigma * (a - K * c)'; % cross covariance of standard and smoothed estimates
    xsmooth = xsmooth + Lambda * Inn; % smoothed estimate of the initial state
    
    % Update the state estimate.
    xhat = a * xhat + K * Inn + b * u;
    % Compute the covariance of the estimation error.
    P = a * P * a' - a * P * c' * K' + Sw;
    
    % Simulate the linear system and the noisy measurement.
    ProcessNoise = accelnoise * [(dt^2/2)*randn; dt*randn];
    x = a * x + b * u + ProcessNoise;
    y = c * x + measnoise * randn;
    % Form the Innovation vector.
    Inn = y - c * xhat;
    
end

if ~PlotFlag
    return;
end

disp(['Pi(1,1) = ', num2str(Pi(1,1)), ', Pi(2,2) = ', num2str(Pi(2,2))]);
Improve = 100 * (TrPArray(1) - trace(Pi)) / TrPArray(1);
disp(['Smoothing Improvement = ',num2str(Improve),' %']);

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

figure;
plot(t,TrPArray,'r-', t,TrPiArray,'b:');
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Time');
ylabel('Analytical Estimation Covariance', 'FontSize', 12);
legend('Standard Filter', 'Smoothed Filter'); 

figure;
plot(t,possmooth-pos(1),'r', t,velsmooth-vel(1),'b:');
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Time');
ylabel('Smoothing Error at Initial Time', 'FontSize', 12);
legend('Position Error', 'Velocity Error'); 

figure;
plot(t,pos,'r-', t,posmeas,'b:', t,poshat,'k--');
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Time');
ylabel('Position');
legend('True Position', 'Measured Position', 'Estimated Position');

figure;
plot(t,pos-posmeas,'r-', t,pos-poshat,'b:');
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Time (sec)');
ylabel('Position Error');
legend('Position Measurement Error', 'Position Estimation Error');

figure;
plot(t,vel,'r-', t,velhat,'b:');
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Time');
ylabel('Velocity');
legend('True Velocity', 'Estimated Velocity');

figure;
plot(t,vel-velhat);
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Time');
ylabel('Velocity Estimation Error');