卡尔曼滤波器

卡尔曼滤波可以看这篇文章：http://web.mit.edu/kirtley/kirtley/binlustuff/literature/control/Kalman%20filter.pdf

基本假设

卡尔曼滤波器的基本假设是，对于t时刻一个状态值 $x_t$ ,注意状态值是一个n维向量。它是通过某种转换关系从上一个时刻的状态 $x_{t-1}$ 转换过来的，并且这个转换过程是存在白噪的，也就是：

$$\begin{equation} x_t = \Phi x_{t-1} + \omega_t \end{equation}$$

同时，卡尔曼滤波认为$x_t$的观测值$z_t$（观测值是一个m维向量，m和n不一定相等，取决于测量方法）与其状态值也存在一个关系：

$$z_t = Hx_t + v_t$$

这里显然$v_t​$就是测量误差，这里也认为这个测量误差符合高斯分布。 卡尔曼滤波认为这两个转换关系中的$\Phi​$和$H​$都是不随着时间改变的，是一个固定的值，由所建模的物理问题所决定，不需要每一帧重新计算，至于初始值是什么，就需要看卡尔曼滤波建模的实际物理量是什么了，比如说这个物理量是速度的话，且是匀加速运动，由于两帧之间的速度关系由加速度来决定，那么加速度就由$\Phi​$来进行建模。

问题定义

定义了这么多，实际上我们要怎样从每一帧及其历史的观测值找到实际的$x_t$呢？答案是，找不到真实的$x_t$, 只能找到它的最优估计值$\hat{x_t}$，定义计算到的$\hat{x_t}$与实际的$x_t$的误差为$e_t = x_t- \hat{x_t}$， 显然，我们的目标函数可以定义为：

$$\tag{loss} \min\sum_i^n E(e_{t,i}^2)$$

也就是每一维误差的平方和。
换一种角度，用概率的思维可以表达为最大似然函数，也就是给定观测值，求实际值的最大似然：

$$\max P(\hat{x_t}|z_t)$$

我们认为测量误差$v_t$符合高斯分布，那么

$$\begin{align} \max P(\hat{x_t}|z_t) &= \max P(z_t/H - v_t/H|z_t) \\ &=\max P(v_t|z_t) \\ &= \max P(v_t) \\ &= \max C_t e^{\frac{-(z_t-Hx_t)^2}{2\sigma^2}} \\ \end{align}$$

其中$C_t$是高斯分布的常数项，最大化多帧似然函数为：

$$\max \Pi_t{P(\hat{x_t}|z_t)}$$

取对数改连乘为连加，再对$\sigma_t$求导即可求出每一帧最优的$\sigma_t$

卡尔曼滤波变量定义

以均方误差为例，我们优化的目标是：

$$\min\sum_i^n E(e_{t,i}^2)$$

为了获得这个最小的均方误差，我们先定义三个协方差矩阵：

$$\begin{align} Q &= E[\omega_t\omega_t^T] \\ R &= E[v_tv_t^T] \\ P_t &= E[e_te_t^T] \\ &= E[(x_t-\hat{x_t})(x_t-\hat{x_t})^T] \end{align}$$

那么我们需要最小化的就是$P_t$的迹$Tr(P_t)$。
现在我们使用类似迭代证明的方式来进行说明，假如通过某种途径，我们初步获得了$x_t$的估计值，记为$\hat{x’_t}$，这时候需要通过卡尔曼滤波来进一步优化这样的一个结果。假如我们认为每一步的精细化都是一种线性变换，那么精细化的公式都是类似于$\hat{x_t}=a\hat{x’_t} + b$这样的形式，卡尔曼滤波认为更加精细化的结果可以通过以下的式子获得：

$$\hat{x_t} = \hat{x'_{t}} + K_t(z_t - H\hat{x'_{t}})$$

这条公式为什么成立下面会说明。其中$K_t$称之为Kalman gain, 记所谓的innovation 或measurement residual为

$$i_t = z_t - H\hat{x'_{t}}$$

将$z_t$代入得：

$$\hat{x_t} = \hat{x'_{t}} + K_t(Hx_t+ v_t - H\hat{x'_{t}})$$

现在我们知道了怎样精细化对$x_t$的预测，那么接下来的问题就是解决$K_t$以及消除$x_t$了。由优化目标，我们应该选择一个$K_t$使其最小化$Tr(P_t)$，因此我们需要获得$P_t$关于$K_t$的式子，再进行求导计算。
由于：

$$x_t-\hat{x_t} = x_t-\hat{x'_{t}} - K_t(Hx_t + v_t - H\hat{x'_{t}}) = (I-K_tH)(x_t-\hat{x'_{t}})-K_tv_t$$

将$\hat{x_t}$代入$P_t$中，注意到噪声$v_t$和$x_t$以及$\hat{x'_{t}}$显然都不相关，因此:

$$\begin{align} P_t &= E[(x_t-\hat{x_t})(x_t-\hat{x_t})^T] \\ &= (I-K_tH)E[(x_t-\hat{x_t})(x_t-\hat{x_t})^T](I-K_tH) + K_tE[v_tv_t^T]K_t^T \\ &= (I-K_tH)P'_t(I-K_tH) + K_tRK_t^T \\ &= P'_t - K_tHP'_t - P'_tH^TK_t^T + K_t(HP'_kH^T+R)K_t^T \end{align}$$

上式用$P’_t$消除了对$x_t$的依赖，而$P’_t$同$\hat{x’_t}$一样认为是通过某种途径初步获得的，两者在迭代一开始的初始值一般是0。将$P_t$的迹与$K_t$进行求导，注意到公式：

$$\nabla_ATr(AB) = B^T$$

我们有：

$$\frac{dTr(P_t)}{dK_t} = -2(HP'_t)^T + 2K_t(HP'_kH^T+R)$$

使之为0，求得最优的$K_t$为：

$$K_t = P'_tH^T(HP'_tH^T+R)^{-1}$$

其中记$S_t = HP’_tH^T+R$。
将$K_t$代入$P_t$, 可以消除后两项得：

$$\begin{align} P_t &= P'_t - K_tHP'_t \\ &= P'_t - P'_tH^T(HP'_tH^T+R)^{-1}HP'_t \\ &= (I-K_tH)P'_k \\ \end{align}$$

我们现在获得了同一帧之间所有变量的迭代优化公式，现在需要将前后两帧联系起来。卡尔曼滤波认为，下一帧的粗略估计值与上一帧的精细估计值存在这样的关系：

$$\hat{x}'_{t+1} = \Phi \hat{x_t}$$

根据这一假设，我们能得到所有其他的粗略估计值：

$$\begin{align} e'_{t+1} &= x_{t+1}-x'_{t+1} \\ &=(\Phi x_t + \omega_t) - \Phi \hat{x}_t \\ &= \Phi e_t + \omega_t \\ \end{align}$$ $$\begin{align} P'_{t+1} &= E[(\Phi e_t+\omega_t)(\Phi e_t+\omega_t)^T] \\ &= E[(\Phi e_t)(\Phi e_t)^T] + E[\omega_t\omega_t^T] \\ &= \Phi P_t\Phi + Q \end{align}$$

整体流程

• 初始化$P’_0, e’_0, \hat{x’_0}$为0，或者为其他数字，这是根据建模需求的求出来的
• 计算$K_t = P’_tH^T(HP’_tH^T+R)^{-1}$
• 计算出当前帧调整后的状态 $$\hat{x_t} = \hat{x'_{t}} + K_t(Hx_t+ v_t - H\hat{x'_{t}})$$
• 更新卡尔曼滤波的参数
  • $P_t = P’_t - K_tHP’_t$
• 准备下一帧的粗略估计值
  • $\hat{x}’_{t+1} = \Phi \hat{x_t}​$
  • $P’_{t+1}= \Phi P_t\Phi + Q$

小结

可以看出，卡尔曼滤波的更新跟状态量一点关系都没有，即使没有状态量，卡尔曼滤波每一帧的参数更新都是固定的。这也是为什么称之为滤波器的原因之一。