generate_traj.m

本文件采用Matlab软件

function X = generate_traj(X1 , Q1 , F , U , Q);

%
% X         = generate_traj(X1 , Q1 , F , U , Q);
%
% Dynamic system
% 
% X_1       = N(X1 , Q1)
% X_{k+1}   = FkXk + Uk + N(0 , Qk)
% Zk        = h(Xk) + N(0 , Rk(Xk))
%
%
%  Author          S閎astien PARIS ([e-mail]) (5/4/08)
%  -------



[d , K]   = size(U);

K         = K + 1;

X         = zeros(d , K);

X(: , 1)  = X1  + chol(Q1)'*randn(d , 1 );

for t = 1 : K - 1 
        
    X(: , t + 1)          = F(: , : , t)*X(: , t) + U(: , t) + chol(Q(: , : , t))'*randn(d , 1 ); 
    
end