粒子滤波 PF——三维匀速运动CV目标跟踪（粒子滤波VS扩展卡尔曼滤波）

对于该博客跟踪代码以及问题探讨可以联系：WX：ZB823618313
对于其他跟踪定位问题的代码及探讨也可以联系

原创不易，路过的各位大佬请点个赞

一、问题描述（离散时间非线性系统描述）

考虑一般非线性系统模型，
$$\begin{gathered} x_k=f(x_{k-1},w_{k-1}) \\ {z}_k=h(x_k,v_k)\text{\hspace{1em}(1)} \end{gathered}$$
其中$x_k$为$k$时刻的目标状态向量。$z_k$为$k$时刻量测向量（传感器数据）。这里不考虑控制器$u_k$。$w_k$和$v_k$分别是过程噪声序列和量测噪声序列，并假设$w_k$和$v_k$为零均值高斯白噪声，其方差分别为$Q_k$和$R_k$的高斯白噪声，即$w_k\sim (0,Q_k)$, $v_k\sim (0,R_k)$，且满足如下关系(线性高斯假设)为：
$$\begin{array}{rl}E[w_i{v}_j^{\prime }]& =0\\ E[w_i{w}_j^{\prime }]& =0i\ne j\\ E[v_i{v}_j^{\prime }]& =0i\ne j\end{array}$$

二、贝叶斯滤波

定义$1$ ~ $k$时刻对状态$x_k$的所有测量数据为
$$z^k=[z_1^T,z_2^T,\cdots ,z_k^T{]}^T$$

贝叶斯滤波问题就是计算对$k$时刻状态$x$估计的置信程度，为此构造概率密度函数$p(x_k|z^k)$，在给定初始分布$p(x_0|z_0)=p(x_0)$后，从理论上看，可以通过预测和更新两个步骤递推得到概率密度函数$p(x_k|z^k)$的值。

是不是卡尔曼滤波的雏形出现了，哈哈哈，预测、更新也存在KF中。

2.1、 预测

现假定$k-1$时刻的概率密度函数已知，则通过将Chapman-Kolmogorov等式应用
于动态方程(1)，即可预测$k$时刻状态的先验概率密度函数为
$$p(x_k|z^{k-1})=\int p(x_k|x_{k-1})p(k-1|z^{k-1})d{x}_{k-1})\text{\hspace{1em}(2)}$$

实际上，状态转移方程写为概率密度的形式即为：$$x_k=f(x_{k-1},w_{k-1})\underset{\text{等价}}{=}p(x_k|x_{k-1})$$
式(2)中隐含假定了$p(x_k|x_{k-1})=p(x_k|x_{k-1},z^{k-1})$，实际上这本身在这里就是成立的，基于(1)式的马尔可夫过程。

2.2、 更新

在获得$p(x_k|z^{k-1})$的基础上，结合$k$时刻得到的新的量测值，基于贝叶斯公式，可以计算$k$时刻状态的后验概率密度函数:
$$p(x_k|z^k)=\frac{p(z_k|x_k)p(x_k|z^{k-1})}{p(z_k|z^{k-1})}\text{\hspace{1em}(3)}$$
式中分子$p(z_k|z^{k-1})$有全概率公式得到
$$p(z_k|z^{k-1})=\int p(z_k|x_k)p(x_k|z^{k-1})d{x}_k\text{\hspace{1em}(4)}$$

我就说吧，上述过程实际上贝叶斯后燕推断的公式，哈哈哈哈啊哈

实际上这也是卡尔曼滤波的更新思想：在$k$时刻得到测量$z_k$后，利用测量$z_k$修正先验概率，进而获得当前时刻状态的后验概率。我正是太机智了，哈哈啊哈

三、粒子滤波 PF（贝叶斯滤波的MC实现）

粒子滤波实际上是递推贝叶斯滤波的蒙特卡洛实现的一种算法，即一种近似的贝叶斯滤波。

核心思想：是使用一组具有相应权值的随机样本(粒子)来表示状态的后验分布。该方法的基本思路是选取一个重要性概率密度并从中进行随机抽样，得到一些带有相应权值的随机样本后，在状态观测的基础上调节权值的大小。和粒子的位置，再使用这些样本来逼近状态后验分布，最后将这组样本的加权求和作为状态的估计值。粒子滤波不受系统模型的线性和高斯假设约束，采用样本形式而不是函数形式对状态概率密度进行描述，使其不需要对状态变量的概率分布进行过多的约束，因而在非线性非高斯动态系统中广泛应用。尽管如此，粒子滤波目前仍存在计算量过大、粒子退化等关键问题亟待突破。

通常情况下选择先验分布作为重要性密度函数、即
$$q(x_k|x_{k-1}^{(i)},z_k)=p(x_k|x_{k-1}^{(i)})$$
对该函数取重要性权值为
$$w_k^{(i)}=w_{k-1}^{(i)}p(z_k|x_k^{(i)})$$
同样$w_k^{(i)}$需要归一化得到${\tilde{w}}_k^{(i)}$。

标准的粒子滤波算法步骤为：

粒子滤波PF:
Step 1: 根据$p(x_0)$采样得到$N$个粒子 $x_0^{(i)}\sim p(x_0)$
For $i=2:N$
  Step 2: 根据状态转移函数产生新的粒子为：$$$x_k^{(i)}\sim p(x_k|x_{k-1}^{(i)})$$
  Step 3: 计算重要性权值：$$w_k^{(i)}=w_{k-1}^{(i)}p(z_k|x_k^{(i)})$$
  Step 4: 归一化重要性权值：$${\tilde{w}}_k^{(i)}=\frac{w_k^{(i)}}{{\sum }_{j=1}^N{w}_k^{(j)}}$$
  Step 5: 使用重采样方法对粒子进行重采样（以随机重采样和系统重采样为例）
  Step 6: 得到$k$时刻的后验状态估计：
$$E[{\hat{x}}_k]=\sum _{i=1}^N{x}_k^{(i)}{\tilde{w}}_k^{(i)}$$
End For

算法：系统重采样 （systematic resampling）
For $i=1:N$
  Step 1: 初始化累积概率密度函数CDF：$c_1=0$
For $i=2:N$
  Step 2: 构造CDF: $c_i=c_{i-1}+w_k^{(i)}$
  Step 3: 从CDF的底部开始：$i=1$
  Step 4: 采样起始点：$u_1=\mathcal{U}[0,1/N]$
End For
For $j=1:N$
  Step 5: 沿CDF移动: $u_j=u_1+(j-1)/N$
  Step 6: While $u_j>c_i$
      $i=i+1$
     End While
  Step 7: 赋值粒子：$x_k^{(j)}=x_k^{(i)}$
  Step 8: 赋值权值：$w_k^{(j)}=1/N$
  Step 9: 赋值父代：$i^{(j)}=i$
End For

四、仿真场景：三维雷达目标跟踪

4.1 仿真场景（三维匀速目标）

目标模型
考虑一各三维的匀速运动目标（CV 模型）：
$$x_{k+1}=F_k{x}_k+G_k{w}_k$$
其中状态向量$x_k=[x_k,{\dot{x}}_k,y_k,{\dot{y}}_k,z_k,{\dot{z}}_k{]}^{\prime }$；噪声为$w_k=[w_x,w_y,w_z{]}^{\prime }$；状态转移矩阵$F_k$和噪声驱动矩阵$G_k$如下
$$F_k=\left[\begin{array}{cccccc}1& T& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& T& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& T\\ 0& 0& 0& 0& 0& 1\end{array}\right]{\Gamma }_k=\left[\begin{array}{ccc}1/2T^2& 0& 0\\ T& 0& 0\\ 0& 1/2T^2& 0\\ 0& T& \\ 0& 0& 1/2T^2\\ 0& 0& T\end{array}\right]$$
采样时间 $T=1s$. 初始状态为 $$\begin{gathered} x_0\sim ({\bar{x}}_0,P_0) \\ {\bar{x}}_0=[1\text{km},20\text{m/s}，1\text{km},20\text{m/s},1\text{km},20\text{m/s}{]}^{\prime } \\ {P}_0=\text{diag}(10^5{\text{m}}^2,10^2{\text{m}}^2/{\text{s}}^2,10^5{\text{m}}^2,10^2{\text{m}}^2/{\text{s}}^2，10^5{\text{m}}^2,10^2{\text{m}}^2/{\text{s}}^2) \end{gathered}$$
过程噪声均值和方差分别为 $q=10$
$$\begin{gathered} {\bar{w}}_k=[0，0，0{]}^{\prime } \\ {Q}_k=\left[\begin{array}{ccc}{q}^2& 0& 0\\ 0& q^2& 0\\ 0& 0& q^2\end{array}\right] \end{gathered}$$

如果为非线性目标，则将状态转移矩阵$F_k$代替为雅可比矩阵即可。为了不是一般性这里采用线性模型进行仿真。主要处理目标跟踪，雷达量测存在的非线性滤波问题。

雷达量测模型
在三维情况下，雷达量测为距离和角度
$$\begin{gathered} r_k^m=r_k+{\tilde{r}}_k \\ {b}_k^m=b_k+{\tilde{b}}_k \\ {e}_k^m=e_k+{\tilde{e}}_k \end{gathered}$$
其中
$$\begin{gathered} r_k=\sqrt{(x_k-x_0{)}^{+}(y_k-y_0{)}^2)} \\ {b}_k={\tan }^{-1}\frac{y_k-y_0}{x_k-x_0} \\ {e}_k={\tan }^{-1}\frac{z_k-z_0}{\sqrt{(x_k-x_0{)}^2+(y_k-y_0{)}^2}} \end{gathered}$$
$[x_0,y_0,z_0]$为雷达坐标，一般情况为0。雷达量测为$z_k=[r_k,b_k,e_k{]}^{\prime }$。雷达量测方差为
$$R_k=\text{cov}(v_k)=\left[\begin{array}{ccc}{\sigma }_r^2& 0& 0\\ 0& {\sigma }_b^2& 0\\ 0& 0& {\sigma }_e^2\end{array}\right]$$且${\sigma }_r=20m$， ${\sigma }_b=20mrad$, ${\sigma }_e=15mrad$。

4.2 跟踪轨迹

4.3 跟踪误差

五、部分代码

对于该博客跟踪代码以及问题探讨可以联系：WX：ZB823618313
对于其他跟踪定位问题的代码及探讨也可以联系

5.1、主函数部分代码

clear all; close all; clc;
%% initial parameter
n=6; %状态维数 ;
T=1; %采样时间
M=1; %雷达数目
N=100; %运行总时刻
MC=10; %蒙特卡洛次数
global N_pf
N_pf=5000;% 采样点数PF
chan=1; %滤波器通道，这里只有一个滤波器
w_mu=[0,0,0]';% mean of process noise 
v_mu=[0,0,0]';% mean of measurement noise
%% target model
%covariance of process noise
q=10; %m/s^2
Qk=q^2*eye(3); 
% state matrix
% CV
Fk=[1,T,0,0,0,0;
      0,1,0,0,0,0;
      0,0,1,T,0,0;
      0,0,0,1,0,0;
      0,0,0,0,1,T;
      0,0,0,0,0,1 ];

Gk=[  T^2/2,    0,    0;
          T,    0,    0;
          0,T^2/2,    0;
          0,    T,    0;
          0,    0,T^2/2;
          0,    0,    T ];
%量测模型
sigma_r(1)=20;  sigma_b(1)=20e-3; sigma_e(1)=15e-3; % covariance of measurement noise (radar)
% sigma_r=300; sigma_b=200e-3; sigma_e=100e-3;
Rk=diag([sigma_r(1)^2, sigma_b(1)^2,sigma_e(1)^2]);
xp=[0,0,0,0,0,0];%雷达位置
%% 定义存储空间
sV=zeros(n,N,MC); % 状态
eV=zeros(n,N,MC,chan); %估计
PV=zeros(n,n,N,MC,chan);%协方差
rV=zeros(3,N,MC,M); % %量测
for i=1:MC
    sprintf('rate of process:%3.1f%%',(2*i)/(4*MC)*100)
    % 初始状态的均值和方差
    x=[1000,500,1000,0,100,100]';
    P_0=diag([1e4,10^2,1e4,10^2, 1e4,10^2]); 
    x=[1000,80,1000,50,100,10]';
    P_0=diag([1e5,10^2,1e5,10^2, 1e5,10^2]); 
%     x=[100,50,100,50,100,50]';
%     P_0=diag([5e5,1e3,5e5,1e3,5e5,1e3]); %initial covariance
    xk_EKF=x;    Pk_EKF=P_0;   % P0|0 x0|0 
    xk_pf=x;     Pk_pf=P_0;   % P0|0 x0|0 
    %产生N个粒子
    for ii = 1 : N_pf
        xpart(:,ii) = x+ sqrtm(P_0) * randn(6,1);
    end

5.2、PF部分代码

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%函数功能：实现随机重采样算法
%输入参数：weight为原始数据对应的权重大小
%输出参数：outIndex是根据weight对inIndex筛选和复制结果
function outIndex=randomR(weight)
%获得数据的长度
L=length(weight);
%初始化输出索引向量，长度与输入索引向量相等
outIndex=zeros(1,L);
%第一步：产生[0,1]上均匀分布的随机数组，并升序排序
u=unifrnd(0,1,1,L);
u=sort(u);
%u=(1:L)/L%这个是完全均匀
%第二步：计算粒子权重积累函数cdf
cdf=cumsum(weight);
%第三步：核心计算
i=1;
for j=1:L
	%此处的基本原理是：u是均匀的，必然是权值大的地方
	%有更多的随机数落入该区间，因此会被多次复制
	while(i<=L)&(u(i)<=cdf(j))
		%复制权值大的粒子
		outIndex(i)=j;
		%继续考察下一个随机数，看它落在哪个区间
		i=i+1;
	end
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 系统重采样子函数
% 输入参数：weight为原始数据对应的权重大小
% 输出参数：outIndex是根据weight筛选和复制结果
function outIndex = systematicR(weight);
N=length(weight);
N_children=zeros(1,N);
label=zeros(1,N);
label=1:1:N;
s=1/N;
auxw=0;
auxl=0;
li=0;
T=s*rand(1);
j=1;
Q=0;
i=0;
u=rand(1,N);
while (T<1)
    if (Q>T)
        T=T+s;
        N_children(1,li)=N_children(1,li)+1;
    else
        i=fix((N-j+1)*u(1,j))+j;
        auxw=weight(1,i);
        li=label(1,i);
        Q=Q+auxw;
        weight(1,i)=weight(1,j);
        label(1,i)=label(1,j);
        j=j+1;
    end
end
index=1;
for i=1:N
    if (N_children(1,i)>0)
        for j=index:index+N_children(1,i)-1
            outIndex(j) = i;
        end;
    end;
    index= index+N_children(1,i);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%