最近邻滤波法

最近邻滤波法(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=Fxk1+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(xk1yk1)时高斯分布的,均值和协方差为 x ^ k − 1 ∣ k − 1 \hat{x}_{k-1|k-1} x^k1k1 P k − 1 ∣ k − 1 P_{k-1|k-1} Pk1k1

2 转移概率密度

由于 v k = x k − F x k − 1 v_k = x_k -Fx_{k-1} vk=xkFxk1,转移概率密度为
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(xkxk1)=pvk(xkFxk1)
由于 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(xkxk1)=(2π)n/21Qk1/2exp{21(xkFxk1)TQk1(xkFxk1)}

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(xkyk1,mk1)=xk1pvk(xkf(xk1))p(xk1yk1,mk1)dxk1
其中,积分第一项 p v k ( x k − f ( x k − 1 ) ) p_{v_k}(x_k-f(x_{k-1})) pvk(xkf(xk1))为正态分布函数 N ( x k ; F x k − 1 , Q k ) N(x_k;Fx_{k-1},Q_k) N(xk;Fxk1,Qk);积分第二项 p ( x k − 1 ∣ y k − 1 , m k − 1 ) p(x_{k-1}|y^{k-1},m^{k-1}) p(xk1yk1,mk1)为前一时刻的先验概率密度,可近似为 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(xk1;x^k1k1,Pk1k1)

预测概率密度可简化为
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(xkyk1,mk1)=N(xk;x^kk1,Pkk1)
其中:卡尔曼预测方程 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^kk1,Pkk1]=KFp[x^k1k1,Pk1k1,F,Q]x^(kk1)=F(k1)x^(k1k1)P(kk1)=F(k1)P(k1k1)FT(k1)+Q(k1)

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^kk1]TSkk11[yk(i)Hx^kk1]
其中
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 Skk1=HPkk1HT+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,mkyk1,mk1)=xkp(yk,mkxk,yk1,mk1)p(xkyk1,mk1)
其中,积分函数第一项为 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^kk1,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^kk1,Pkk1),可得
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,mkyk1,mk1)=N(yk(i);Hx^kk1,Skk1)

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(xkyk,mk)=N(yk(i);Hx^kk1,Skk1)N(yk(i);Hx^k,Rk)N(xk;x^kk1,Pkk1)=N(xk,x^kk,Pkk)
其中
[ 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^kk,Pkk]=KFE[yk(i),x^kk1,Pkk1,H,R]X^(kk)=X^(kk1)+K(k)v(k)v(k)=z~(kk1)=z(k)z~(kk1)K(k)=PxzPzx1=P(kk1)HT(k)S1(k)P(kk)HT(k)R1(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^kk1,Pkk1]=KFp[x^k1k1,Pk1k1,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^kk1]TSk1k11[yk(i)Hx^kk1]
其中,$ 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^kk,Pkk]=KFE[yk(i),x^kk1,Pkk1,H,R]

你可能感兴趣的:(最近邻滤波法)