bot_asir.m

bearing only tarcking based sampleing importance resampling (SIR) filter

% particle for bearing only tracking example
% Refer to the following reference:
% N.J.Gordon, D.J. Saimond, A.F.M. Smith, "Novel approach to
% nonlinear/ non-Gaussian Bayesian state estimation", IEEE proceedings-F,
% Vol. 140, No. 2, April 1993

% state equation: X(k)=phi*X(k-1)+ta*W(k)
% phi=([1 1 0 0; 0 1 0 0; 0 0 1 1; 0 0 0 1]); ta=([0.5 0; 1 0; 0 0.5; 0 1])
% X(k)=([x, dx/dt, y, dy/dt]'); W(k)=([wx, wy]');
% Convariance of system noise:
% Q=q*[1 0; 0 1]=0.001^2*[1 0; 0 1]; q=0.001^2;

% observe equation: z(k)=arctan(y/x)+v(k)
% convarance of observe noise: E[v(k) v(j)]=r*delt(k,j)=0.005^2*delt(k,j)

% the actual initial state is:
% x(1)=([-0.05 0.001 0.7 -0.055]');

% the prior distribution of initial state:
% x_bar(1)=([0.0 0.0 0.4 -0.05]');
% the convariance matrix is:
% M(1)=([0.5 0 0 0; 0 0.005 0 0; 0 0 0.3 0; 0 0 0 0.01])
% clear;
clc;
run_times=24;
particle_num=4000;

phi=[1 1 0 0; 0 1 0 0; 0 0 1 1; 0 0 0 1];
ta=[0.5 0; 1 0; 0 0.5; 0 1];
sqrt_q=0.001;
sqrt_r=0.005;

% initial condition
X(1:4,1)=[-0.05 0.001 0.7 -0.055]';

% generate the state equation
for i=2:run_times
    X(1:4,i)=phi*X(1:4, i-1)+ta*sqrt_q*[randn randn]';
end
% generate the observe equation
for i=1:run_times
    z(i)=atan(X(3,i)/X(1,i))+sqrt_r*randn;
end

%plot the figure 5 in the paper
figure; plot(X(1,:), X(3,:), '-^'); hold on; plot(0, 0, 'r*');

% generating particles
x1_bar=[0.0 0.0 0.4 -0.05]';
M=[0.1 0 0 0; 0 0.005 0 0; 0 0 0.1 0; 0 0 0 0.01];
for i=1:particle_num
    particle(1:4, i)=x1_bar+M*[randn randn randn randn]';
end
%plot the generated particles
%figure; plot(particle(1,:), particle(3,:), '^');

% begin filtering
for i=1:run_times
    % particle generation (sampling step)
    for j=1: particle_num
        new_particle(1:4, j)=phi*particle(1:4,j)+ta*sqrt_q*[randn randn]';
    end
   
    % calculate the weight
    for j=1:particle_num
        weight(j)= 1/(sqrt_r*sqrt(2*pi))*exp(-(z(i)-atan(new_particle(3,j)./new_particle(1,j))).^2/(2*sqrt_r^2));
    end
   
    % normalize the weight
    weight=weight/sum(weight);
    
    % generate the state estimation
    est(:, i)=new_particle*weight';
    
    % resampling
    cdf = cumsum(weight);
    u1 = 1/particle_num*rand(1);
    j=1;
    for k=1:particle_num
    u(k) = u1+1/particle_num * (k-1);
    while u(k)>cdf(j)
        j=j+1;
    end
    particle(:, k) = new_particle(:, j);
    end
    weight = ones(1,particle_num)/particle_num;
    % end of resampling
    
end

plot(est(1,:),est(3,:),'-pr')