粒子滤波Matlab代码解读

粒子滤波概念

理解粒子滤波(particle filter)

粒子滤波的Matlab仿真算法实现

1.1 机器人定位问题

关于机器人定位，有三大问题，它们分别是：

(1)“全局定位”：指初始位置未知，机器人靠自身运动确定自己在地图中的位姿。

(2)“位姿跟踪”：指已知自身位姿或者已经通过“全局定位”得到了一个较好的位姿估计，在后续运动时补偿精度较差的运动控制误差；

(3)“绑架劫持”：指机器人在已知自身位姿的情况下，得到了一个错误的位姿信息或者外界将其放到另外一个位姿，而里程计信息给出了错误的信息甚至没有给出控制信息。

1.2 粒子滤波步骤(可结合2中例题)

(1)初始状态：用大量粒子模拟运动状态，使粒子在空间内均匀分布；

(2)预测阶段：根据状态转移方程，将每一个粒子带入，得到一个预测粒子；

(3)校正阶段：对预测粒子进行评价（计算权重），越接近于真实状态的粒子，其权重越大；

(4)重采样：根据粒子权重对粒子进行筛选，筛选过程中，既要大量保留权重大的粒子，又要有一小部分权重小的粒子；

(5)滤波：将重采样后的粒子带入状态转移方程得到新的预测粒子，即步骤(2)。

代码分析

虽然选修课选修了Matlab， 但是自己Matlab工程代码到底写了太少， 特别是涉及维度转换以及数学相关的玩的都不是很好， 因此这次看这份代码学到了很多基础入门的写法， 记录一下：

• 变量关系：
  • X为对象的真实位置， Z是观测到对象的观测位置（在真实的坐标上加上了噪声），P是粒子群，Pcenter是所有粒子的几何中心
• 变量维度的确定： 行为一维，列为二维。其中X(1, :)表示X坐标， X(2, :)表示Y坐标。其中列的T表示多少测量数量，测量时间。
• err大小也是(2, T)， 但是(第一行为粒子与真实路径误差 第二行为测量与真实路径误差)
• 权重更新方式： 权重 (权重与距离的关系 为 均值是0,方差是sqrt®的高斯分布曲线)
• 代码组成部分：
  1. 参数设置
  2. 初始化粒子群
  3. 开始运动
  4. 绘制轨迹
  5. 绘制误差图

%粒子滤波(定位运动轨迹)
%在二维空间,假设运动物体的一组(非线性)运动位置、速度、加速度数据,用粒子滤波方法进行处理
clc,clear,close all

%% 参数设置
N = 200; %粒子总数
Q = 5; %过程噪声(控制误差)  状态转移方程中使用
R = 5; %测量噪声  由真实位置叠加测量噪声得到测量位置
T = 10; %测量时间(总步数)
theta = pi/T; %旋转角度
distance = 80/T; %每次走的距离(步长)
WorldSize = 100; %世界大小
% 设定变量维度
X = zeros(2, T); %存储系统状态(每列存储二维位置坐标(x,y),共T个位置)
Z = zeros(2, T); %存储系统的观测状态(每列存储二维位置坐标(x,y),共T次测量)
P = zeros(2, N); %建立粒子群(每列存储当前粒子的二维位置坐标,共N个粒子)
PCenter = zeros(2, T); %所有粒子的中心位置
w = zeros(N, 1); %每个粒子的权重
err = zeros(2,T); %误差(第一行为粒子与真实路径误差  第二行为测量与真实路径误差)


X(:, 1) = [50; 20]; %初始系统状态 即初始位置在坐标(50,20)
Z(:, 1) = X(:,1) + wgn(2,1,10*log10(R)); %初始系统的观测状态(为真实位姿叠加高斯噪声)
                                         %y = wgn(m,n,p) 产生一个m行n列的高斯白噪声的矩阵，p以dBW为单位指定输出噪声的强度

%% 初始化粒子群
for i = 1 : N
    P(:, i) = [WorldSize*rand; WorldSize*rand];  %随机产生第i个粒子的坐标(rand为产生[0,1]之间均匀分布)
    dist = norm(P(:, i)-Z(:, 1)); %与测量位置相差的距离
    %求权重 (权重与距离的关系 为 均值是0,方差是sqrt(R)的高斯分布曲线)  
    %   因为均值为0且距离大于0 因此权重随着距离增加沿高斯曲线右侧递减
    w(i) = (1 / sqrt(R) / sqrt(2 * pi)) * exp(-(dist)^2 / 2 / R); 
end
PCenter(:, 1) = sum(P, 2) / N;%t=1时刻(初始时刻)所有粒子的几何中心位置
% 初始状态(t=1)画图
err(1,1) = norm(X(:, 1) - PCenter(:, 1));%粒子群几何中心与系统真实状态的误差
err(2,1) = wgn(1, 1, 10*log10(R));
figure(1);
hold on
set(0,'defaultfigurecolor','w')
plot(X(1, 1), X(2, 1), 'r.', 'markersize',30) %真实的初始状态位置(红点表示)
%grid on
axis([0 100 0 100]);
set(gca,'XTick',0:10:100) %改变x轴坐标间隔显示 这里间隔为10
set(gca,'YTick',0:10:100) %改变y轴坐标间隔显示 这里间隔为10
plot(P(1, :), P(2, :), 'k.', 'markersize',5); %各个粒子位置(N个黑点)
plot(PCenter(1, 1), PCenter(2, 1), 'b.', 'markersize',25); %所有粒子的中心位置(蓝点表示)
legend('真实位置', '粒子群', '粒子群的几何中心');
title('初始状态');
hold off




%% 开始运动
for k = 2 : T %从t=2到T
    %模拟一个弧线运动的状态
    X(:, k) = X(:, k-1) + distance * [(-cos(k * theta)); sin(k * theta)] + wgn(2, 1, 10*log10(Q)); %状态方程
    Z(:, k) = X(:, k) + wgn(2, 1, 10*log10(R)); %观测方程(状态上叠加测量的高斯噪声) 
    
    %粒子滤波
    % 1.预测
    for i = 1 : N
        P(:, i) = P(:, i) + distance * [-cos(k * theta); sin(k * theta)] + wgn(2, 1, 10*log10(Q));%粒子群带入状态方程
        dist = norm(P(:, i)-Z(:, k)); %粒子群中各粒子 与 测量位置 的距离
        w(i) = (1 / sqrt(R) / sqrt(2 * pi)) * exp(-(dist)^2 / 2 / R); %求权重(距离近权重大)
    end
    
    % 2.归一化权重
    wsum = sum(w);
    for i = 1 : N
        w(i) = w(i) / wsum;
    end
    
    % 3.重采样（更新）——可以理解为转转盘
    for i = 1 : N
        wmax = 2 * max(w) * rand; %另一种重采样规则
        index = randi(N, 1);%生成一个在[1(默认值),N]之间均匀分布的伪随机整数
        while(wmax > w(index))			% 找到具体落在哪个区间上
            wmax = wmax - w(index);
            index = index + 1;
            if index > N
                index = 1;
            end 
        end
        Pnext(:, i) = P(:, index); %得到新粒子放入临时集Pnext
    end
    P=Pnext;%用临时集Pnext更新粒子集P
    PCenter(:, k) = sum(P, 2) / N; %重采样后所有粒子的中心位置
    %计算误差
    err(1,k) = norm(X(:, k) - PCenter(:, k)); %粒子几何中心与系统真实状态的误差
    err(2,k) = norm(X(:, k) - Z(:, k));
    %画图
    figure(2);
    set(0,'defaultfigurecolor','w')
    clf;%清空figure(2)中的图像 以便循环重新画
    hold on
    plot(X(1, k), X(2, k), 'r.', 'markersize',30); %系统状态位置
    plot(P(1, :), P(2, :), 'k.', 'markersize',5); %各个粒子位置
    plot(PCenter(1, k), PCenter(2, k), 'b.', 'markersize',25); %所有粒子的中心位置
    axis([0 100 0 100]);
    title('运动过程');
    legend('真实状态', '粒子群', '粒子群的几何中心');
    hold off
    pause(0.1);%停0.1s开始下次迭代
end

%% 绘制轨迹
figure(3);
set(0,'defaultfigurecolor','w')
plot(X(1,:), X(2,:), 'r.-', Z(1,:), Z(2,:), 'g.-', PCenter(1,:), PCenter(2,:), 'b.-');
axis([0 100 0 100]);
set(gca,'XTick',0:10:100) %改变x轴坐标间隔显示 这里间隔为10
set(gca,'YTick',0:10:100) %改变y轴坐标间隔显示 这里间隔为10
legend('真实轨迹', '测量轨迹', '粒子群几何中心轨迹');
xlabel('横坐标 x'); ylabel('纵坐标 y');

%% 绘制误差
figure(4);
set(0,'defaultfigurecolor','w')
%set(gca,'FontSize',12);%设置图标字体大小
plot(err(1,:),'b.-');%err1为各时刻 真实位置与粒子群中心的几何距离
hold on
plot(err(2,:),'r.-');%err2为各时刻 真实位置与测量位置的几何距离
hold off
xlabel('步数 t');
legend('粒子群误差', '测量误差');
title('真实位置与粒子群中心的集合距离');

用到的函数记录

Matlab中的norm

demo代码中有关norm的运用如下,

%% 初始化粒子群
for i = 1 : N
    P(:, i) = [WorldSize*rand; WorldSize*rand];  %随机产生第i个粒子的坐标(rand为产生[0,1]之间均匀分布)
    dist = norm(P(:, i)-Z(:, 1)); %与测量位置相差的距离
    %求权重 (权重与距离的关系 为 均值是0,方差是sqrt(R)的高斯分布曲线)  
    %   因为均值为0且距离大于0 因此权重随着距离增加沿高斯曲线右侧递减
    w(i) = (1 / sqrt(R) / sqrt(2 * pi)) * exp(-(dist)^2 / 2 / R); 
end

其中没有特别指出是啥范数, 因此可以通过help norm来查看一下关于norm的使用介绍.

>> help norm
 norm   Matrix or vector norm.
      norm(X,2) returns the 2-norm of X.
 	
      norm(X) is the same as norm(X,2).
 
      norm(X,1) returns the 1-norm of X.

可以得到, norm(X)的效果默认是取2-范数的。

同时， norm函数既可以取向量范数又可以取矩阵范数：

1、如果A为矩阵

• n=norm(A) 《Simulink与信号处理》

  返回A的最大奇异值，即max(svd(A))

• n=norm(A,p)

  根据p的不同，返回不同的值

p	返回值
1	返回A中最大一列和，即max(sum(abs(A)))
2	返回A的最大奇异值，和n=norm(A)用法一样
inf	返回A中最大一行和，即max(sum(abs(A’)))
‘fro’	A和A‘的积的对角线和的平方根，即sqrt(sum(diag(A’*A)))

2、如果A为向量

• norm(A,p)

  返回向量A的p范数。即返回 sum(abs(A).^p)(1/p),对任意 1<p<+∞.

• norm(A)

  返回向量A的2范数，即等价于norm(A,2)。

• norm(A,inf)

  返回max(abs(A))

• norm(A,-inf)

  返回min(abs(A))

Matlab中的sum

• sum(A, 1)： 对一维求和， 如果A为二维数组， 则按列求和
• sum(A, 2): 对二维求和， 如果A为二维数组， 则按行求和

Matlab中的wgn

>> help wgn
 wgn Generate white Gaussian noise.
    Y = wgn(M,N,P) generates an M-by-N matrix of white Gaussian noise. P
    specifies the power of the output noise in dBW. The unit of measure for
    the output of the wgn function is Volts. For power calculations, it is
    assumed that there is a load of 1 Ohm. 
 
    Y = wgn(M,N,P,IMP) specifies the load impedance in Ohms.