粒子滤波代码与卡尔曼做比较

这是用于目标跟踪的粒子滤波代码， 用matlab编写的，很有借鉴性，一维情况下， 非高斯非线性，其中将扩展卡尔曼滤波与粒子滤波进行比较，更好的说明了粒子滤波的优越性

function ParticleEx1

% Particle filter example adapted from Gordon Salmond and Smith paper.

x = 0.1; % initial state
Q = 1; % process noise covariance
R = 1; % measurement noise covariance
tf = 50; % simulation length

N = 100; % number of particles in the particle filter

xhat = x;
P = 2;
xhatPart = x;

% Initialize the particle filter.
for i = 1 : N
    xpart（i） = x + sqrt（P） * randn;
end

xArr = [x];
yArr = [x^2 / 20 + sqrt（R） * randn];
xhatArr = [x];
PArr = [P];
xhatPartArr = [xhatPart];

close all;

for k = 1 : tf
    % System simulation
    x = 0.5 * x + 25 * x / （1 + x^2） + 8 * cos（1.2*（k-1）） + sqrt（Q） * randn;%状态方程
    y = x^2 / 20 + sqrt（R） * randn;%观测方程
    % Extended Kalman filter
    F = 0.5 + 25 * （1 - xhat^2） / （1 + xhat^2）^2;
    P = F * P * F‘ + Q;
    H = xhat / 10;
    K = P * H‘ * （H * P * H‘ + R）^（-1）;
    xhat = 0.5 * xhat + 25 * xhat / （1 + xhat^2） + 8 * cos（1.2*（k-1））;%预测
    xhat = xhat + K * （y - xhat^2 / 20）;%更新
    P = （1 - K * H） * P;
    % Particle filter
    for i = 1 : N
        xpartminus（i） = 0.5 * xpart（i） + 25 * xpart（i） / （1 + xpart（i）^2） + 8 * cos（1.2*（k-1）） + sqrt（Q） * randn;
        ypart = xpartminus（i）^2 / 20;
        vhat = y - ypart;%观测和预测的差
        q（i） = （1 / sqrt（R） / sqrt（2*pi）） * exp（-vhat^2 / 2 / R）;
    end
    % Normalize the likelihood of each a priori estimate.
    qsum = sum（q）;
    for i = 1 : N
        q（i） = q（i） / qsum;%归一化权重
    end
    % Resample.
    for i = 1 : N
        u = rand; % uniform random number between 0 and 1
        qtempsum = 0;
        for j = 1 : N
            qtempsum = qtempsum + q（j）;
            if qtempsum >= u
                xpart（i） = xpartminus（j）;
                break;
            end
        end
    end
    % The particle filter estimate is the mean of the particles.
    xhatPart = mean（xpart）;
    % Plot the estimated pdf‘s at a specific time.
    if k == 20
        % Particle filter pdf
        pdf = zeros（811）;
        for m = -40 : 40
            for i = 1 : N
                if （m <= xpart（i）） && （xpart（i） < m+1）
                    pdf（m+41） = pdf（m+41） + 1;
                end
            end
        end
        figure;
        m = -40 : 40;
        plot（m pdf / N ‘r‘）;
        hold;
        title（‘Estimated pdf at k=20‘）;
        disp（[‘min max xpart（i） at k = 20: ‘ num2str（min（xpart）） ‘ ‘ num2str（max（xpart））]）;
        % Kalman filter pdf
        pdf = （1 / sqrt（P） / sqrt（2*pi）） .* exp（-（m - xhat）.^2 / 2 / P）;
        plot（m pdf ‘b‘）;
        legend（‘Particle filter‘ ‘Kalman filter‘）;
    end
    % Save data in arrays for later plotting
    xArr = [xArr x];
    yArr = [yArr y];
    xhatArr = [xhatArr xhat];
    PArr = [PArr P];
    xhatPartArr = [xhatPartArr xhatPart];
end

t = 0 : tf;

%figure;
%plot（t xArr）;
%ylabel（‘true state‘）;

figure;
plot（t xArr ‘b.‘ t xhatArr ‘k-‘ t xhatArr-2*sqrt（PArr） ‘r:‘ t xhatArr+2*sqrt（PArr）

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       3636  2007-01-14 17:09  粒子滤波 目标跟踪 一维情况下 非线性非高斯\ParticleEx1.m

     文件      28672  2008-07-04 10:52  粒子滤波 目标跟踪 一维情况下 非线性非高斯\Doc1.doc

     目录          0  2008-07-04 09:50  粒子滤波 目标跟踪 一维情况下 非线性非高斯

----------- ---------  ---------- -----  ----

                32526                    4