概率数据关联滤波(PDA)
PDA近似基于7条假设:
(1)被跟踪目标总是存在的且没有其他目标存在:单目标存在
(2)目标运动模型遵循线性高斯统计特性:目标运动模型
(3)感兴趣的目标只能产生一个观测:目标单观测
(4)观测噪声为高斯白噪声:传感器观测模型
(5)目标以一定的概率 P d P_d Pd被检测到
(6)所有非目标产生的观测都来自于杂波;杂波空间上服从均匀分布,时间上服从泊松分布:杂波模型
(7)只有落入有效波门内的观测被处理
1 目标运动模型、传感器观测模型和噪声模型
(1)目标状态函数 f ( ⋅ ) f(\cdot) f(⋅)是目标状态的线性函数,满足
x k = F x k − 1 + v k x_k = Fx_{k-1} + v_k xk=Fxk−1+vk
(2)传感器观测也是目标状态的线性函数,满足
y k = H x k + w k y_k = Hx_k + w_k yk=Hxk+wk
(3) v k v_k vk和 w k w_k wk为不相关的零均值高斯白噪声序列,协方差分别为 R k R_k Rk、 Q k Q_k Qk。
(4)目标状态的先验概率密度 p ( x k − 1 ∣ y k − 1 ) p(x_{k-1}|y^{k-1}) p(xk−1∣yk−1)是高斯分布的,均值和协方差为 x ^ k − 1 ∣ k − 1 \hat{x}_{k-1|k-1} x^k−1∣k−1、 P k − 1 ∣ k − 1 P_{k-1|k-1} Pk−1∣k−1。
2 转移概率密度
由于 v k = x k − F x k − 1 v_k = x_k -Fx_{k-1} vk=xk−Fxk−1,转移概率密度为
p ( x k ∣ x k − 1 ) = p v k ( x k − F x k − 1 ) p(x_k|x_{k-1}) = p_{v_k}(x_k- Fx_{k-1}) p(xk∣xk−1)=pvk(xk−Fxk−1)
由于 p v k ( ⋅ ) p_{v_k}(\cdot) pvk(⋅)为高斯分布,转移概率表示为
p ( x k ∣ x k − 1 ) = 1 ( 2 π ) n / 2 ∣ Q k ∣ 1 / 2 exp { − 1 2 ( x k − F x k − 1 ) T Q k − 1 ( x k − F x k − 1 ) } p(x_k|x_{k-1}) = \frac{1}{(2\pi)^{n/2}}{|Q_k|^{1/2}} \exp \left\{ -\frac{1}{2}(x_k-Fx_{k-1})^T Q_k^{-1}(x_k-Fx_{k-1}) \right\} p(xk∣xk−1)=(2π)n/21∣Qk∣1/2exp{
−21(xk−Fxk−1)TQk−1(xk−Fxk−1)}
3 预测概率密度
预测概率密度
p ( x k ∣ y k − 1 , m k − 1 ) = ∫ x k − 1 p v k ( x k − f ( x k − 1 ) ) p ( x k − 1 ∣ y k − 1 , m k − 1 ) d x k − 1 p(x_k|y^{k-1},m^{k-1}) = \int_{x_{k-1}} p_{v_k}(x_k-f(x_{k-1})) p(x_{k-1}|y^{k-1},m^{k-1}) dx_{k-1} p(xk∣yk−1,mk−1)=∫xk−1pvk(xk−f(xk−1))p(xk−1∣yk−1,mk−1)dxk−1
其中,积分第一项 p v k ( x k − f ( x k − 1 ) ) p_{v_k}(x_k-f(x_{k-1})) pvk(xk−f(xk−1))为正态分布函数 N ( x k ; F x k − 1 , Q k ) N(x_k;Fx_{k-1},Q_k) N(xk;Fxk−1,Qk);积分第二项 p ( x k − 1 ∣ y k − 1 , m k − 1 ) p(x_{k-1}|y^{k-1},m^{k-1}) p(xk−1∣yk−1,mk−1)为前一时刻的先验概率密度,可近似为 N ( x k − 1 ; x ^ k − 1 ∣ k − 1 , P k − 1 ∣ k − 1 ) N(x_{k-1};\hat{x}_{k-1|k-1},P_{k-1|k-1}) N(xk−1;x^k−1∣k−1,Pk−1∣k−1)。
预测概率密度可简化为
p ( x k ∣ y k − 1 , m k − 1 ) = N ( x k ; x ^ k ∣ k − 1 , P k ∣ k − 1 ) p(x_k|y^{k-1},m^{k-1}) = N(x_k;\hat{x}_{k|k-1},P_{k|k-1}) p(xk∣yk−1,mk−1)=N(xk;x^k∣k−1,Pk∣k−1)
其中:卡尔曼预测方程 K F p KF_p KFp
[ x ^ k ∣ k − 1 , P k ∣ k − 1 ] = K F p [ x ^ k − 1 ∣ k − 1 , P k − 1 ∣ k − 1 , F , Q ] [\hat{x}_{k|k-1},P_{k|k-1}] = KF_p[\hat{x}_{k-1|k-1},P_{k-1|k-1},F,Q] [x^k∣k−1,Pk∣k−1]=KFp[x^k−1∣k−1,Pk−1∣k−1,F,Q]
4 似然函数
卡方检验函数确定统计距离并确定接收到的观测是否落入最小超椭球门内,这个超椭球门与预测观测的概率分布有关,称其为有效波门,定义为:
G = { y ∈ R n : [ y − y ^ k ∣ k − 1 ] T S ( k ) − 1 [ y − y ^ k ∣ k − 1 ] } G = \{ y \in R^n: [ y - \hat{y}_{k|k-1} ]^T S(k)^{-1} [ y - \hat{y}_{k|k-1} ] \} G={
y∈Rn:[y−y^k∣k−1]TS(k)−1[y−y^k∣k−1]}
其中, γ \sqrt{\gamma} γ为波门大小; y ^ k ∣ k − 1 \hat{y}_{k|k-1} y^k∣k−1为观测预测值; S ( k ) S(k) S(k)为观测预测协方差矩阵。当观测落入有效波门以内称为有效观测。
n维波门体积为
V k = π n / 2 Γ ( n / 2 + 1 ) ∣ S ( k ) ∣ γ 1 / 2 V_k = \frac{\pi^{n/2}}{\Gamma(n/2 +1)}\sqrt{|S(k)|}\gamma^{1/2} Vk=Γ(n/2+1)πn/2∣S(k)∣γ1/2
其中, ∣ S ( k ) ∣ |S(k)| ∣S(k)∣为 S ( k ) S(k) S(k)的行列式。
令 P G P_G PG表示正确观测落入门限的概率,似然函数为
p ( y k ( 1 ) , y k ( 2 ) , ⋯ , y k ( m k ) ∣ x k , m k , θ k ( i ) ) = ( 1 V k ) m k − 1 p ( y k ( i ) ∣ x k ) p(y_k(1),y_k(2),\cdots,y_k(m_k)|x_k,m_k,\theta_k(i)) = \left( \frac{1}{V_k}\right)^{m_k-1}p(y_k(i)|x_k) p(yk(1),yk(2),⋯,yk(mk)∣xk,mk,θk(i))=(Vk1)mk−1p(yk(i)∣xk)
其中, p ( y k ( i ) ∣ x k ) p(y_k(i)|x_k) p(yk(i)∣xk)表示第 i i i个观测源于目标的似然函数,当有效观测空间为观测空间是,其为 N ( y k ( i ) ; H x k , R k ) N(y_k(i);Hx_k,R_k) N(yk(i);Hxk,Rk)。由于有效观测空间是有效波门截断的空间,高斯概率密度函数被截断,积分不在等于1,通过波门概率归一化后仍可认为是观测概率密度函数,即
p ( y k ( i ) ∣ x k ) = 1 P G N ( y k ( i ) ; H x k , R k ) p(y_k(i)|x_k) = \frac{1}{P_G} N(y_k(i);Hx_k,R_k) p(yk(i)∣xk)=PG1N(yk(i);Hxk,Rk)
所以似然函数表示为
p ( y k ( 1 ) , y k ( 2 ) , ⋯ , y k ( m k ) ∣ x k , m k , θ k ( i ) ) = ( 1 V k ) m k − 1 p ( y k ( i ) ∣ x k ) = { ( 1 V k ) m k − 1 p ( y k ( i ) ∣ x k ) ∀ i ≠ 0 ( 1 V k ) m k i = 0 p(y_k(1),y_k(2),\cdots,y_k(m_k)|x_k,m_k,\theta_k(i)) = \left( \frac{1}{V_k}\right)^{m_k-1}p(y_k(i)|x_k) \\ = \begin{cases} \left( \frac{1}{V_k} \right)^{m_k-1} p(y_k(i)|x_k) & \forall i \ne 0 \\ \\ \left( \frac{1}{V_k} \right)^{m_k} & i = 0\\ \end{cases} p(yk(1),yk(2),⋯,yk(mk)∣xk,mk,θk(i))=(Vk1)mk−1p(yk(i)∣xk)=⎩⎪⎪⎨⎪⎪⎧(Vk1)mk−1p(yk(i)∣xk)(Vk1)mk