最近邻滤波法
最近邻滤波法(NNF)
5条假设：
（1）真实目标时存在且总能被检测到
（2）距离观测预测最近的观测值来源于目标
（3）其他观测源于杂波
（4）目标运动特性遵循线性高斯统计特性
总结：观测 y k y_k yk中，只有统计距离于预测的观测距离最近的那个观测 y k ( i ) y_k(i) yk(i)被认为源于目标的观测。
-
当杂波密集时，性能会变差。
-
这种方法没有解释这个事实：用来更新目标航迹的观测可能与目标不相观，但是门限内的任何观测都有可能与目标相关。
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 ] x ^ ( k ∣ k − 1 ) = F ( k − 1 ) x ^ ( k − 1 ∣ k − 1 ) P ( k ∣ k − 1 ) = F ( k − 1 ) P ( k − 1 ∣ k − 1 ) F T ( k − 1 ) + Q ( k − 1 ) \begin{aligned}&[\hat{x}_{k|k-1},P_{k|k-1}] = KF_p[\hat{x}_{k-1|k-1},P_{k-1|k-1},F,Q]\\\\&\hat{x}(k|k-1) = F(k-1)\hat{x}(k-1|k-1)\\\\&P(k|k-1) = F(k-1)P(k-1|k-1)F^T(k-1)+Q(k-1)\end{aligned} [x^k∣k−1,Pk∣k−1]=KFp[x^k−1∣k−1,Pk−1∣k−1,F,Q]x^(k∣k−1)=F(k−1)x^(k−1∣k−1)P(k∣k−1)=F(k−1)P(k−1∣k−1)FT(k−1)+Q(k−1)
4 似然函数
NNF似然函数：选择 y k y_k yk中的 y k ( i ) y_k(i) yk(i)来近似
-
对 y k ( i ) y_k(i) yk(i)的选择依据是观测与观测预测的统计距离。
-
由于过程噪声和观测噪声是高斯分布的，可通过卡方检验函数确定统计距离。
所有观测中只有一个观测值关联和更新目标航迹，选取方法如下：
y k ( i ) = arg min y k ( j ) , ∀ j ∈ { 1 , ⋯ , m k } [ y k ( i ) − H x ^ k ∣ k − 1 ] T S k ∣ k − 1 − 1 [ y k ( i ) − H x ^ k ∣ k − 1 ] y_k(i) = \mathop{\arg\min}_{y_k(j),\forall j \in \{ 1,\cdots ,m_k\}} [y_k(i) - H\hat{x}_{k|k-1}]^T S_{k|k-1}^{-1} [y_k(i) - H\hat{x}_{k|k-1}] yk(i)=argminyk(j),∀j∈{1,⋯,mk}[yk(i)−Hx^k∣k−1]TSk∣k−1−1[yk(i)−Hx^k∣k−1]
其中
S k ∣ k − 1 = H P k ∣ k − 1 H T + R k S_{k|k-1} = HP_{k|k-1}H^T + R_k Sk∣k−1=HPk∣k−1HT+Rk
5 归一化因数
归一化因数为
p ( y k , m k ∣ y k − 1 , m k − 1 ) = ∫ x k p ( y k , m k ∣ x k , y k − 1 , m k − 1 ) p ( x k ∣ y k − 1 , m k − 1 ) p(y_k,m_k|y^{k-1},m^{k-1}) = \int_{x_k} p(y_k,m_k|x_k,y^{k-1},m^{k-1})p(x_k|y^{k-1},m^{k-1}) p(yk,mk∣yk−1,mk−1)=∫xkp(yk,mk∣xk,yk−1,mk−1)p(xk∣yk−1,mk−1)
其中，积分函数第一项为 N ( y k ( i ) ; H x ^ k ∣ k − 1 , R k ) N(y_k(i);H\hat{x}_{k|k-1},R_k) N(yk(i);Hx^k∣k−1,Rk)，第二项为 N ( x k ; x ^ k ∣ k − 1 , P k ∣ k − 1 ) N(x_k;\hat{x}_{k|k-1},P_{k|k-1}) N(xk;x^k∣k−1,Pk∣k−1),可得
p ( y k , m k ∣ y k − 1 , m k − 1 ) = N ( y k ( i ) ; H x ^ k ∣ k − 1 , S k ∣ k − 1 ) p(y_k,m_k|y^{k-1},m^{k-1}) = N(y_k(i);H\hat{x}_{k|k-1},S_{k|k-1}) p(yk,mk∣yk−1,mk−1)=N(yk(i);Hx^k∣k−1,Sk∣k−1)
6 条件概率密度
后验概率密度为
p ( x k ∣ y k , m k ) = N ( y k ( i ) ; H x ^ k , R k ) N ( x k ; x ^ k ∣ k − 1 , P k ∣ k − 1 ) N ( y k ( i ) ; H x ^ k ∣ k − 1 , S k ∣ k − 1 ) = N ( x k , x ^ k ∣ k , P k ∣ k ) \begin{aligned} p(x_k|y^k,m^k) &= \frac{N(y_k(i);H\hat{x}_k,R_k)N(x_k;\hat{x}_{k|k-1},P_{k|k-1})}{ N(y_k(i);H\hat{x}_{k|k-1},S_{k|k-1}) } \\ &= N(x_k,\hat{x}_{k|k},P_{k|k}) \\ \end{aligned} p(xk∣yk,mk)=N(yk(i);Hx^k∣k−1,Sk∣k−1)N(yk(i);Hx^k,Rk)N(xk;x^k∣k−1,Pk∣k−1)=N(xk,x^k∣k,Pk∣k)
其中
[ x ^ k ∣ k , P k ∣ k ] = K F E [ y k ( i ) , x ^ k ∣ k − 1 , P k ∣ k − 1 , H , R ] X ^ ( k ∣ k ) = X ^ ( k ∣ k − 1 ) + K ( k ) v ( k ) v ( k ) = z ~ ( k ∣ k − 1 ) = z ( k ) − z ~ ( k ∣ k − 1 ) K ( k ) = P x z P z x − 1 = { P ( k ∣ k − 1 ) H T ( k ) S − 1 ( k ) P ( k ∣ k ) H T ( k ) R − 1 ( k ) \begin{aligned}&[\hat{x}_{k|k},P_{k|k}] = KF_E[y_k(i),\hat{x}_{k|k-1},P_{k|k-1},H,R]\\\\&\hat{X}(k|k) = \hat{X}(k|k-1) + K(k)v(k) \\\\&v(k) = \tilde{z}(k|k-1) = z(k) - \tilde{z}(k|k-1) \\\\&K(k) = P_{xz}P_{zx}^{-1} = \begin{cases} P(k|k-1)H^T(k)S^{-1}(k)\\ \\ P(k|k)H^T(k)R^{-1}(k)\\\end{cases}\end{aligned} [x^k∣k,Pk∣k]=KFE[yk(i),x^k∣k−1,Pk∣k−1,H,R]X^(k∣k)=X^(k∣k−1)+K(k)v(k)v(k)=z~(k∣k−1)=z(k)−z~(k∣k−1)K(k)=PxzPzx−1=⎩⎪⎨⎪⎧P(k∣k−1)HT(k)S−1(k)P(k∣k)HT(k)R−1(k)
7 最近邻滤波方程
（1）预测
[ 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]
（2）观测选择
y k ( i ) = arg min y k ( j ) , ∀ j ∈ { 1 , ⋯ , m k } [ y k ( i ) − H x ^ k ∣ k − 1 ] T S k − 1 ∣ k − 1 − 1 [ y k ( i ) − H x ^ k ∣ k − 1 ] y_k(i) = \mathop{\arg\min}_{y_k(j),\forall j \in \{ 1,\cdots ,m_k\}} [y_k(i) - H\hat{x}_{k|k-1}]^T S_{k-1|k-1}^{-1} [y_k(i) - H\hat{x}_{k|k-1}] yk(i)=argminyk(j),∀j∈{1,⋯,mk}[yk(i)−Hx^k∣k−1]TSk−1∣k−1−1[yk(i)−Hx^k∣k−1]
其中，$ S_{k-1|k-1}{-1}=HP_{k|k-1}HT + R_k$
（3）航迹估计输出
[ x ^ k ∣ k , P k ∣ k ] = K F E [ y k ( i ) , x ^ k ∣ k − 1 , P k ∣ k − 1 , H , R ] [\hat{x}_{k|k},P_{k|k}] = KF_E[y_k(i),\hat{x}_{k|k-1},P_{k|k-1},H,R] [x^k∣k,Pk∣k]=KFE[yk(i),x^k∣k−1,Pk∣k−1,H,R]