
% *************************************************************************
%
% FILE: genpath.m
%
% Date Created: April 6, 2004
% Author: Mark E. Irwin
%
% Contents: Generates true and observed path for linear dynamic model
%
% Revision History
% Date          Name            Changes/Reasons
%
% *************************************************************************
%
% Input:
%
%  None
%
% Output:
%
%   None, but many variables are created
%
% *************************************************************************

F = [1 0 0 0; 0 1 0 0; 1 0 0 0; 0 1 0 0];
G = eye(4);
G(1,3) = 1;
G(2,4) = 1;
H = [0.5 0; 0 0.5; 1 0; 0 1];

accelsigma = 0.003 *eye(2);
meassigma = 0.03 * eye(4);
meassigma(3:4,3:4) = 0.04 * eye(2);
meassigma(3,4) = 0.04 * 0.2;
meassigma(4,3) = 0.04  * 0.2;

state = [0 0 1 1]';
accel = [0; 0];
obspos = zeros(4,1);

for i = 1:500
    % update true state
    newaccel = mvnrnd([0 0], accelsigma)';
    newstate = G * state(:,i) + H * newaccel;
    accel = [accel newaccel];
    state = [state newstate];
    
    % update measured position
    newpos = F * newstate + mvnrnd([0 0 0 0],meassigma)';
    obspos = [obspos newpos];
end
    
    