粒子滤波与PF目标追踪
PF目标追踪
粒子滤波
- https://www.bilibili.com/read/cv5073126
先定义一下基本场景，以及理清楚基础概念。
我们有一个小车，可以在2D平面上移动。
我们离散化整个过程，意思是，我们在每个k*T（T是周期）的时候用粒子滤波定位小车的位置。比如，出发后第一秒1s，第二秒2s，第三秒3s。实际上肯定也是这么做。
粒子滤波定位小车的过程分为三步，需要三个信息（就是我们知道三件事）。
哪三个信息呢？
1.车上一秒在哪
2.车的运动是什么
3.测量结果，比如GPS
哪三步呢？
1.Prediction Step
2.Innovation Step
- Re-sampling Step
这三步是干嘛呢？这三个信息在这三步里怎么用呢？
一步一步来,
第一步，Prediction Step（预测）
用到上一步的结果，和运动。请看下图
这个就像是卡尔曼滤波里面的根据此时刻状态预测下一时刻状态。
我们用“粒子”点来表示车的位置，也就是上图的×。
上一步的位置，我们用红色的x来表示，他们都是车的可能的位置。比如上图的(1,5),(3,5),(3,7),(5,5)
那你说这些位置听谁的呢？车在哪呢？我们可以给每个位置定个权重，然后加权平均一下。在预测这步权重都是一样的，我们有四个粒子，那么每个权重就是0.25。（其实和强化学习很像）。
也就是我们有四个粒子x1，x2，x3，x4，x[(横坐标x，纵坐标y)，权重w]
x1[(1,5),0.25]
x2[(3,5),0.25]
x3[(3,7),0.25]
x4[(5,5),0.25]
然后我们有个运动，这个运动可能是轮速传感器得来的，或者是比如IMU，复杂一点对于无人车来讲，7自由度整车模型的预测结果等等（这就专业一点了）。不管怎么来的吧。
假设我们现在知道了运动是(5,4)。但是还有个误差。
预测这步就是把所有权重不变，在状态（运动）加上这个运动的结果。
x1[(6,9),0.25]
x2[(8,9),0.25]
x3[(8,11),0.25]
x4[(10,9),0.25]
如果你知道噪声的大小，可以在结果上加上一个随机数。结果如下：
x1[(6.1,9.2),0.25]
x2[(7.9,9),0.25]
x3[(8.3,10.8),0.25]
x4[(10.1,8.9),0.25]
预测这步就完啦。其实卡尔曼滤波也得知道噪声的信息，滤波算法就是这样，非线性优化就不需要。
第二步，Innovation Step（这个咋翻译，更新？）
这步的作用是，嘿我不是还有个GPS吗（或者其他的测量，图像定位之类的）？
我们说测量值是下图那个绿色的点，然后测量是不准的嘛，假设他遵循高斯分布。
我们怎么把这个测量和那一堆现在有的，经过prediction的粒子的信息融合在一起呢？
是通过之前那个权重值。
按照测量的概率分布来重新分配权重。高斯分布的话当然是离中心点越近权重越大啦。上图举例x2 这里的权重应该是第一步得到的概率乘以这里的概率得到最后的权重。 x1[(6.1,9.2),0.4] x2[(7.9,9),0.5] x3[(8.3,10.8),0.35] x4[(10.1,8.9),0.25] 好现在权重更新了，但是还有个问题！我们稍后希望用概率的方式来使用权重，但是权重加在一起大于1了怎么办？归一化一下就好了嘛。每个新权重=旧权重/求和（旧权重） x1[(6.1,9.2),0.26] x2[(7.9,9),0.33] x3[(8.3,10.8),0.23] x4[(10.1,8.9),0.16] 然后这步就完了，简单吧 第三步，Resampling step 这一步是干嘛呢？ 你上一步得到的那些权重没用上对不对，这步用上了。 我们之前那些粒子不用了，重新摇色子摇几个粒子出来。不过一般来讲，粒子数量是不变的，都是能足够多就足够多。 比如之前的 x1[(6.1,9.2),0.26] x2[(7.9,9),0.33] x3[(8.3,10.8),0.23] x4[(10.1,8.9),0.16] 比如x2的概率是0.33，那么新摇号摇出来的粒子可能有两个x2，因为它概率最大，x4的概率比较小可能一个没有。 然后我们认为摇出来的新的粒子权重相等。 x1[(6.1,9.2),0.25] x2[(7.9,9),0.25] x3[(8.3,10.8),0.25] x4[(7.9,9),0.25] 最终的估计值可以是四个粒子的平均。 粒子滤波结束总结
%% 粒子滤波的应用
clear all
close all
clc
%% initialize the variables
x = 0.1; % initial actual state
x_N = 0.9; % 系统过程噪声的协方差 (由于是一维的，这里就是方差)
x_R = 1; % 测量的协方差
T = 30; % 共进行75次
N = 10; % 粒子数，越大效果越好，计算量也越大
%initilize our initial, prior particle distribution as a gaussian around
%the true initial value
V = 2; %初始分布的方差
x_P = []; % 粒子
% 用一个高斯分布随机的产生初始的粒子
for i = 1:N
x_P(i) = x + sqrt(V) * randn;
end
z_out = [x^2 / 20 + sqrt(x_R) * randn]; %实际测量值
x_out = [x]; %the actual output vector for measurement values.
x_est = [x]; % time by time output of the particle filters estimate
x_est_out = [x_est]; % the vector of particle filter estimates.
for t = 1:T
x = 0.5*x + 25*x/(1 + x^2) + 8*cos(1.2*(t-1)) + sqrt(x_N)*randn;
z = x^2/20 + sqrt(x_R)*randn;
for i = 1:N
%从先验p(x(k)|x(k-1))中采样
x_P_update(i) = 0.5*x_P(i) + 25*x_P(i)/(1 + x_P(i)^2) + 8*cos(1.2*(t-1)) + sqrt(x_N)*randn;
%计算采样粒子的值，为后面根据似然去计算权重做铺垫
z_update(i) = x_P_update(i)^2/20;
%对每个粒子计算其权重，这里假设量测噪声是高斯分布。所以 w = p(y|x)对应下面的计算公式
P_w(i) = (1/sqrt(2*pi*x_R)) * exp(-(z - z_update(i))^2/(2*x_R));
end
% 归一化.
P_w = P_w./sum(P_w);
%% Resampling
for i = 1 : N
x_P(i) = x_P_update(find(rand <= cumsum(P_w),1)); % 粒子权重大的将多得到后代
end % find( ,1) 返回第一个 符合前面条件的数的 下标
%状态估计，重采样以后，每个粒子的权重都变成了1/N
x_est = mean(x_P);
% Save data in arrays for later plotting
x_out = [x_out x];
z_out = [z_out z];
x_est_out = [x_est_out x_est];
end
t = 0:T;
figure(1);
clf
plot(t, x_out, '.-b', t, x_est_out, '-.r','linewidth',3);
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('time step(1-3个粒子)'); ylabel('距离远点的距离');
legend('真值', '估计值');
%legend('True flight position', 'Particle filter estimate');