Kalman 滤波

文章目录

• Kalman 滤波
• 1. 简介
• 2. 极大似然估计
• 3. 状态空间
• 参考

Kalman 滤波

1. 简介

Kalman 滤波常用于目标的追踪、预测等任务。滤波器被设计为最小化均方误差。此外，还可以用极大似然的方法得到滤波器。滤波器就是用来从信号中提取出有用的信息，使用一个损失函数评价滤波器的性能。

2. 极大似然估计

使用极大似然的方法得到一个滤波器，最终的目标就是得到一个$\hat{x}$x^ 使得$y$y的条件概率最大，即
$max [P(y|\hat{x})]$max[P(y∣x^)]
假设信号中的噪声是服从高斯分布的$\mathcal{N}(0,\sigma_k)$N(0,σk​)。假设信号为
$y_k=a_kx_k+n_k$yk​=ak​xk​+nk​
其中$n_k$nk​是噪声，$x_k$xk​是承载数据的信号，$a_k$ak​是系数，那么根据噪声服从高斯分布以及条件概率公式可以得到：
$P(y_k|\hat{x}_k)=K_k \cdot e^{-\left(\frac{(y_k-a_k\hat{x_k})^2}{2\sigma_k^2}\right)}$P(yk​∣x^k​)=Kk​⋅e−(2σk2​(yk​−ak​xk​^​)2​)
对上式求对数得到：
$\log P(y|\hat{x})=-\frac{1}{2}\sum_{k}\left(\frac{(y_k-a_k\hat{x}_k)^2}{\sigma_k^2}\right)+C$logP(y∣x^)=−21​k∑​(σk2​(yk​−ak​x^k​)2​)+C
其中$C$C是常数项。

3. 状态空间

假设目标是要获得如下过程中变量的值：
$x_{k+1}=\Phi x_k+\omega_k$xk+1​=Φxk​+ωk​
其中 $x_k$xk​是$t$t时刻的状态向量，$\Phi$Φ是从时刻 $t$t到时刻 $t+1$t+1的状态转移矩阵，并且是被认为时间平稳的，$\omega_k$ωk​是高斯白噪声。

变量的观察量定义为：
$z_k=H x_k+v_k$zk​=Hxk​+vk​
其中 $z_k$zk​是时刻$t$t的$x$x的观测量，$H$H是将状态向量转换为观测向量的矩阵，并且被认为是时间平稳的，$v_k$vk​是度量误差，是高斯白噪声。

两个噪声的协方差被认为是时间平稳的，分别表示为：
$Q=E[\omega_k \omega_k^T]\\ R=E[v_kv_k^T]$Q=E[ωk​ωkT​]R=E[vk​vkT​]
将$x_k$xk​的估计表示为 $\hat{x}_k$x^k​，使用均方误差衡量滤波器的性能，并表示为
$f(e_k)=(x_k-\hat{x}_k)^2$f(ek​)=(xk​−x^k​)2

设$P_k=E[e_ke_k^T]=E\left[(x_k-\hat{x}_k)(x_k-\hat{x}_k)^T\right]$Pk​=E[ek​ekT​]=E[(xk​−x^k​)(xk​−x^k​)T]，并将之前一次的估计记为$\hat{x}'_k$x^k′​，那么根据历史估计以及误差情况得到下面这个等式：
$\hat{x}_k=\hat{x}'_k+K_k(z_k-H\hat{x}'_k)$x^k​=x^k′​+Kk​(zk​−Hx^k′​)
其中 $K_k$Kk​是Kalman 增益值，后面给出推导公式，后面一项$z_k-H\hat{x}'_k$zk​−Hx^k′​为测量残留(measurement residual)。
将$z_k$zk​的定义替换到上式中得到：
$\hat{x}_k=\hat{x}'_k+K_k(Hx_k+v_k-H\hat{x}'_k)$x^k​=x^k′​+Kk​(Hxk​+vk​−Hx^k′​)
再将$\hat{x}_k$x^k​带入到上式$P_k$Pk​的等式中得到：
$\begin{aligned} &P_k=E\left[\left[(I-K_kH)(x_k-\hat{x}_k')-K_kv_k\right]\left[(I-K_kH)(x_k-\hat{x}_k')\right]\right]\\ &=(I-K_kH)E\left[(x_k-\hat{x}_k')(x_k-\hat{x}_k')^T\right](I-K_kH)+K_kE[v_kv_k^T]K_k^T \end{aligned}$​Pk​=E[[(I−Kk​H)(xk​−x^k′​)−Kk​vk​][(I−Kk​H)(xk​−x^k′​)]]=(I−Kk​H)E[(xk​−x^k′​)(xk​−x^k′​)T](I−Kk​H)+Kk​E[vk​vkT​]KkT​​
将之前的预测$P_k'$Pk′​以及$R$R替换上面等式中的均方误差项以及噪声的协方差项得到：
$P_k=(I-K_kH)P_k'(I-K_kH)^T+K_kRK_k^T$Pk​=(I−Kk​H)Pk′​(I−Kk​H)T+Kk​RKkT​

对于矩阵 $P_k$Pk​，其对角线上包含均方误差项，因此最小化均方误差的目标可以转换为最小化矩阵 $P_k$Pk​的迹(trace)。
首先将上式展开得到

$P_k=P_k'-K_kHP_k'-P_k'H^TK_k^T+K_k(HP_k'H^T+R)K_k^T$Pk​=Pk′​−Kk​HPk′​−Pk′​HTKkT​+Kk​(HPk′​HT+R)KkT​
设$T[]$T[]表示矩阵的迹，则上式两边表示为
$T[P_k]=T[P'_k]-2T[K_kHP_k']+T[K_k(HP_k'H^T+R)K_k^T]$T[Pk​]=T[Pk′​]−2T[Kk​HPk′​]+T[Kk​(HPk′​HT+R)KkT​]
对$K_k$Kk​求导得到：
$\frac{dT[P_k]}{dK_k}=-2(HP_k')^T+2K_k(HP_k'H^T+R)$dKk​dT[Pk​]​=−2(HPk′​)T+2Kk​(HPk′​HT+R)
并令其等于0，得到：
$(HP'_k)^T=K_k(HP_k'H^T+R)$(HPk′​)T=Kk​(HPk′​HT+R)
并最终得到Kalman增益等式：
$K_k=P_k'H^T(HP_k'H^T+R)^{-1}$Kk​=Pk′​HT(HPk′​HT+R)−1
将该等式带入到上面$P_k$Pk​的等式中，得到：
$\begin{aligned} &P_k=P_k'-P_k'H^T(HP_k'H^T+R)^{-1}HP_k'\\ &=P_k'-K_kHP_k'\\ &=(I-K_kH)P_k' \end{aligned}$​Pk​=Pk′​−Pk′​HT(HPk′​HT+R)−1HPk′​=Pk′​−Kk​HPk′​=(I−Kk​H)Pk′​​

前面已经定义了关于变量$x_k$xk​从时刻$k$k到时刻 $k+1$k+1的转移：
$x_{k+1}'=\Phi\hat{x}_k$xk+1′​=Φx^k​
对于均方误差中的 $e_k$ek​，更新方法为：
$\begin{aligned} &e_{k+1}'=x_{k+1}-\hat{x}'_{k+1}\\ &=(\Phi x_k+\omega_k)-\Phi \hat{x}_k\\ &=\Phi e_k+\omega_k \end{aligned}$​ek+1′​=xk+1​−x^k+1′​=(Φxk​+ωk​)−Φx^k​=Φek​+ωk​​

和前面$P_k$Pk​的等式类似，$k+1$k+1时刻：
$\begin{aligned} &P_{k+1}'=E[e^{'}_{k+1} e_{k+1}^{'T}]\\ &= E\left[\Phi e_k(\Phi e_k)^T\right]+E[\omega_k \omega_k^T]\\ &=\Phi P_k\Phi^T+Q \end{aligned}$​Pk+1′​=E[ek+1′​ek+1′T​]=E[Φek​(Φek​)T]+E[ωk​ωkT​]=ΦPk​ΦT+Q​

算法的整体流程如下图所示：

参考

[1] The Kalman filter