1. 基础 - 状态空间模型与噪声
状态空间方程
为了进行最优估计，我们首先需要对系统进行数学建模。卡尔曼滤波是在线性状态空间模型的基础上进行的。
状态方程 (State Equation): 描述系统状态如何随时间演变。
- : k时刻的系统状态向量。
- : k-1时刻的系统状态向量。
- : k-1时刻的控制输入向量。
- : 状态转移矩阵，描述状态从k-1到k的变化。
- : 控制输入矩阵，描述控制输入如何影响状态。
- : 过程噪声，代表模型本身无法完全描述的、内在的随机扰动。
观测方程 (Measurement Equation): 描述如何通过传感器等方式观测到系统状态。
- : k时刻的观测向量。
- : 观测矩阵，描述状态如何映射到观测值。
- : 测量噪声，代表测量过程中的不确定性和误差。
噪声模型
卡尔曼滤波的一个核心假设是，过程噪声 和测量噪声 都是高斯白噪声，它们的期望（均值）为0，并且相互独立。
- 过程噪声 服从正态分布：
- 测量噪声 服从正态分布：
其中， 和 是噪声的协方差矩阵。
- 是过程噪声的协方差矩阵。
- 是测量噪声的协方差矩阵。
协方差矩阵的对角线元素是各个噪声分量的方差（即不确定性的平方），非对角线元素是不同噪声分量之间的协方差（即相关性）。
2. 卡尔曼滤波的核心思想：预测与更新
卡尔曼滤波是一个递归的过程，主要包含两个步骤：预测(Predict) 和 更新(Update)。
先验估计 (预测)
在得到k时刻的测量值之前，我们可以根据k-1时刻的状态和系统模型，预测出k时刻的状态。这个预测值被称为先验估计（a priori estimate），记为 。
这里的 是k-1时刻的最优估计值（后验估计）。
后验估计 (更新)
得到k时刻的测量值 后，我们需要利用这个新的信息来修正我们的先验估计 ，从而得到一个更准确的后验估计（a posteriori estimate）。这就是数据融合的过程。
后验估计是先验估计和一个修正项的加权和：
- : k时刻的后验最优估计，是我们的最终输出。
- : 残差（Residual）或新息（Innovation）。它代表了真实测量值与预测测量值之间的差异。如果残差为0，说明我们的预测非常准确。
- : 卡尔曼增益 (Kalman Gain)。这是整个滤波器的核心。它是一个权重，决定了我们应该在多大程度上相信当前的测量值（即残差）。
- 如果 很大，意味着我们更相信测量值。
- 如果 很小，意味着我们更相信模型的预测值。
3. 推导卡尔曼增益
我们的目标是找到一个最优的 ，使得后验估计 尽可能地接近真实值 。
优化目标：最小化估计误差协方差
我们定义后验估计误差 为真实值与后验估计值之差：
我们希望这个误差的总体大小最小。在统计上，我们通过最小化误差的协方差矩阵 的迹 (trace) 来实现这一目标。迹是矩阵对角线元素之和，在这里代表了所有状态变量估计误差的方差之和。
目标：找到 ，使得 最小。
步骤一：建立误差 与 的关系
我们将更新公式代入误差定义中：
再将测量方程 代入：
定义先验估计误差 ，上式可以简化为：
步骤二：展开误差协方差矩阵
现在我们计算 ：
展开上式，得到四项：
根据假设，先验误差 与测量噪声 相互独立，因此它们的交叉项的期望为零：
这样，中间两项就消失了。我们得到：
定义先验误差协方差矩阵 ，并代入测量噪声协方差矩阵 ，得到 的最终表达式：
展开后可得：
步骤三：求迹并对 求导
为了找到使 最小的 ，我们需要对 关于 求导，并令其等于零。
利用矩阵迹的性质 ，我们有 ，因此：
现在对 关于 求导。这里使用两条矩阵求导法则：
- (当B为对称矩阵时)
应用这些法则，我们得到：
因为协方差矩阵 是对称的，所以 。令导数为零：
步骤四：求解
整理上述方程：
最后，两边同时右乘 ，我们便得到了最优卡尔曼增益的计算公式：
