学习卡尔曼滤波的一些粗浅认识以及如何在雷达系统中进行应用

本文章先根据以下两处学习资料对卡尔曼滤波卡尔曼增益的由来做一个简要推导，其次简单分析了以下在雷达系统中应用卡尔曼滤波进行跟踪需要哪些变量。

感谢大佬们的付出，学习资料主要参考以下两个部分：

B站视频: link
CSDN博客：link

1、卡尔曼增益的简单推导

首先借用B站up的PPT里的卡尔曼滤波五个公式：
卡尔曼滤波算法
对于预测部分， x ^ t − 1 {\hat x_{t - 1}} x^t−1​表示 ( t − 1 ) (t-1) (t−1)时刻的状态估计， F F F表示状态转移矩阵， x ^ t − \hat x_t^- x^t−​的 − ^- −号表示该值还不是最佳的状态估计值，待会儿还要根据观测值对该值进行修正。 B B B是控制矩阵，表示控制量 u u u如何作用于当前状态。举链接视频里的例子，假如小车的状态有位置和速度两个变量，用 x t = [ p t , v t ] T {x_t} = {\left[ {{p_t},{v_t}} \right]^T} xt​=[pt​,vt​]T表示，那么做匀加速运动的小车的状态方程可以表示成：
在这里插入图片描述而预测部分的第二个公式表示的是协方差矩阵的更新。上述引用的博客对此作的解释是：基于高斯分布来建立状态变量，对于 ( t − 1 ) (t-1) (t−1)时刻，均值为 x ^ t − 1 {\hat x_{t - 1}} x^t−1​，方差为 P t − 1 {P_{t - 1}} Pt−1​，其中均值被称为最佳估计，这也很好理解，因为均值代表了小车的位置和速度。而 F F F的作用是将上一时刻估计的每个点移动到了新的位置，那么此时刻的均值和方差就分别为 F x ^ t − 1 F{\hat x_{t - 1}} Fx^t−1​， F P t − 1 F T F{P_{t - 1}}F^T FPt−1​FT。
到了更新部分，则需要用观测值来修正估计量了。同样以视频里的小车为例，观测矩阵为：
z t = H x ^ t − + v z_t = H\hat x_t^-+v zt​=Hx^t−​+v
也就是说观测量和预测部分的状态预测 x ^ t − \hat x_t^- x^t−​还有关系。根据博客里的解释，这里也有一个高斯分布的变化，即观测矩阵 H H H作用域状态预测 x ^ t − \hat x_t^- x^t−​上，得到了我们希望看到的观测值的分布，均值为 H x ^ t − H\hat x_t^- Hx^t−​，方差为 H P t − H T H{P_{t}^-}H^T HPt−​HT。
对于实际的观测而言，我们有观测值 z t z_t zt​和测量噪声协方差 R R R。

现在有了两个高斯分布，将这两个高斯分布乘起来会得到我们的滤波结果。根据相乘后高斯分布的均值和方差：

μ ′ = μ 0 + σ 0 2 ( μ 1 − μ 0 ) σ 0 2 + σ 1 2 σ ′ 2 = σ 0 2 − σ 0 4 σ 0 2 + σ 1 2 \mu ' = {\mu _0} + \frac{{\sigma _0^2\left( {{\mu _1} - {\mu _0}} \right)}}{{\sigma _0^2 + \sigma _1^2}}\\ \sigma {'^2} = \sigma _0^2 - \frac{{\sigma _0^4}}{{\sigma _0^2 + \sigma _1^2}} μ′=μ0​+σ02​+σ12​σ02​(μ1​−μ0​)​σ′2=σ02​−σ02​+σ12​σ04​​
令 k = σ 0 2 σ 0 2 + σ 1 2 k = \frac{{\sigma _0^2}}{{\sigma _0^2 + \sigma _1^2}} k=σ02​+σ12​σ02​​，新得到高斯分布的均值、方差可以化简为
μ ′ = μ 0 + k ( μ 1 − μ 0 ) σ ′ 2 = σ 0 2 − k σ 0 2 \mu ' = {\mu _0} + k\left( {{\mu _1} - {\mu _0}} \right)\\ \sigma {'^2} = \sigma _0^2 - k\sigma _0^2 μ′=μ0​+k(μ1​−μ0​)σ′2=σ02​−kσ02​
写成矩阵形式为
K = Σ 0 ( Σ 0 + Σ 1 ) − 1 μ ′ = μ 0 + K ( μ 1 − μ 0 ) Σ ′ = Σ 0 − K Σ 0 \begin{array}{l} {\bf{K}} = {{\bf{\Sigma }}_0}{\left( {{{\bf{\Sigma }}_0} + {{\bf{\Sigma }}_1}} \right)^{ - 1}}\\ {\bf{\mu }}' = {{\bf{\mu }}_0} + {\bf{K}}\left( {{{\bf{\mu }}_1} - {{\bf{\mu }}_0}} \right)\\ {\bf{\Sigma }}{'} = {\bf{\Sigma }}_0 - {\bf{K\Sigma }}_0 \end{array} K=Σ0​(Σ0​+Σ1​)−1μ′=μ0​+K(μ1​−μ0​)Σ′=Σ0​−KΣ0​​
将上面的两个高斯分布的均值和方差 ( μ 0 , Σ 0 ) = ( H x ^ t − , H P t − H T ) \left( {{{\bf{\mu }}_0},{{\bf{\Sigma }}_0}} \right) = \left( {H\hat x_t^ - ,HP_t^ - {H^T}} \right) (μ0​,Σ0​)=(Hx^t−​,HPt−​HT)、 ( μ 1 , Σ 1 ) = ( z t , R ) \left( {{{\bf{\mu }}_1},{{\bf{\Sigma }}_1}} \right) = \left( {{z_t},R} \right) (μ1​,Σ1​)=(zt​,R)带入上式可得
K = H P t − H T ( H P t − H T + R ) − 1 μ ′ = H x ^ t − + K ( z t − H x ^ t − ) Σ ′ = H P t − H T − K H P t − H T \begin{array}{l} {\bf{K}} = HP_t^ - {H^T}{\left( {HP_t^ - {H^T} + R} \right)^{ - 1}}\\ {\bf{\mu }}' = H\hat x_t^ - + {\bf{K}}\left( {{z_t} - H\hat x_t^ - } \right)\\ {\bf{\Sigma }}' = HP_t^ - {H^T} - {\bf{K}}HP_t^ - {H^T} \end{array} K=HPt−​HT(HPt−​HT+R)−1μ′=Hx^t−​+K(zt​−Hx^t−​)Σ′=HPt−​HT−KHPt−​HT​
又由于
μ ′ = H x ^ t Σ ′ = H P t − H T {\bf{\mu }}' = H{{\hat x}_t}\\ {\bf{\Sigma }}' = HP_t^ - {H^T} μ′=Hx^t​Σ′=HPt−​HT
同时将 K {\bf{K}} K中左边第一项的 H H H约掉后，可以化简成
K = P t − H T ( H P t − H T + R ) − 1 x ^ t = x ^ t − + K ( z t − H x ^ t − ) P t = P t − − K H P t − \begin{array}{l} {\bf{K}} = P_t^ - {H^T}{\left( {HP_t^ - {H^T} + R} \right)^{ - 1}}\\ {{\hat x}_t} = \hat x_t^ - + {\bf{K}}\left( {{z_t} - H\hat x_t^ - } \right)\\ {P_t} = P_t^ - - {\bf{K}}HP_t^ - \end{array} K=Pt−​HT(HPt−​HT+R)−1x^t​=x^t−​+K(zt​−Hx^t−​)Pt​=Pt−​−KHPt−​​
也就推出了更新部分的公式。

2、如何在雷达系统中应用应用卡尔曼滤波

接下来阐述如何在实际的雷达系统中应用卡尔曼滤波进行跟踪。从预测和更新两个部分的变量分别进行分析。
1、首先对于预测部分，状态方程由目标的运动方式来建模，例如匀速或匀加速运动就不需要控制变量部分。对于一个三维空间中的目标， x ^ t − 1 \hat x_{t-1} x^t−1​可以用一个九维向量来表示，例如
x ^ t − 1 = [ x 1 , v 1 , a 1 , x 2 , v 2 , a 2 , x 3 , v 3 , a 3 ] T {{\hat x}_{t - 1}} = {[{{x_1}},{{v_1}},{{a_1}},{{x_2}},{{v_2}},{{a_2}},{{x_3}},{{v_3}},{{a_3}} ]^T} x^t−1​=[x1​,v1​,a1​,x2​,v2​,a2​,x3​,v3​,a3​]T
其中， x 1 , x 2 , x 3 x_1,x_2,x_3 x1​,x2​,x3​分别表示 ( t − 1 ) (t-1) (t−1)时刻目标的坐标， v 1 , v 2 , v 3 v_1,v_2,v_3 v1​,v2​,v3​分别表示 ( t − 1 ) (t-1) (t−1)时刻目标沿三个坐标轴方向的速度， a 1 , a 2 , a 3 a_1,a_2,a_3 a1​,a2​,a3​分别表示 ( t − 1 ) (t-1) (t−1)时刻目标沿三个坐标轴方向的加速度。目标的速度和加速度则可以通过连续三帧目标检测的结果进行求解，求得之后可以知道 F F F矩阵。 P P P矩阵距离、方位、俯仰测量精度给出。 Q Q Q矩阵这里如何给我还不太懂。有了以上参数，就可以完成预测部分了。
2、对于更新部分，第一个是 H H H矩阵。 H H H矩阵其实很好给，比如量测得到目标位置，那么可以令 H H H为
H = [ 1 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ; 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 0 ; 0 , 0 , 0 , 0 , 0 , 0 , 1 , 0 , 0 ] ; H=[1,0,0,0,0,0,0,0,0; 0,0,0,1,0,0,0,0,0; 0,0,0,0,0,0,1,0,0]; H=[1,0,0,0,0,0,0,0,0;0,0,0,1,0,0,0,0,0;0,0,0,0,0,0,1,0,0];
z t z_t zt​则通过当前时刻的量测值来给出。 R R R矩阵这里如何给我也不太懂。当这些参数获取完毕后，更新部分也可以进行。
因此，卡尔曼滤波就可以应用在雷达系统中进行跟踪滤波了。