卡尔曼滤波
一些基本概念
滤波：这是信号里面的一个概念，简单说就是从信号里挑出我们想要的部分，同时尽量去掉那些不需要的部分或噪声。
- 卡尔曼滤波它的核心思想其实是把多个含有误差的数据融合在一起。通过这种融合计算，我们能在很大程度上降低误差的影响，最终得到对真实情况更准、更可靠的一个估计结果。这个过程本质上也是提取更干净、更有用的信息，对两个存在误差的数据中有用的部分进行了增强，就像是过滤掉测量中的噪声，所以这玩意也叫做滤波。
先验估计值：指的是在获得某些证据或数据之前，我们对某个参数或者事件的概率分布的估计。就是根据经验猜出来的值
后验估计值：就是说我们一旦获得了新的数据或证据，就可以利用这些信息来更新我们对参数或者事件的估计。这个更新后的概率分布就称为后验估计值。
迹：指的是一个方阵的对角线上元素之和。
状态转移模型 (
F)： 描述系统状态如何从时刻k-1演变到时刻k的数学方程。通常是一个矩阵（状态转移矩阵）。观测模型 (
H)： 描述系统的状态(x)如何映射为我们能观测到的测量值(z)的数学方程（观测矩阵）。它的作用是把预测的状态空间转换到观测空间，以便与实际测量值进行比较和融合。过程噪声 (
w, 协方差Q)： 代表驱动系统动态的模型本身的不准确性和外部扰动。例如风对飞行器的影响、路面摩擦力变化对汽车模型的影响。测量噪声 (
v, 协方差R)： 代表传感器测量的不精确性和噪声。例如GPS的位置误差、陀螺仪的漂移。状态估计协方差 (
P)： 这是卡尔曼滤波的核心“元信息”。它不是一个状态变量本身的值，而是一个矩阵，代表我们对当前状态估计值可信度（不确定性）的量化。对角线上的元素代表各个状态变量估计的方差（不确定度），非对角元素代表状态变量之间估计误差的协方差（相关性）。卡尔曼滤波在每一步都同时更新状态估计 (x) 和其协方差 (P)，协方差P的演变是算法精妙之处所在。卡尔曼增益 (
K)：它是一个权重矩阵，决定了我们更“相信” 预测值还是测量值。- 测量噪声小（
R小 → 传感器精度高），K会更大 → 新测量值在更新中的权重更高。 - 预测噪声小（预测协方差
P小 → 模型预测非常准确/不确定性低），K会更小 → 更倾向于相信预测值。 - 事实上
K是基于预测协方差P、观测模型H和测量噪声协方差R计算出来的。计算方法为$K = P_{predicted} * H^T * (H * P_{predicted} * H^T + R)^-1$
- 测量噪声小（
Previously
卡尔曼滤波，用人话讲就是你有多个不确定的数据，经过一系列的算法过程，然后能获得一个相对准确的结果。
他是一种递归算法，有几个前提条件：
- 当前时刻状态只和上一时刻状态有关。这个讲的高级点就是马尔可夫性
- 模型和系统均满足线性关系
- 噪声符合高斯分布
对于和多时刻状态有关、非线性、非高斯问题，就不能简单地使用卡尔曼滤波。例如使用拓展卡尔曼滤波器进行姿态解算，此时的卡尔曼滤波器是拓展的，适用于非线性的情况。这是后话。
他通过预测和更新两个步骤来实现对系统状态的最优估计：
- 预测：基于前一时刻的状态估计值和当前的系统模型，预测当前时刻的状态
- 更新：当获得当前时刻的观测值后，利用这个信息来修正预测值，得到一个更精确的状态估计值
省流版
我们先看省流版。其实看了这个就直接能拿来用了，有兴趣再往下看。
卡尔曼滤波主要就两个步骤：预测（先验估计）和更新（后验估计）。
预测的时候，我们需要预测状态：
$$\bar{x_{k}}= F x_{k-1} + B u_{k-1} \tag{1}$$
以及使用协方差传播方程预测协方差：
$$\bar{P_{k}} = FP_{k-1}F^{T}+ Q \tag{2}$$
然后求出一个k时刻的卡尔曼增益K：
$$K_k = \bar{P_k} H^T (H \bar{P_k} H^{T + R)^{-1}}\tag{3}$$
然后进行更新步骤。首先用K算出最优估计值：
$$\hat{x_k} = \bar{x_k} + K(z_k - Hx_k^-)\tag{4}$$
同时更新协方差：
$$P_k = (I - KH) \bar{P_k} (I - KH)^T + K R K^{T}\tag{5}$$
然后我们将更新后的$\hat{x_k}$和协方差估计$P_{K}$作为下一个时间步的输入。重复以上的算法过程。这就是卡尔曼滤波的算法过程。
预测
根据上文所述，卡尔曼滤波具有三个前提条件，因此我们可以根据这个条件写出以下的两个公式：
$$x_{k}= F x_{k-1} + B u_{k-1} + w_{k}$$
$$z_{k}= H x_{k} + v_{k}$$
其中$x_{k}$表示k时刻的真实值，是待估计值，而$x_{k-1}$表示k-1时刻的真实值。$u_{k-1}$表示k-1时刻的控制输入量，$z_{k}$表示k时刻的观测值，是我们获取到的传感器的值（请注意他可能和$x_{k}$维度不同）。
可以看出，我们其实并不知道k时刻的噪声，其实就算是模型很精确他多少也存在噪声，因此我们并不能很准确的得到一个真实值$x_k$，所以我们的目标就是获得一个最优的估计值$\hat{x_{k}}$来尽可能接近$x_{k}$，让他的误差达到最小。
对于过程模型来说，我们面临两个问题，就是其实我们并不知道过程模型$w_{k}$和$x_{k-1}$
但我们其实可以直接忽略$w_{k}$。不是因为它不存在，而是因为它的具体值未知且无法直接用来计算，后续我们会用过程噪声协方差 Q 来建模 $w_k$ 的整体统计特性并加以补偿。既然我们没有k-1时刻的真实值，就用我们上一个时刻得到的最好的估计结果 $\hat{x_{k-1}}$ 来近似它。由此可以得到：
$$\bar{x_{k}}= F x_{k-1} + B u_{k-1} \tag{1}$$
我们使用上一时刻的最优估计值$\hat{x_{k-1}}$ 来替代真实值$x_{k-1}$，并将$\bar{x_{k}}$称为当前时刻通过过程模型得到的预测值，也叫做先验估计值。显然，这是一个误差较大的预估值，我们使用观测值来进行修正，这也就是卡尔曼滤波的核心思想：使用观测值修正我们得出的预测值
那他是怎么修正的？请看下文的预测步骤。
最优估计值
其实有两种推导方式，一种是从预测值的角度出发，一种是从观测值的角度出发，本质上都是一样的。都能得到下面的公式：
$$\hat{x_{k}} = x_k^- + K(z_k - Hx_k^-)$$
我们先来看从预测值的角度出发的方法。
预测值
我们这里和上文所述一样，可以忽略掉观测噪声$v_{k}$。因此我们可以得到：
$$z_k = H x_k^{\text{measure}} \Rightarrow x_k^{\text{measure}} = H^{-1} z_k$$
这样我们就得到了一个$x_{k}^{measure}$，可以用它来和我们之前得到的$\bar{x_{k}}$作差，得到一个误差值，用来修正我们的预测值。其中A、B分别代表模型预测值的可信度权重和传感器测量值的可信度权重。
$$
\hat{x}_k = A x_k^- + B x_k^{measure}, \quad A + B = I
$$
当然也有人把这个公式写成下面这样的形式。本质上是相同的。其中G是一个系数矩阵
$$
\hat{x}_k = x_k^- + G \cdot (x_k^{\text{measure}} - x_k^-)
$$
这种写法可以进一步简化。让我们设$G = K \cdot H$，于是有：
$$
\begin{align*}
\hat{x}_k &= \bar{x_k} + G \cdot (x_k^{measure} - \bar{x_k}) \
&= \bar{x_k} + KH(H^{-1}z_k - \bar{x_k}) \
&= \bar{x_k} + K(z_k - H\bar{x_k})\tag{4}
\end{align*}
$$
其中K是卡尔曼增益，$\hat{x}$ 是最优值，因为他是用测量值修正的，这就叫做后验估计值
观测值
和上面的方法一样，我们直接忽略掉噪声$v_{k}$，但是我们这里使用先验估计值$\bar{x_{k}}$来表示真实值，因此我们可以得到以下公式
$$z_{k}^{measure}=H\bar{x_{k}}$$
我们通过预测值和观测矩阵获得了和测量值相同维度的$z_{k}^{measure}$，然后我们就可以用它来获得一个与观测值$z_{k}$的误差值，然后用这个误差来修正我们之前得出的预测值。考虑到观测值和预测值维度可能不同等因素，我们用系数矩阵K进行修正。因此有：
$$
\begin{align*} \
\hat{x}_k &= \bar{x_k} + K(z_k - z_k^{\text{measure}}) \ \
&= \bar{x_k} + K(z_k - H\bar{x_k}) \
\end{align*}
$$
这里的K和上文一样表示卡尔曼增益，因此我们得到了最优估计值和预测值、观测值的关系式，并且希望求出一个合适的K，使得最优估计值最接近真实值。
后验误差
我们只需要把我们上面求出来的最优估计值和我们的真实值$x_{k}$作差就行了。也就是：
$$
\begin{align*}\
x_k - \hat{x}_k &= x_k - \bar{x_k} - K(Hx_k + v_k - H\bar{x_k}) \ \
&= (I - KH)(x_k - \bar{x_k}) - Kv_k \
\end{align*}
$$
我们记$\bar{e_{k}} = x_{k} - \bar{x_{k}}$，$\bar{e_{k}}$表示预测值和真实值的误差，于是可以知道：
$$
e_{k}=(I-KH)\bar{e_{k}}-K v_{k}\tag{b}
$$
于是我们现在的目标是要求解最优目标函数$min_{K}|e_{k}|$。直接通过随机变量的关系式来求解最优目标函数显然不可行，因为他每次都不一样。因此我们需要一个能表征这个随机变量整体统计特性的量作为目标函数，最简单的特征值就是数学期望。我们引入误差协方差矩阵，并设：
- $P_{k} = E[e_{k}e_{k}^{t}]$，表示真实值和最优值的后验误差协方差矩阵。
- $\bar{P_{k}} = E[\bar{e_{k}} \bar{e_{k}^{t}}]$，表示的是真实值和预测值的先验误差协方差矩阵。
根据定义，我们其实可以知道$P_{k}^{t} = P_{k}$，因为$P_k^t = E[(e_k^t e_k^t)^t] = E[(e_k^t)^t e_k^t] = E[e_k e_k^t] = P_k$。并且我们同理可知，$\bar{P_{k}^{t}} = \bar{P_{k}}$
我们一步步来，先讲公式(b)带入到$P_{k}$表达式里面，由此我们可以得到：
$$P_k = E [ ( (I - K H) e_k^- - K v_k ) ( (I - K H) e_k^- - K v_k )^T ]$$
令 $A = I - K H$, $B = -K v_k$（为了书写方便），上式变为：
$$
P_k = E [ (A e_k^- + B) (A e_k^- + B)^T ]
= E [ (A e_k^- + B) (e_k^-T A^T + B^T) ]
$$
此处省略一些中间步骤，我们最终可以得到：
$$P_k = E [ (I - KH) e_k^- e_k^-T (I - KH)^T ] + E [ (I - KH) e_k^- (-v_k^T K^T) ] + E [ (-K v_k) e_k^-T (I - KH)^T ] + E [ K v_k v_k^T K^T ]$$
而我们之前提到过，卡尔曼滤波有一个假设是他过程噪声$w_{k}$和测量噪声$v_{k}$独立同分布，服从高斯分布。所以可以知道：
- $E[\bar{e_k}] = 0, E[v_k] = 0$
- $E[\bar{e_k} v_k^T] = 0$ (独立且不相关)
- $E[v_k \bar{e_k^T}] = 0$(独立且不相关)
所以，$$E [ (I - KH) e_k^- (-v_k^T K^T) ] = (I - KH) E[e_k^- (-v_k^T)] K^T = (I - KH) [0] K^T = 0$$$$E [ (-K v_k) \bar{e_k}T (I - KH)^T ] = -K E[v_k \bar{e_k}T] (I - KH)^T = -K [0] (I - KH)^T = 0$$
化简过后，我们可以得到后验协方差矩阵$P_k$的表达式：
$$P_k = (I - KH) \bar{P_k} (I - KH)^T + K R K^T \tag{a}$$
对于这个公式，我的理解是$I - KH$ 是一个缩小因子，用来调整预测误差对最终估计的影响，然后$(I - KH) P_k^- (I - KH)^T$这个部分代表了校正后的预测不确定性。K 在减小预测误差贡献的同时也改变了它的协方差结构。而$K R K^T$这个部分代表了由测量噪声 (R) 通过卡尔曼增益 K 引入的额外不确定性。
从现在开始，我们的目标就从求解最优目标函数$min_{K}|e_{k}|$变成了找到一个最优的卡尔曼增益矩阵 K，使得后验误差协方差矩阵 $P_K$ 的迹最小化。这里迹反映了状态估计整体误差方差的总和。
求解最优K
上文可知，我们现在的目标是最小化$P_k$的迹，即$J = tr(P_{k})$。
那我们现在就将$P_{k}$代入进去，然后对K求偏导，并令他导数为零。这里过程不再赘述，我们最终可以得到一个公式：
$$K = \bar{P_k} H^T (H \bar{P_k} H^{T + R)^{-1} \tag{3}}$$
这就是著名的卡尔曼增益公式。
协方差矩阵
我们将刚刚获得的最优K代入到公式(a)中，进行化简，则有：
$$P_{k}= (I-K H) \bar{P_{k}} \tag{5}$$
同理，我们可以得到先验误差协方差矩阵
$$\bar{P_{k}} = FP_{k-1}F^{T}+ Q \tag{2}$$
$$\mathscr{END}$$