Download All

Code covered by the BSD License  

Highlights from
Particle filter and PCRB for terrain-aided navigation

image thumbnail
from Particle filter and PCRB for terrain-aided navigation by Sebastien PARIS
Demo for terrain-aided navigation by particle filter

generate_traj(X1 , Q1 , F , U , Q); 
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          Sbastien PARIS (sebastien.paris@lsis.org) (5/4/08)
%  -------



[d , K]   = size(U);

K         = K + 1;

X         = zeros(d , K);

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

X(: , 1)  = X1;


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

Contact us at files@mathworks.com