卡爾曼濾波與粒子濾波比較

兩種濾波的重要性不必贅述，在自動控制領域的無異於牛頓的三大定律，都是貝葉斯濾波的不同表述和推廣。

對卡爾曼濾波，[1]是狀態估計的經典教材，它的第一章從簡單的例子出發，深入淺出地解釋什麼是濾波，爲什麼卡爾曼濾波是最優的；粒子濾波強烈推薦[3]， 這篇論文詳細的介紹了例子濾波的發展歷程和各類變種，最有用的是有人根據這篇論文實現了代碼[3]；[4]是博士論文的節選，從編程的角度介紹了粒子濾波如何應用到移動機器人定位。

採用簡單的線性模型，[ x vx y vy]'爲狀態變量，觀測量是位置[ x y ]'，跟蹤位置x的結果如下，注意到KF要比PF的跟蹤效果好（PF的RMS大概是KF的四倍），這是因爲從誤差最小的原則來講，Kalman Filter是最優的。

源代碼：

Kalman Filter

%% Lucas Wang, XJTU, 2014.09.18, and I appreciate for the protype code offering by my girl friend, I love her very much~
clc;clear all;close all;
%% parameter of state equation
T = 0.01;
F=[1 T 0 0;
    0 1 0 0;
    0 0 1 T;
    0 0 0 1];
G=[T^2/2 0;
    T 0;
    0 T^2/2;
    0 T];
Q = diag( [0.04 0.09] );
%parameter of measurement equation
H=[1 0 0 0;
    0 0 1 0];
R=diag([900 900]);

% Initialization of state estimators
x0=[5 20 6 18]';
% P0=diag( [400 64 625 49] );
L = 30;
P0 = diag( [ L L L L ]);
StateDim = size( F, 1 );
ObvDim   = size( H, 1 );
% reset the random number generator to a different state each time
randn('state',sum(100*clock));


% %% Kalman Filter.
x_True    = [ x0 ];
x_Noise   = [ x0 ];
x_Predict = [ x0 ];
x_Est     = [ x0 ];
z_True    = [];
z_Noise   = [];
P_Predict = [];
P_Est( :, :, 1 ) = P0;
MaxIter = 1000;
for k = 2 : 1 : MaxIter
    % ground truth.
    x_True( :, k ) = F * x_True( :, k - 1 ); 
    Z_True( :, k ) = H * x_True(:, k );
    % Add noise.
    Wk = G * sqrtm(Q) * randn( 2, 1 );
    x_Noise( :, k ) = F * x_Noise( :, k - 1 ) + Wk; 
    Vk = sqrtm(R)*randn( 2, 1 );
    z_Noise( :, k ) = H * x_Noise( :, k ) + Vk;
    % Predicate.
    x_Predict( :, k ) = F * x_Est( :, k - 1 );
    P_Predict( :, :, k ) = F * P_Est( :, :, k - 1 ) * F' + G * Q * G';
    % Update.
    y(:, k ) = z_Noise( :, k ) - H * x_Predict( :, k );
    S(:, :, k ) = H * P_Predict( :, :, k ) * H' + R;
    K( :, :, k ) = P_Predict( :, :, k ) * H' * inv( S( :, :, k ) );
    x_Est(:, k ) = x_Predict( :, k ) + K( :, :, k ) * y( :, k );
    P_Est( :, :, k ) = ( eye(StateDim) - K( :, :, k ) * H ) * P_Predict( :, :, k );
end
RMS = mean( ( x_Est - x_True ).^2, 2 )
%% Visualize.
figure(2);
plot( x_True( 1, : ), 'b*' );
hold on;plot( x_Est( 1, : ), 'g*' );
hold on; plot( z_Noise( 1, : ), 'r*' );
legend('True State', 'Optimal State', 'Measurement' );
title( ' Kalman Filter For Linear Motion( Courtesy of JJH ) ');
xlabel('Time(s)');ylabel( 'Position(m)' );
grid on;

Particle Filter, 其中的particle_filter()取自[3].

%% Lucas Wang, XJTU, 2014.09.18
clc;clear all;close all;
% parameter of state equation
T = 0.01;
F=[1 T 0 0;
    0 1 0 0;
    0 0 1 T;
    0 0 0 1];
G=[T^2/2 0;
    T 0;
    0 T^2/2;
    0 T];
Q = diag( [0.04 0.09] );
%parameter of measurement equation
H=[1 0 0 0;
    0 0 1 0];
R=diag([900 900]);

% Initialization of state estimators
x0=[5 20 6 18]';
% P0=diag( [400 64 625 49] );
L = 30;
P0 = diag( [ L L L L ]);
StateDim = size( F, 1 );
ObvDim   = size( H, 1 );
% %% Kalman Filter.
x_True    = [ x0 ];
x_Noise   = [ x0 ];
z_True    = [];
z_Noise   = [];
%% Particle Filter.
TotalTime = 1000;
nx = StateDim;
% function description.
sys = @(k, xkm1, uk) F * xkm1 + uk;
obs = @(k, zkm1, vk) H * zkm1 + vk;
p_yk_given_xk = @( k, yk, xk ) mvnpdf( yk - H * xk, zeros( ObvDim, 1 ), R ); % associate with weight calculate.
gen_x0 = @(x) mvnrnd( x0, P0 );
gen_sys_noise = @(u) ( mvnrnd( zeros( StateDim, 1 ), G * Q * G' ) )';
gen_obs_noise = @(u) ( mvnrnd( zeros( ObvDim,   1 ), R ) )';
% initialization of particle filter.
pf.k               = 1;                   % initial iteration number
pf.Ns              = 100;                 % number of particles
pf.w               = zeros(pf.Ns, TotalTime);     % weights
pf.particles       = zeros( nx, pf.Ns, TotalTime ); % particles
pf.gen_x0          = gen_x0;              % function for sampling from initial pdf p_x0
pf.p_yk_given_xk   = p_yk_given_xk;       % function of the observation likelihood PDF p(y[k] | x[k])
pf.gen_sys_noise   = gen_sys_noise;       % function for generating system noise

% Estimate state
for k = 2:TotalTime
   fprintf( 'Iteration = %d/%d\n',k, TotalTime ); 
    x_True( :, k ) = sys( k, x_True(:, k - 1 ), 0 ); 
    Z_True( :, k ) = obs( k, x_True(:, k - 1 ), 0 );
    % Add noise.
    Wk = gen_sys_noise();
    x_Noise( :, k ) = sys( k, x_True(:, k - 1 ), Wk ); 
    Vk = gen_obs_noise();
    z_Noise( :, k ) = obs( k, x_True(:, k - 1 ), Vk );
   % state estimation
   pf.k = k;
   %[xh(:,k), pf] = particle_filter(sys, y(:,k), pf, 'multinomial_resampling');
   [xh(:,k), pf] = particle_filter( sys, z_Noise( :, k ), pf, 'systematic_resampling' );   
 
   % filtered observation
   yh(:,k) = obs(k, xh(:,k), 0);
end
RMS = mean( ( xh - x_True ).^2, 2 )

figure;
plot( x_True( 1, : ), 'b*' );
hold on;plot( xh( 1, : ), 'g*' );
hold on; plot( z_Noise( 1, : ), 'r*' );
legend('True State', 'Optimal State', 'Measurement' );
title( 'Particle Filter For Linear Motion' );xlabel('Time(s)'); ylabel('Position(m)');grid on;

參考文獻：

[1] Maybeck, Stochastic Models, Estimation, and Control.

[2] M. Sanjeev Arulampalam, A turtorial on Particle Filters for online Nonlinear/Non-Gaussian Bayesian Tracking[J], IEEE Transactions on Signal Processing, 2002.

[3] 點擊打開鏈接

[4] Ioannis M. Rekleitis, A Particle Filter Tutorial for Mobile Robot Localization, Technical Report of Centre for Intelligent Machines, McGill University, 2004.

ps: 特別感謝韓姑娘在卡爾曼濾波上對我的幫助，和她的討論讓我對KF的理解更加深刻，而且Kalman Filter部分的代碼也是她提供的。