function [state, weight, normweight] = sipath(start, obspath, F, G, H, meassigma, ...
                                  accelsigma, nimpute)
% *************************************************************************
%
% FILE:sipath.m
%
% Date Created: April 7, 2004
% Author: Mark E. Irwin
%
% Contents: Sequential Imputation for target tracking with model
%           Process: X_t = GX_t-1 + H delta_t; delta_t ~ N(0,lambda)
%           Measurement: Z_T = FX_t + epsilon_t;   epsilson_t ~ N(0,sigma)
%
% Revision History
% Date          Name            Changes/Reasons
%
% *************************************************************************
%
% Input:
%
%   start: starting point of target
%   obspath: observed target path
%   F: measurement model indicator
%   G: process model evolution
%   H: process acceleration transform
%   meassigma: measurement error variance
%   accelsigma: acceleration variance
%   nimpute: number of imputations
%
% Output:
%
%   state: posterior mean of state (#states x #obs + 1 x nimpute)
%   weight: importance sampling weights (#obs + 1 x nimpute)
%   normweight: normalized importance sampling weights (#obs + 1 x nimpute)
%
% *************************************************************************

[nstates,obs] = size(obspath);

state = zeros(nstates, obs + 1, nimpute);
weight = ones(obs + 1, nimpute);
normweight = ones(obs + 1, nimpute) / nimpute;

% initial state variable with initial state
for i = 1:nimpute
    state(:,1,i) = start;
end

% run sampler
for i = 1:obs
    for j = 1:nimpute
        
        % initialize means and variances
        
        pstate = G * state(:,i,j);  % prior mean of state at time i+1
        psigma = H * accelsigma * H'; % prior state variance
        msigma = meassigma + F * psigma * F'; % obs variance given previous state
        
        avg = pstate + psigma / msigma * (obspath(:,i) - F * pstate);
        sig = psigma - psigma * F' / msigma * F * psigma;
        
        state(:, i+1, j) = mvnrnd(avg,sig)';
        
        % update weight
        
        multi = mvnpdf(obspath(:, i)', (F * pstate)', msigma);
        weight(i+1,j) = weight(i,j) * multi;
        normweight(i+1,j) = normweight(i,j) * multi;
        
    end
    
    sumnweight = sum(normweight(i+1,:));
    normweight(i+1,:) = normweight(i+1,:) / sumnweight;
    
end