Particle Filter

Predefined Distribution

p( x_k | x_k-1 )  : dynamic distribution

p( y_k | x_k )    : observation distribution

Parameter

P : the number of particle

Algorithm

1. P 개의 particle을 dynamic model, p( x_k | x_k-1{L} ) ,에서 draw한다.

   particle은 x_k{L}에 저장된다.

2. (k번째 observation이 들어오면) observation model, p( y_k | x_k{L} ) ,에서 weight를 update한다.

   weight update: what_k{L} = what_k-1{L}*p( y_k | x_k{L} )

3. weight들을 normalize한다.

4. particle의 effective number를 계산한다.

   N_eff = 1/(sum(w_k));

5. Neff < Nthr 이면, 새롭게 resample한다.

   a) 현재 weight들의 distribution에서 P개의 particle x_k{L}를 뽑는다.

   b) weight는 1/P로 한다.

결과

[Results]

 measurement error: 2.359, estimation error: 2.000 

소스 코드 

1. main_2d_track_particle.m

%% Particle Filter

clc

clear all

close all

addpath '../Solvers/sjGP_lib/'

addpath 'particle_filter\\'

rand('seed', 1);

randn('seed', 1);

% 2. dynamic_distribution과 observation distribution을 설정한다.

dynamic_distribution     = @pf_dyn_dist_2d_track;

observation_distribution = @pf_obs_dist_2d_track;

%% Particle Filter를 초기화 한다. 

do_pf_initialization_2d_track;

%%

% +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ %

%                                   particle filter 의 실제 구현                                   %

% +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ %

fprintf(1, '4. Lets start particle filter \n');

% 처음 sample을 정의한다.

for particle_idx = 1:pf.nr_particle

    % 처음 particle의 x와 y는 stage에 random하게 뿌린다.

    pf.particle{1}.p{particle_idx}.x(1) = pf.stage.xmin+pf.stage.xlen*rand();

    pf.particle{1}.p{particle_idx}.x(3) = pf.stage.ymin+pf.stage.ylen*rand();

end

pf.z.est = [pf.y.ref_traj(1, :)];

for k = 2:pf.T

    

    % 1. P 개의 particle을 dynamic model, p( x_k | x_k-1{L} ) ,에서 draw한다.

    for particle_idx = 1:pf.nr_particle

        [dummy_var, x_next, x_var_next] = feval(dynamic_distribution ...

            , [], pf.particle{k-1}.p{particle_idx}.x, pf_option);

        pf.particle{k}.p{particle_idx}.x = x_next + sqrt(x_var_next)*randn(4, 1);

    end

    

    % 2. (k번째 observation이 들어오면) observation model, p( y_k | x_k{L} ) ,에서 weight를 update한다.

    %    weight update: what_k{L} = what_k-1{L}*p( y_k | x_k{L} )

    current_observation   = pf.y.meas_traj(k, :)';

    current_real_position = pf.y.ref_traj(k, :)';

    sum_weight = 0;

    for particle_idx = 1:pf.nr_particle

        % p(y_k|x_k)를 계산한다. 

        [y_prob, y_next, y_var_next] = feval(observation_distribution ...

            , current_observation, pf.particle{k}.p{particle_idx}.x, pf_option);

        

        % particle의 weight를 update한다. 

        pf.particle{k}.p{particle_idx}.w ...

            = pf.particle{k}.p{particle_idx}.w*y_prob;

        sum_weight = sum_weight + pf.particle{k}.p{particle_idx}.w;

    end

    

    % 3. weight들을 normalize한다.

    sum_normalized_weight_square = 0;

    weights = zeros(pf.nr_particle, 1);

    for particle_idx = 1:pf.nr_particle

        pf.particle{k}.p{particle_idx}.w = pf.particle{k}.p{particle_idx}.w/sum_weight;

        sum_normalized_weight_square = sum_normalized_weight_square ...

            + (pf.particle{k}.p{particle_idx}.w)^2;

        weights(particle_idx) = pf.particle{k}.p{particle_idx}.w;

    end

    

    % 4. particle의 effective number를 계산한다.

    N_eff = 1/sum_normalized_weight_square;

    

    % 5. Neff < Nthr 이면, 새롭게 resample한다. (이거 말고 weight 작은 놈들만 뽑자.)

    N_thr = pf.nr_particle/3;

    % a) 현재 weight들의 distribution에서 P개의 particle x_k{L}를 뽑는다.

    % b) weight는 1/P로 한다.

    

    % 5. weight가 작은 놈들을 다시 뽑는게 아니라 좋은 놈들로 덮어 씌운다.?

    if N_eff < N_thr

        if disp_pf_iteration

            fprintf(' do resampling (%.1f < %.1f) \n' ...

                , N_eff, N_thr);

        end

        good_index = resampleMultinomial(weights);

        for particle_idx = 1:pf.nr_particle

            cur_good_index = good_index(particle_idx);

            new_particle = pf.particle{k}.p{cur_good_index}.x;

            pf.particle{k}.p{particle_idx}.x = new_particle;

            

            % 모든 weight는 초기화 시킨다? 그냥 원래 weight로 하자. 이렇게 하면 normalize는 안됨. (어차피 뒤에서 다시하자나?)

            % pf.particle{k}.p{particle_idx}.w = 1/pf.nr_particle;

            pf.particle{k}.p{particle_idx}.w = pf.particle{k}.p{cur_good_index}.w;

        end

    end

    

    % 6. weight 중에서 상위 몇 개만 골라서 그놈들로 mean을 취한 것을 estimate으로 한다.

    z_estimate = zeros(1, 2);

    sum_weight = 0;

    for particle_idx = 1:pf.nr_particle

        curr_weight = pf.particle{k}.p{particle_idx}.w;

        if curr_weight >= 0 % 일단은 모든 sample을 이용해서

            z_estimate = z_estimate ...

                + pf.particle{k}.p{particle_idx}.x([1 3])'*curr_weight;

            sum_weight = sum_weight ...

                + curr_weight;

        end

    end

    z_estimate = z_estimate / sum_weight;

    pf.z.est = [pf.z.est ; z_estimate];

    

    % 99. plot한다.

    title_str = sprintf('%d/%d #effective particle: %.2f/%d', k, pf.T, N_eff, pf.nr_particle );

    if plot_pf_process_detail

        figure(99); clf; hold on;

        for particle_idx = 1:pf.nr_particle

            cur_particle = pf.particle{k}.p{particle_idx}.x;

            h1 = plot( cur_particle(1), cur_particle(3), 'go' );

            str = sprintf(' [%d] %.4f', particle_idx, pf.particle{k}.p{particle_idx}.w);

            text(  cur_particle(1), cur_particle(3), str);

        end 

        h2 = plot( current_observation(1), current_observation(2), 'ro');

        h3 = plot( z_estimate(1), z_estimate(2), 'bx');

        h4 = plot( current_real_position(1), current_real_position(2), 'ko');

        mn = 0.5; axis([pf.stage.xmin-mn pf.stage.xmax+mn pf.stage.ymin-mn pf.stage.ymax+mn ]);

        legend([h1 h2 h3 h4], 'particles', 'observation', 'estimation', 'real'); hold off;

        

        title(title_str);

        drawnow;

    end

    if disp_pf_iteration, fprintf('%s \n', title_str); end;

    

end

%%  

% +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ %

%                            Analyze Results (plot and display)                                   %

% +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ %

% 결과를 분석한다.

meas_err = pf.y.meas_traj - pf.y.ref_traj;

est_err  = pf.z.est - pf.y.ref_traj; 

disp('[Results]');

fprintf(' measurement error: %.3f, estimation error: %.3f \n' ...

    , norm(meas_err), norm(est_err) );

% 최종 결과를 plot한다.

if plot_pf_process_final

    figure(98);

    hold on;

    h2 = plot( pf.y.meas_traj(:, 1), pf.y.meas_traj(:, 2), 'ro-');

    h3 = plot( pf.z.est(:, 1), pf.z.est(:, 2), 'bx-');

    h4 = plot( pf.y.ref_traj(:, 1), pf.y.ref_traj(:, 2), 'go-');

    hold off;

    mn = 0.5; axis([pf.stage.xmin-mn pf.stage.xmax+mn pf.stage.ymin-mn pf.stage.ymax+mn ]);

    legend([h2 h3 h4], 'observation', 'estimation', 'real'); hold off;    

end 

%%

2. do_pf_initialization_2d_track

% +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ %

%                     experimental setting (실험의 속도에 영향을 준다.)                             %

% +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ %

make_new_traj           = 0;  % 새로운 trajectory를 만든다. 

plot_new_traj           = 0;

make_new_gp             = 0;  % 새로운 Gaussian process field를 만든다. 

make_new_gp_meas        = 0;  % 새로운 Sensor measurement를 한다. 

plot_new_gp             = 0;

disp_pf_iteration       = 0;  % particle filter 내부의 iteration을 출력한다. 

plot_pf_process_detail  = 0;  % 각 단계에서 sample이 어떻게 뿌려지는지 확인한다. 

plot_pf_process_final   = 1;  % pf가 모두 끝난 후에 결과를 그림으로 확인한다. 

%%

% +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ %

%                                      gp parameter setting                                       %

% +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ %

fprintf(1, '1. Initializing stage and gp field \n');

% 1. gp realization, measurement에 사용될 parameter를 설정한다.

pf.gp_param   = set_simple_param();

% 2. 실험을 할 stage와 gp realized field를 설정한다.

pf.test_index = 1; 

pf.nr_meas    = 20;

[pf.field, pf.stage, pf.nr_meas] ...

    = make_gp_realization(pf.test_index, pf.nr_meas, pf.gp_param, make_new_gp);

[pf.ref, pf.meas, pf.pred, pf.nr_meas] ...

    = make_gp_measurements(pf.test_index, pf.nr_meas, pf.field, pf.stage, pf.gp_param, make_new_gp, plot_new_gp);

%%

% +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ %

%                                  particle filter를 초기화 한다.                                  %

% +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ %

fprintf(1, '2. Initializing particle filter \n');

pf.T            = 50;              % experiment duration

% 1. particle filter parameter

pf.nr_particle       = 1E2;              % particle의 수

pf.weight            = ones(pf.nr_particle, 1)/pf.nr_particle;   % Initialize weights

pf.sampleT           = .1;               % sampling period

pf_option.sigma2x    = .1;               % sample의 위치에 대한 variance

pf_option.sigma2v    = pf_option.sigma2x/pf.sampleT;   % sample의 속도에 대한 variance

pf_option.sigma2y    = 2E-1;             % 위치 measuremet에 대한 variance

pf_option.sigma2s    = 1E-1;             % 센서 measurement에 대한 variance

pf_option.sampling_T = pf.sampleT;

pf_option.gp_param   = pf.gp_param.realize;

pf_option.gp_data.nr_meas   = pf.nr_meas;

pf_option.gp_data.ref       = pf.ref;

pf_option.gp_data.meas      = pf.meas;

pf.traj              = cell(pf.T, 1);    % 여기에는 reference state를 담는다.

pf.particle          = cell(pf.T, 1);

for time_index = 1:pf.T

    pf.particle{time_index}.p = cell(pf.nr_particle, 1);

    for particle_index = 1:pf.nr_particle

        % state의 dimension은 4이다. [pos_x vel_x pos_y  vel_y]'

        pf.particle{time_index}.p{particle_index}.x = zeros(4, 1);

        % 처음 weight는 uniform하게 준다.

        pf.particle{time_index}.p{particle_index}.w = 1/pf.nr_particle;

    end

end

% 3. initialize trajectory @ t=1

if make_new_traj

    fprintf(1, '3. Initializing trajectory \n');

    vel             = 0.2/pf.sampleT;   % <= 이동 속도는 fix시키자.

    angle           = 2*pi*rand();      % <= 각도는 조금씩 변화시키자.

    % traj의 처음 point는 랜덤하게 준다.

    pf.traj{1}.x_ref   = [pf.stage.xmin+pf.stage.xlen*rand() vel*cos(angle) ...

        pf.stage.ymin+pf.stage.ylen*rand() vel*sin(angle) ]';

    % y.ref는 position을 의미한다.

    pf.y.ref_traj  = [pf.traj{1}.x_ref(1) pf.traj{1}.x_ref(3)];

    for ti = 2:pf.T

        angle = atan2( pf.traj{ti-1}.x_ref(4), pf.traj{ti-1}.x_ref(2) );

        

        % 주어진 stage 영역 내에 있도록 한다.

        while 1

            angle = angle + 10*pi/180*rand();   % 각도를 최대 5도 까지 변화

            angle = mod(angle, 2*pi);           % 0~2pi 사이로 normalize

            pf.traj{ti-1}.x_ref(2) = vel*cos(angle);

            pf.traj{ti-1}.x_ref(4) = vel*sin(angle);

            % update를 한다.

            [dummy_var, x_next] = feval(dynamic_distribution ...

                , [], pf.traj{ti-1}.x_ref, pf_option);

            pf.traj{ti}.x_ref = x_next;

            % 주어진 영역 내에 있는지 확인한다.

            idx = inpolygon( x_next(1), x_next(3) ...

                , pf.stage.boundary(:, 1)'  ...

                , pf.stage.boundary(:, 2)' );

            if idx == 1, break; end;

        end

        

        % 영역 내에 있는 trajectory를 저장한다.

        pf.y.ref_traj = [pf.y.ref_traj ; pf.traj{ti}.x_ref(1) pf.traj{ti}.x_ref(3)];

    end

    

    % measurement를 ref를 바탕으로 만든다.

    pf.sigma2meas_pos = 0.1;   % <== 위치 측정 에러

    pf.y.meas_traj        = pf.y.ref_traj + sqrt(pf.sigma2meas_pos)*randn(size(pf.y.ref_traj));

    

    trajectory = pf.y;

    save(['data/Trajectory_' num2str(pf.test_index) '.mat'], 'trajectory');

else

    fprintf(1, '3. Loading trajectory \n');

    load(['data/Trajectory_' num2str(pf.test_index) '.mat']);

    pf.y = trajectory;

end 

% 4. 추가로 각 measurement에서 센서 정보를 읽어온다. 

pf.sigma2meas_sensor = 1E-4;   % <== 센서 측정 에러 

pf.y.meas_sensor     = zeros(pf.T, 1);

pf.y.ref_sensor      = zeros(pf.T, 1);

for ti = 1:pf.T

    cur_pos          = pf.y.ref_traj(ti, :);

    pf.y.ref_sensor(ti)  = interp2(pf.field.xgrid, ...

            flipud(pf.field.ygrid), pf.field.zdata, ...

            cur_pos(1), cur_pos(2) );

    pf.y.meas_sensor(ti) = pf.y.ref_sensor(ti) + sqrt(pf.sigma2meas_sensor)*randn();

end

% reference 와 measurement trajectory를 plot한다.

if plot_new_traj

    href = plot(pf.y.ref_traj(:, 1), pf.y.ref_traj(:, 2), 'ro-');

    hold on;

    hmeas = plot(pf.y.meas_traj(:, 1), pf.y.meas_traj(:, 2), 'bo-');

    hold off;

    legend([href hmeas], 'reference', 'measurement');

    mn = 0.5; % stage의 margin

    axis([pf.stage.xmin-mn pf.stage.xmax+mn pf.stage.ymin-mn pf.stage.ymax+mn ]);

end

3. pf_dyn_dist_2d_track.m

%% particle filter dynamic distribution for 2d tracking

function [prob mu_x_k var_x_k] ...

    = pf_dyn_dist_2d_track(x_k, x_km1, option)

%% arguments

%

% x_k               : k번째 state x_{k}

% x_km1             : k-1번째 state x_{k-1}

% option.sigma2x    : dynamic model의 에러의 공분산을 의미한다. 

% option.sampling_T : sampling time을 의미한다. 

% 

%% etc

%

% state x는 [pos.x, vel.x, pos.y, vel.y]'으로 가정한다. 

% 

%% inside 

sampling_T  = option.sampling_T;

sigma2x     = option.sigma2x;

sigma2v     = option.sigma2v;

dim_x       = size(x_km1, 1);

mu_x_k      = f(x_km1, sampling_T);

%var_x_k     = sigma2x*eye(dim_x, dim_x);

var_x_k     = diag([sigma2x sigma2v sigma2x sigma2v]);

if ~isempty(x_k)

    prob = mvnpdf(x_k , mu_x_k , var_x_k);

else

    prob = 0;

end

function x_k = f(x_km1, sampling_T)

%% dynamic model이다. 

%

% state x는 [pos.x, vel.x, pos.y, vel.y]'으로 가정한다. 

% 이때 discretize된 A_D는 

% [ 1 T 0 0 ;

%   0 1 0 0 ; 

%   0 0 1 T ; 

%   0 0 0 1 ] 이다. 

%

T   = sampling_T;

A_D = [1 T 0 0 ; 0 1 0 0 ; 0 0 1 T ; 0 0 0 1];

x_k = A_D*x_km1;

4. pf_obs_dist_2d_track.m

%% particle filter observation distribution for 2d tracking

function [prob mu_y_k var_y_k] ...

    = pf_obs_dist_2d_track(y_k, x_k, option)

%% arguments

%

% x_k            : k번째 state x_{k}

% x_k            : k번째 measurement y_{k}

% option.sigma2y : observation model의 에러의 공분산을 의미한다. 

%

%% etc

%

% state x는 [pos.x, vel.x, pos.y, vel.y]'으로 가정한다. 

% obs   y는 [pos.x pos.y]'를 가정한다.  

%

%% inside

sigma2y     = option.sigma2y;

dim_x       = size(x_k, 1);

dim_y       = size(y_k, 1);

mu_y_k      = h(x_k);

var_y_k     = sigma2y*eye(dim_y, dim_y);

prob        = mvnpdf(y_k , mu_y_k , var_y_k);

function y_k = h(x_k)

%% observation model이다. 

%

% state x는 [pos.x, vel.x, pos.y, vel.y]'으로 가정한다. 

% obs   y는 [pos.x, pos.y]'를 가정한다. 

% 

C_D = [1 0 0 0 ; 0 0 1 0 ];

y_k = C_D*x_k;

5. resampleMultinomial.m

function [ indx ] = resampleMultinomial( w )

M = length(w);

Q = cumsum(w);

Q(M)=1; % Just in case...

i=1;

while (i<=M),

    sampl = rand(1,1);  % (0,1]

    j=1;

    while (Q(j)<sampl),

        j=j+1;

    end;

    indx(i)=j;

    i=i+1;

end

그외 필요한 것들

data.zip

sjGP_lib.zip