【滤波跟踪】基于粒子、sigma和卡尔曼滤波器实现目标跟踪滤波附matlab代码

✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。

🍎个人主页：Matlab科研工作室

🍊个人信条：格物致知。

⛄ 内容介绍

粒子滤波器是一种也称序列蒙特卡洛采样的近似贝叶斯滤波器,其具有不受目标系统是否线性,噪声系统是否高斯分布等限制的特性,能够对目标系统进行最小方差估计,广泛运用在目标跟踪,自动控制和参数估计等领域.而具有线性递推特性的卡尔曼滤波在线性系统中根据前一时刻的目标状态和当前时刻的观测量,来获得当前时刻状态的最优估计,具有无偏,稳定和最优的特点.本文通过建立在目标系统中的状态转移方程和目标观测方程,运用粒子滤波和卡尔曼滤波对在仿真环境下的汽车的行驶轨迹进行跟踪预测,并在MATLAB软件环境下建模并仿真验证:通过对比两种滤波器的估计误差.

⛄ 部分代码

%% Particle Filter Demonstration

%

% This script demonstrates using a particle filter to determine the

% position and velocity of a bouncing ball for the article "How Kalman

% Filters Work".

%

% <http://www.anuncommonlab.com/articles/how-kalman-filters-work/>

%

% Copyright 2016 Tucker McClure @ An Uncommon Lab

%% Set up and start the figure.

% Set the random number generator seed so the results are the same every

% time we run the script. (Comment out this line to see different results

% every time.)

rng(1);

% Initial true state, measurement noise covariance, and measurement

x0 = [0; 3; 1; 0];

R  = 0.25^2 * eye(2);

z0 = x0(1:2) + covdraw(R);

% Initial state estimate and covariance. The initial particles will be

% centered on this initial estimate and distributed according to the

% covariance. The initial particle weights are all equal (1/N).

xh0 = [z0; 1; 0];

P0  = blkdiag(R, 2^2 * eye(2));

nX  = 100;

X   = bsxfun(@plus, covdraw(P0, nX), xh0);

w   = 1/nX * ones(1, nX);

% Calculate the whole true trajectory.

[~, x, t] = propagate_ball(0, 10, x0);

% Create a function to calculate the colors for the particles according to

% their weights. This looks pretty good.

colors = @(w) 1 - max(0.95*w/max(w).^0.85, 0.05);

% Prepare the figure.

set(clf(figure(1)), 'Color', [1 1 1]);

colormap('gray');

axis equal;

axis([-1 11 0 5]);

caxis([0 1]);

xlabel('Right [m]');

ylabel('Up [m]');

hold on;

% Add the initial particles.

hX = scatter(X(1,:), X(2,:), [], colors(w), '.');

% Measurement

hz = plot(z0(1), z0(2), 'o', 'Color', [0.85 0.325 0.098]);

% Start the legend.

legend([hz hX], 'Measurement', 'Particles');

% colorbar();

%% Propagate the truth and draw the measurement.

% Manually propagate the truth and take a noisy measurement.

tk = 1;

xk = propagate_ball(0, tk, x0);

zk = xk(1:2) + covdraw(R);

set(hz, 'XData', [get(hz, 'XData'), zk(1)], ...

        'YData', [get(hz, 'YData'), zk(2)]);

%% Propagate the particles, drawing their trajectories.

% Get trajectories for each ball.

xt = cell(1, nX);

Xn = zeros(size(X));

ht = zeros(1, nX);

line_colors = colors(w).' * [1 1 1];

for k = 1:nX

    [Xn(:,k), xt{k}] = propagate_ball(0, 1, X(:,k));

    ht(k) = plot(xt{k}(1,:), xt{k}(2,:), 'Color', line_colors(k,:));

end

set(hX, 'XData', Xn(1,:), 'YData', Xn(2,:), 'CData', colors(w));

uistack(hz, 'top');

%% Show distance to each particle.

hd = zeros(1, nX);

for k = 1:nX

    hd(k) = plot([xt{k}(1,end) zk(1)], [xt{k}(2,end), zk(2)], ...

                 'Color', [0.85 0.325 0.098]);

end

%% Use the bootstrap filter to update the weights.

% Get rid of the error lines.

delete(hd);

% Create the propagation and observation functions.

f = @propagate_ball;

h = @(t, x, u) x(1:2);

% Create a function to make a random process noise draw. Since we don't use

% process noise for the bouncing ball, this is pretty easy:

d = @(varargin) [];

% Create a function to determine how likely a measurement error is. Since

% our measurement errors are Gaussian, we'll use the probability density

% function for a multivariate Gaussian distribution. (Note, since the

% "probabilities" of each particle are only important relative to each

% other, we can drop the constant scaling factor of the PDF.)

invR = inv(R);

p = @(t, dz, varargin) exp(-0.5 * dz.' * invR * dz); %#ok<MINV>

% Run the bootstrap filter for the first time step.

[xh, X, w, wt] = bf(0, tk, ...  % Last and current times

                    X, w, ...   % Last particles and weights

                    [], ...     % Input vector (we don't use one)

                    zk, ...     % Current measurement

                    f, h, ...   % Propagation and measurement functions

                    d, ...      % Function to draw random process noise

                    p, ...      % Fcn to determine prob. of meas. error

                    [], ...     % Tuning parameter (let it use a default)

                    false);     % Don't resample and regularize (see below)

% Update the particle plot.

set(hX, 'XData', X(1,:), 'YData', X(2,:), 'CData', colors(wt));

% Update the trajectories.

line_colors = colors(wt).' * [1 1 1];

for k = 1:nX

    set(ht(k), 'Color', line_colors(k,:));

end

[~, si] = sort(wt, 'descend');

uistack(ht(si), 'top');

%% Show weighted average.

hxh = scatter(xh(1), xh(2), 100, [0.25 0.75 0.25], 'o', 'filled');

legend([hz hX hxh], 'Measurement', 'Particles', 'Estimated State');

%% Propagate again.

% Propagate the truth and create a noisy measurement.

tkm1 = tk;

tk   = 2;

xk = propagate_ball(tkm1, tk, xk);

zk = xk(1:2) + covdraw(R);

% Create the little trajectories for each particle.

for k = 1:nX

    [Xn(:,k), xt{k}] = propagate_ball(tkm1, tk, X(:,k));

end

% Update the measurement.

set(hz, 'XData', [get(hz, 'XData'), zk(1)], ...

        'YData', [get(hz, 'YData'), zk(2)]);

% Update the particles.

set(hX, 'XData', Xn(1,:), 'YData', Xn(2,:), 'CData', colors(wt));

% Update their trajectories.

line_colors = colors(wt).' * [1 1 1];

for k = 1:nX

    set(ht(k), 'XData', xt{k}(1,:), 'YData', xt{k}(2,:), ...

        'Color', line_colors(k,:));

end

[~, si] = sort(wt, 'descend');

uistack(ht(si), 'top');

%% Update weights again.

% Just get the updated weights. We don't want the new particles, because

% we're going to show the resampled and regularized particles next.

[~, ~, ~, wt] = bf(tkm1, tk, X, w, [], zk, f, h, d, p, [], false);

% draw_particles(Xn, wt);

% Update the particles.

set(hX, 'CData', colors(wt));

% Update their trajectories.

line_colors = colors(wt).' * [1 1 1];

for k = 1:nX

    set(ht(k), 'Color', line_colors(k,:));

end

[~, si] = sort(wt, 'descend');

uistack(ht(si), 'top');

%% Resample and regularize.

% Run the filter again, this time resampling and regularizing the

% particles.

[xh, X, w, wt] = bf(tkm1, tk, X, w, [], zk, f, h, d, p, [], true);

% Show the new estimate.

set(hxh, 'XData', [get(hxh, 'XData'), xh(1)], ...

         'YData', [get(hxh, 'YData'), xh(2)]);

% Update the particles.

set(hX, 'XData', X(1,:), 'YData', X(2,:), 'CData', colors(w));

% Hide the trajectories; they no longer connect to the particles.

set(ht, 'Visible', 'off');

% return;

%% Run out the filter for 10s.

% Turn the trajectories back on.

set(ht, 'Visible', 'on');

% Truth

hx = plot(xk(1), xk(2), 'x', 'Color', [0 0.4470 0.7410]);

legend([hz hX hxh hx], 'Measurements', 'Particles', 'Estimated', 'Truth');

% Start the animated GIF.

make_gif = true;

if make_gif

    animation_name = fullfile('animations', 'particle_demo_animation.gif');

    [A, map] = rgb2ind(frame2im(getframe()), 256);

    imwrite(A, map, animation_name, 'gif', ...

            'LoopCount', inf, ...

            'DelayTime', 2);

end

% Set the time step for the simulation.

dt = 0.1;

% Loop until we reach 10s, animating the results.

tic();

t0 = tk;

for tk = tk+dt:dt:10

    

    % Update truth and create noisy measurement.

    xk = propagate_ball(tk-dt, tk, xk);

    zk = xk(1:2) + covdraw(R);

    

    % Create little trajectories for particles (just for the plot).

    for k = 1:nX

        [~, xt{k}] = propagate_ball(tk-dt, tk, X(:,k));

    end

    % Resample on every other time step.

    resample = mod(tk, 0.2) < eps;

    

    % Run the filter.

    [xhk, X, w, wt] = bf(tk-dt, tk, X, w, [], zk, f, h, d, p, [], ...

                         resample);

    % Draw everything.

    set(hX,  'XData', X(1,:), 'YData', X(2,:), 'CData', colors(w));

    line_colors = colors(wt).' * [1 1 1];

    for k = 1:nX

        set(ht(k), 'XData', xt{k}(1,:), ...

                   'YData', xt{k}(2,:), ...

                   'Color', line_colors(k,:));

    end

    set(hz,  'XData', [get(hz, 'XData') zk(1)], ...

             'YData', [get(hz, 'YData') zk(2)]);

    set(hxh, 'XData', [get(hxh, 'XData') xhk(1)], ...

             'YData', [get(hxh, 'YData') xhk(2)]);

    set(hx,  'XData', [get(hx, 'XData') xk(1)], ...

             'YData', [get(hx, 'YData') xk(2)]);

    % Add to the animated GIF.

    if make_gif

        [A, map] = rgb2ind(frame2im(getframe()), 256);

        delay = dt;

        if tk == 10

            delay = 2;

        end

        imwrite(A, map, animation_name, 'gif', ...

                'WriteMode', 'append', ...

                'DelayTime', delay);

    end

    

    % Wait until it's time for the next frame.

    while toc() + t0 < tk + dt

        pause(0.01);

    end

    

end % simulation loop

% Plop full truth on top of it.

htt = plot(x(1, :), x(2, :), 'Color', [0 0.4470 0.7410]);

%% Example multimodal probability distribution

% This simple function will create the matrix square root of a random,

% valid covariance matrix.

randsqrtcov = @()   orth(randn(2)) ...      % Random rotation matrix

                  * diag(sqrt(rand(1, 2))); % Random eigenvalues

% Draw 3 random Gaussian distributions about 3 distinct means.

rng(2);

n = 500;

figure(2);

a = [randsqrtcov() * randn(2, n) + repmat(3 * randn(2,1), 1, n), ...

     randsqrtcov() * randn(2, n) + repmat(3 * randn(2,1), 1, n), ...

     randsqrtcov() * randn(2, n) + repmat(3 * randn(2,1), 1, n)];

plot(a(1,:), a(2,:), '.', 'Color', 0.75*[1 1 1]);

title('Example of Particles for a Multimodal Probability Distribution');

xlabel('State 1');

ylabel('State 2');

⛄ 运行结果

⛄ 参考文献

[1]杨玉林. 基于卡尔曼滤波和粒子滤波的目标跟踪性能对比[J]. 佳木斯大学学报:自然科学版, 2021.

⛄ Matlab代码关注

❤️部分理论引用网络文献，若有侵权联系博主删除

❤️ 关注我领取海量matlab电子书和数学建模资料