数据关联的步骤:
(1)建立关联门,确定关联门限
(2)门限过滤
(3)确定相似性度量方法
(4)建立关联矩阵
(5)确定关联判定准则
(6)形成关联对
一、最近邻关联(Nearest Neighbour, NN)
利用加权欧式距离来计算每个观测数据到真实目标的距离,然后再取距离最近的一个观测值作为目标真实状态。
加权欧式距离的计算
假设在第k次扫描之前,已经建立N条航迹。第k次新观测为
Z
i
(
k
)
,
j
=
1
,
2
,
.
.
,
N
Z_i(k), j=1,2,..,N
Zi(k),j=1,2,..,N。在第i条航迹的关联门内,观测j和航迹i的差矢量定义为测量值和预测值之间的差,即滤波器残差,如下所示:
e
i
j
(
k
)
=
Z
j
(
k
)
−
H
X
^
i
(
k
∣
k
−
1
)
e_{ij}(k)=Z_j(k)-H\hat{X}_i(k|k-1)
eij(k)=Zj(k)−HX^i(k∣k−1)
其中,
X
^
i
(
k
∣
k
−
1
)
\hat{X}_i(k|k-1)
X^i(k∣k−1)是状态估计的下一步预测,
X
^
i
(
k
∣
k
−
1
)
=
A
X
i
(
k
−
1
∣
k
−
1
)
\hat{X}_i(k|k-1)=AX_i(k-1|k-1)
X^i(k∣k−1)=AXi(k−1∣k−1),其中
A
A
A是状态转移矩阵,
H
H
H为观测矩阵,设
S
(
k
)
S(k)
S(k)为
e
i
j
(
k
)
e_ij(k)
eij(k)的协方差矩阵,则统计距离(加权欧式距离)为:
d
i
j
=
e
i
j
(
k
)
S
(
k
)
−
1
e
i
j
(
k
)
T
d_{ij}=e_{ij}(k)S(k)^{-1}e_{ij}(k)^{T}
dij=eij(k)S(k)−1eij(k)T
将落在关联门内并且与被跟踪目标的预测位置“最近邻”的观测点作为与航迹相关联的观测。
二、全局最近邻关联(Global Nearest Neighbour, GNN)
同样使用加权欧式距离来计算每个观测数据到真实目标的距离,但是需要满足总的距离或关联代价最小,实现最优分配的问题。
m
i
n
{
∑
i
=
1
n
∑
j
=
1
n
c
i
j
x
i
j
}
,其中
∑
i
=
1
n
x
i
j
=
1
∑
j
=
1
n
x
i
j
=
1
min \left\{ \sum_{i=1}^{n} \sum_{j=1}^{n} c_{ij}x_{ij}\right\},其中\sum_{i=1}^{n}x_{ij}=1 \sum_{j=1}^{n}x_{ij}=1
min{i=1∑nj=1∑ncijxij},其中i=1∑nxij=1j=1∑nxij=1
其中
x
i
j
x_{ij}
xij为二值变量,0表示不关联,1表示关联,
c
i
j
c_{ij}
cij表示测量i与目标j之间距离,用矩阵表示时,矩阵的每行每列只能有1个元素为1。
关联矩阵较大时,二维分配问题可以用Munkre算法或Burgeois算法求解,求解具有多项式复杂度,是非NP问题。
三、概率数据关联(Probabilistic Data Association, PDA)
PDA算法和NN算法都是针对单目标或稀疏环境中的常用算法,PDA的思想是认为只要是存在于关联门内的有效观测值,就有可能源于目标,只是每个有效观测值源于目标的概率不同。通过大量的相关计算给出每个有效观测值的概率加权系数,计算所有有效观测值的加权和作为真实目标观测值的估计,从而在卡尔曼滤波中更新目标状态。主要用于解决杂波环境中的单目标跟踪问题。
3.1 系统建模
在现代控制理论中,系统的数学模型由运动方程和观测方程组成(每个符号的具体含义可自行百度):
X(k+1)=FX(k)+BU(k)+W(k)-运动方程
Z(k)=HX(k)+V(k) -观测方程
3.2 卡尔曼滤波
(1)k时刻的状态预测值:
X
^
(
k
∣
k
−
1
)
=
F
X
^
(
k
−
1
∣
k
−
1
)
\hat{X}(k|k-1)=F\hat{X}(k-1|k-1)
X^(k∣k−1)=FX^(k−1∣k−1)
(2)k时刻的预测误差协方差矩阵:
P
^
(
k
∣
k
−
1
)
=
F
P
^
(
k
−
1
∣
k
−
1
)
F
⊤
+
Q
\hat{P}(k|k-1)=F\hat{P}(k-1|k-1)F^\top+Q
P^(k∣k−1)=FP^(k−1∣k−1)F⊤+Q
(3)k时刻的观测预测值:
Z
^
(
k
∣
k
−
1
)
=
H
X
^
(
k
∣
k
−
1
)
\hat{Z}(k|k-1)=H\hat{X}(k|k-1)
Z^(k∣k−1)=HX^(k∣k−1)
(4)预测观测值误差的协方差:
S
(
k
)
=
H
P
^
(
k
−
1
∣
k
−
1
)
H
⊤
+
R
S(k)=H\hat{P}(k-1|k-1)H^\top+R
S(k)=HP^(k−1∣k−1)H⊤+R
(5)卡尔曼增益:
K
(
k
)
=
P
^
(
k
∣
k
−
1
)
H
⊤
S
(
k
∣
k
−
1
)
K(k)=\hat{P}(k|k-1)H^{\top}S(k|k-1)
K(k)=P^(k∣k−1)H⊤S(k∣k−1)
(6)k时刻的最优估计值:
X
^
(
k
∣
k
)
=
X
^
(
k
∣
k
−
1
)
+
K
(
Z
(
k
)
−
H
X
^
(
k
∣
k
−
1
)
)
\hat{X}(k|k)=\hat{X}(k|k-1)+K(Z(k)-H\hat{X}(k|k-1))
X^(k∣k)=X^(k∣k−1)+K(Z(k)−HX^(k∣k−1))
(7)k时刻的最优估计值协方差矩阵:
P
^
(
k
∣
k
)
=
(
I
−
K
(
k
)
H
)
P
^
(
k
∣
k
−
1
)
\hat{P}(k|k)=(I-K(k)H)\hat{P}(k|k-1)
P^(k∣k)=(I−K(k)H)P^(k∣k−1)
3.3 PDA算法解析
(1)定义变量
Z
k
=
{
z
k
,
i
}
,
i
=
0
,
1
,
.
.
.
,
m
k
Z_k=\left\{ z_{k,i}\right\},i=0,1,...,m_k
Zk={zk,i},i=0,1,...,mk表示在k时刻目标先验
X
^
k
∣
k
−
1
\hat{X}_{k|k-1}
X^k∣k−1的关联门内共有
m
k
m_k
mk个有效观测值;
Z
k
=
{
Z
j
}
,
j
=
1
,
2
,
.
.
.
,
k
Z^k=\left\{ Z_{j}\right\},j=1,2,...,k
Zk={Zj},j=1,2,...,k表示从时刻1到k所有关联门内的有效观测值的集合。
(2)均值估计
当已知
Z
k
Z^k
Zk时,状态的条件概率的均值估计为:
连续情况:
X
^
k
∣
k
=
E
{
X
^
k
∣
k
−
1
∣
Z
k
}
=
∫
X
^
k
∣
k
−
1
p
(
X
^
k
∣
k
−
1
∣
Z
k
)
d
X
^
k
∣
k
−
1
\hat{X}_{k|k}=E\left\{ \hat{X}_{k|k-1}|Z^k\right\}=\int \hat{X}_{k|k-1}p(\hat{X}_{k|k-1}|Z^k)d\hat{X}_{k|k-1}
X^k∣k=E{X^k∣k−1∣Zk}=∫X^k∣k−1p(X^k∣k−1∣Zk)dX^k∣k−1
离散情况:
X
^
k
∣
k
=
E
{
X
^
k
∣
k
−
1
∣
Z
k
}
=
∑
k
=
1
N
X
^
k
∣
k
−
1
P
(
X
^
k
∣
k
−
1
∣
Z
k
)
\hat{X}_{k|k}=E\left\{ \hat{X}_{k|k-1}|Z^k\right\}=\sum_{k=1}^{N} \hat{X}_{k|k-1}P(\hat{X}_{k|k-1}|Z^k)
X^k∣k=E{X^k∣k−1∣Zk}=k=1∑NX^k∣k−1P(X^k∣k−1∣Zk)
(3)完备事件组
如上图所示某一时刻在一个目标状态的先验估计
Z
^
1
\hat{Z}^1
Z^1的关联门内,有三个观测值,定义如下几个事件:1.
Z
1
,
Z
2
,
Z
3
Z_1,Z_2,Z_3
Z1,Z2,Z3都是干扰信号,即都不是目标的真实观测;2.
Z
1
Z_1
Z1起源于目标,
Z
2
,
Z
3
Z_2, Z_3
Z2,Z3是干扰信号;3.
Z
2
Z_2
Z2起源于目标,
Z
1
,
Z
3
Z_1, Z_3
Z1,Z3是干扰信号;4.
Z
3
Z_3
Z3起源于目标,
Z
1
,
Z
2
Z_1, Z_2
Z1,Z2是干扰信号。这四个事件组成了一个完备事件组,各个事件之间互斥,且概率和为1.
定义:
χ
k
,
i
=
{
z
k
,
i
起源于目标
}
,
i
=
1
,
2
,
.
.
.
,
m
k
\chi_{k,i}=\left\{z_{k,i}起源于目标\right\},i=1,2,...,m_k
χk,i={zk,i起源于目标},i=1,2,...,mk
χ
k
,
0
=
{
没有一个有效观测起源于目标
}
\chi_{k,0}=\left\{没有一个有效观测起源于目标\right\}
χk,0={没有一个有效观测起源于目标}
(4)k时刻目标的状态估计
X
^
k
∣
k
=
E
{
X
^
k
∣
k
−
1
∣
Z
k
}
=
∑
k
=
1
N
X
^
k
∣
k
−
1
P
(
X
^
k
∣
k
−
1
∣
Z
k
)
=
∑
k
=
1
N
∑
i
=
0
m
k
X
^
k
∣
k
−
1
P
(
X
^
k
∣
k
−
1
∣
χ
k
,
i
,
Z
k
)
P
(
χ
k
,
i
∣
Z
k
)
=
∑
i
=
0
m
k
E
{
X
^
k
∣
k
−
1
∣
χ
k
,
i
,
Z
k
}
P
(
χ
k
,
i
∣
Z
k
)
=
∑
i
=
0
m
k
X
^
k
∣
k
i
P
(
χ
k
,
i
∣
Z
k
)
\begin{aligned} \hat{X}_{k|k} & =E\left\{ \hat{X}_{k|k-1}|Z^k\right\} \\ & =\sum_{k=1}^{N} \hat{X}_{k|k-1}P(\hat{X}_{k|k-1}|Z^k) \\ & =\sum_{k=1}^{N}\sum_{i=0}^{m_k}\hat{X}_{k|k-1}P(\hat{X}_{k|k-1}|\chi_{k,i},Z^k)P(\chi_{k,i}|Z^k)\\ & =\sum_{i=0}^{m_k}E\left\{ \hat{X}_{k|k-1}|\chi_{k,i},Z^k\right\}P(\chi_{k,i}|Z^k) \\ & =\sum_{i=0}^{m_k}\hat{X}_{k|k}^iP(\chi_{k,i}|Z^k) \end{aligned}
X^k∣k=E{X^k∣k−1∣Zk}=k=1∑NX^k∣k−1P(X^k∣k−1∣Zk)=k=1∑Ni=0∑mkX^k∣k−1P(X^k∣k−1∣χk,i,Zk)P(χk,i∣Zk)=i=0∑mkE{X^k∣k−1∣χk,i,Zk}P(χk,i∣Zk)=i=0∑mkX^k∣kiP(χk,i∣Zk)
其中,应用全概率公式,
P
(
X
^
k
∣
k
−
1
∣
Z
k
)
=
∑
i
=
0
m
k
P
(
X
^
k
∣
k
−
1
∣
χ
k
,
i
,
Z
k
)
P
(
χ
k
,
i
∣
Z
k
)
P(\hat{X}_{k|k-1}|Z^k)=\sum_{i=0}^{m_k}P(\hat{X}_{k|k-1}|\chi_{k,i},Z^k)P(\chi_{k,i}|Z^k)
P(X^k∣k−1∣Zk)=∑i=0mkP(X^k∣k−1∣χk,i,Zk)P(χk,i∣Zk),
X
^
k
∣
k
i
\hat{X}_{k|k}^i
X^k∣ki表示在k时刻用第i个测量值对目标进行卡尔曼滤波所得的状态估计,
β
k
,
i
=
P
(
χ
k
,
i
∣
Z
k
)
,
i
=
0
,
1
,
.
.
.
,
m
k
\beta_{k,i}=P(\chi_{k,i}|Z^k),i=0,1,...,m_k
βk,i=P(χk,i∣Zk),i=0,1,...,mk为事件
χ
k
,
i
\chi_{k,i}
χk,i在
Z
k
Z^k
Zk上的条件概率。
(5)目标状态更新协方差
P
^
k
∣
k
=
β
k
,
0
P
^
k
∣
k
−
1
+
[
1
−
β
k
,
0
]
P
k
∣
k
c
+
P
~
k
\hat{P}_{k|k}=\beta_{k,0}\hat{P}_{k|k-1}+[1-\beta_{k,0}]P_{k|k}^c+\tilde{P}_k
P^k∣k=βk,0P^k∣k−1+[1−βk,0]Pk∣kc+P~k
其中,
β
k
,
0
\beta_{k,0}
βk,0表示关联门内没有一个观测来源于目标的概率,作为一个权重值来更新协方差矩阵。
(6)参数
β
k
,
i
\beta_{k,i}
βk,i的求解过程可参考【2】和【3】
3.4 缺点
PDA算法无法准备考虑处在多个目标关联门相交区域中的公共回波会航迹更新的影响,故在回波密集时跟踪性能不理想。
四、联合概率数据关联(Joint Probabilistic Data Association, JPDA)
4.1 算法流程
(1)利用马氏距离建立量测于目标的对应关系,形成确认矩阵
Ω
\Omega
Ω;
(2)对
Ω
\Omega
Ω进行拆分,产生互联矩阵,进而得到互联事件;
(3)计算量测与目标的关联概率:
β
t
j
=
∑
i
P
{
θ
i
∣
Z
k
}
ω
^
t
j
(
θ
)
\beta_{tj}=\sum_{i}P\left\{ \theta_{i}|Z^{k}\right\} \hat{\omega}_{tj}(\theta)
βtj=i∑P{θi∣Zk}ω^tj(θ)
P
{
θ
i
∣
Z
k
}
=
λ
ϕ
c
t
∏
p
=
1
N
p
{
N
[
V
p
(
k
)
]
}
τ
p
∏
t
=
1
N
t
(
P
D
)
δ
t
(
1
−
P
D
)
1
−
δ
t
P\left\{ \theta_{i}|Z^{k}\right\}=\frac{\lambda^{\phi}}{c^{t}}\prod_{p=1}^{N_p} \left\{N\left [ V_p(k)\right ] \right\}^{\tau_p} \prod_{t=1}^{N_t}(P_D)^{\delta _t}(1-P_D)^{1-\delta _t}
P{θi∣Zk}=ctλϕp=1∏Np{N[Vp(k)]}τpt=1∏Nt(PD)δt(1−PD)1−δt
其中,
P
D
P_D
PD是检测目标概率,
N
p
N_p
Np是量测数量,
N
t
N_t
Nt是轨迹数量,
λ
\lambda
λ是错误量测密度,
ϕ
\phi
ϕ是杂波数量,
c
t
c^t
ct是归一化常数,
δ
t
\delta_t
δt是二进制量,
τ
p
\tau_p
τp是二进制量,表示量测是否分配给某个轨迹。
4.2 卡尔曼滤波
利用全概率公式:
v
t
(
k
)
=
∑
j
=
1
m
k
β
t
j
(
k
)
v
t
j
(
k
)
v_t(k) =\sum_{j=1}^{m_k}\beta_{tj}(k)v_{tj}(k)
vt(k)=∑j=1mkβtj(k)vtj(k),则
X
^
t
(
k
∣
k
)
=
X
^
t
(
k
∣
k
−
1
)
+
K
t
(
k
)
v
t
(
k
)
\hat{X}_t(k|k)=\hat{X}_t(k|k-1)+K_t(k)v_t(k)
X^t(k∣k)=X^t(k∣k−1)+Kt(k)vt(k)。
具体算法过程为:
假设对目标的状态向量预测和量测向量预测分别为:
X
^
(
k
∣
k
−
1
)
=
F
(
k
−
1
)
X
^
(
k
−
1
∣
k
−
1
)
\hat{X}(k|k-1)=F(k-1)\hat{X}(k-1|k-1)
X^(k∣k−1)=F(k−1)X^(k−1∣k−1)
Z
^
(
k
∣
k
−
1
)
=
H
(
k
−
1
)
X
^
(
k
∣
k
−
1
)
\hat{Z}(k|k-1)=H(k-1)\hat{X}(k|k-1)
Z^(k∣k−1)=H(k−1)X^(k∣k−1)
则卡尔曼滤波器的更新方程为:
X
^
(
k
∣
k
)
=
X
^
(
k
∣
k
−
1
)
+
K
(
k
)
v
(
k
)
\hat{X}(k|k)=\hat{X}(k|k-1)+K(k)v(k)
X^(k∣k)=X^(k∣k−1)+K(k)v(k)
其中,
v
(
k
)
v(k)
v(k)是组合更新,
K
(
k
)
K(k)
K(k)为滤波器增益矩阵,可以得到:
v
(
k
)
=
∑
p
=
1
N
p
β
t
p
(
k
)
v
t
p
(
k
)
v(k)=\sum_{p=1}^{N_p}\beta_{tp}(k)v_{tp}(k)
v(k)=p=1∑Npβtp(k)vtp(k)
其中,
V
t
p
(
k
)
V_{tp}(k)
Vtp(k)是对轨迹t的第p个量测的更新,
β
t
p
(
k
)
\beta_{tp}(k)
βtp(k)是位置量测p来自于目标t的概率,
N
p
N_p
Np是量测数。
预测状态的协方差为:
P
(
k
∣
k
−
1
)
=
F
(
k
−
1
)
P
(
k
−
1
∣
k
−
1
)
F
⊤
(
k
−
1
)
+
Q
(
k
−
1
)
P(k|k-1)=F(k-1)P(k-1|k-1)F^\top(k-1)+Q(k-1)
P(k∣k−1)=F(k−1)P(k−1∣k−1)F⊤(k−1)+Q(k−1)
更新状态协方差为;
P
(
k
∣
k
)
=
β
0
(
k
)
P
(
k
∣
k
−
1
)
+
[
1
−
β
0
(
k
)
]
P
c
(
k
∣
k
)
+
P
~
(
k
)
P(k|k)=\beta_{0}(k)P(k|k-1)+[1-\beta_0(k)]P^c(k|k)+\tilde{P}(k)
P(k∣k)=β0(k)P(k∣k−1)+[1−β0(k)]Pc(k∣k)+P~(k)
其中,
P
~
(
k
)
=
K
(
k
)
[
∑
p
=
1
N
p
β
t
p
(
k
)
v
t
p
(
k
)
v
t
p
⊤
(
k
)
−
v
(
k
)
v
⊤
(
k
)
]
K
⊤
(
k
)
\tilde{P}(k)=K(k)\left[ \sum_{p=1}^{N_p} \beta_{tp}(k)v_{tp}(k)v^\top_{tp}(k)-v(k)v^\top(k)\right]K^\top(k)
P~(k)=K(k)
p=1∑Npβtp(k)vtp(k)vtp⊤(k)−v(k)v⊤(k)
K⊤(k)
P
c
(
k
∣
k
)
=
[
1
−
K
(
k
)
H
(
k
)
]
P
(
k
∣
k
−
1
)
P^c(k|k)=[1-K(k)H(k)]P(k|k-1)
Pc(k∣k)=[1−K(k)H(k)]P(k∣k−1)
其中,
β
t
p
=
∑
i
P
{
θ
i
∣
Z
k
}
ω
^
t
p
(
θ
i
)
\beta_{tp}=\sum_i P\left\{ \theta_i|Z^k\right\} \hat{\omega}_{tp}(\theta_i)
βtp=∑iP{θi∣Zk}ω^tp(θi),
ω
^
t
p
(
θ
i
)
\hat{\omega}_{tp}(\theta_i)
ω^tp(θi)表示在第i个互联事件中,如果量测j源于目标p,则
ω
^
t
p
\hat{\omega}_{tp}
ω^tp为1;否则,为0.
参考:
【1】数据关联算法之最近邻数据关联(Nearest Neighbor,NN)
【2】传感器融合之数据关联(NN,GNN,PDA,JPDA,JCBB)
【3】概率数据关联(PDA)算法解析
【4】联合概率数据关联(JPDA)算法.ppt