function [state, statevar] = kfpath(priormean, priorvar, obspath, F, G, H, meassigma, accelsigma)
% *************************************************************************
%
% FILE:kfpath.m
%
% Date Created: April 6, 2004
% Author: Mark E. Irwin
%
% Contents: Kalman filter 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:
%
%   priormean: mean of starting point of target
%   priorvar: variance of 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
%
% Output:
%
%   state: posterior mean of state
%   startvar: posterior variances of states
%
% *************************************************************************

state = priormean;
statevar = diag(priorvar);
postmean = priormean;
postvar = priorvar;
obs = size(obspath,2);

for i = 1:obs
    priormean = G * postmean;
    priorvar = G * postvar * G' + H * accelsigma * H';
    kalman = (priorvar * F') / (meassigma + F * priorvar * F');
    postmean = priormean + kalman * (obspath(:,i) - F * priormean);
    postvar = priorvar - kalman * F * priorvar;
    state = [state postmean];
    statevar = [statevar diag(postvar)];
end