Kalman Filter
算法
这是一篇卡尔曼滤波器的笔记。在后文中对卡尔曼滤波器进行简单介绍，重点放在证明推导上。
首先给出卡尔曼滤波器的五个关键方程：
前两个为计算预测的置信度。
称为卡尔曼增益。这里的
是状态变量的先验估计（预测）值，而
是后验估计值。
是状态变量先验误差的协方差，
是后验误差协方差。这五个方程中的前两个是预测步骤，后三个是测量更新步骤。
1. 假设
卡尔曼滤波器对系统的状态进行预测和矫正，首先我们需要给出系统的模型以及其观测模型。卡尔曼滤波器假设被估计的系统是线性动态系统。这个系统会受到噪声的影响，而这些噪声假设都是独立的零均值的高斯白噪声。
1.1 系统模型
系统模型描述系统真实状态是如何随时间转移的。卡尔曼滤波器通过系统模型去预测下一时刻的状态。这里给出线性系统的模型如下：
系统在
时刻的真实状态
取决于上一时刻的状态
，系统的控制输入以及噪声
。为了简化，这里的控制输入
可以省略，并不影响卡尔曼滤波的过程。
1.2 观测模型
观测模型描述了测量变量和系统状态之间的关系。卡尔曼滤波器通过观测模型去矫正预测的状态。
实际的测量
取决于
时刻的系统状态
。矩阵
描述了在没有噪声影响下（传感器）测量和系统实际状态之间的关系。而实际情况往往会有噪声的存在，这里用
表示。
1.3 噪声模型
在系统模型和观测模型中都会受到噪声的影响。这里的噪声假设都是独立的零均值的高斯白噪声。这里的随机变量和服从：
它们的协方差也就是随机变量的二阶矩：
2. 测量更新过程
卡尔曼增益是卡尔曼滤波器中十分关键的一个参数矩阵。假设我们已经有测量更新方程如下：
在已有预测（先验）值的情况下，用新的测量值对预测进行矫正。这里我们需要求出增益矩阵
。我们期望估计得到的系统状态
和实际的状态
之间的误差越小越好，估计的后验误差为：
我们给出优化方程为最小化
后验估计误差的期望：
把状态更新方程
和测量方程
代入
有:
我们可以计算后验误差的协方差矩阵：
由于观测误差和状态变量是相互独立的，所以他们联合分布的期望都为
，即有
后验误差的协方差矩阵
可以展开为：
这里可以看到，后验估计误差协方差矩阵的迹就等于后验估计误差的均方误差，也就是
这里等同于找到一个使得的迹最小。为计算得到，把优化方程对做一阶偏导数，取偏导值为位置处的就是我们所求的最优的解。
对其迹求一阶偏导有:
这里涉及对矩阵的迹求偏导，可以参考The Matrix Cookbook。这里用到的矩阵迹的偏导公式有：
考虑
和
都是对称阵，从而有：
令式子
为
，解得卡尔曼增益：
把
代入
得：
也就是卡尔曼滤波中的误差协方差更新方程
:
3. 预测过程
上一部分已经对对测量更新（矫正）部分的三个公式进行了推导。接下来我们看预测过程的两个公式。
首先我们看状态转移方程：
当我们知道前一时刻的状态，可以推算得到当前时刻的状态。我们计算当前状态的期望：
以及协方差：
计算协方差的时候，由于系统状态和误差是不相关的，所以他们联合分布的期望都是
，这里直接略去了。这里计算的是预测的置信度，也就有卡尔曼滤波器中预测步骤的两个公式
和
：
4. 误差分析
我们有先验误差：
\begin{equation}
\mathbf{\check e}_k \equiv \mathbf{\check x}_k - \mathbf x_k\label{eq:error_prior}
\end{equation}
以及
中的后验误差
：
把和代入，把和代入，有:
由
，所以只要
，则对
就有
。也就是说假设初始状态的后验估计是真值，那么卡尔曼滤波之后的步骤就都是无偏的。
我们计算误差向量的二阶距，也就是得到误差的协方差：
假设
，有
由于系统误差、观测误差和状态变量是相互独立的，所以他们联合分布的期望都为
，上述两式就给出了卡尔曼滤波器中的误差预测方程
以及误差更新方程
:
这里需要注意的是卡尔曼滤波的初值。如给给的初值和真值一致，那么系统观测误差就是无偏的，也就算滤波稳定的。但是这样的初值很难得到。如果系统是一致完全可控和一致完全可观的话，那么卡尔曼滤波就是一致渐进稳定的，也就是说这种情况下随意选取的初值不会影响最终的估计值。
Reference
- Tutorial: The kalman filter
- An Introduction to the Kalman Filter
- Understanding the basis of the kalman filter via a simple and intuitive derivation
- State Estimation for Robotics
- Robot Localization and Kalman Filters
- The Matrix Cookbook
