卡尔曼滤波--学习笔记

Kalman滤波算法的本质就是利用两个正态分布的融合仍是正态分布这一特性进行迭代，其核心就是5个算法公式：

   (1)

     (2)

(3)

(4)

(5)

公式分析：

公式1：卡尔曼滤波模型假设k时刻的真实状态是从(k − 1)时刻的状态演化而来，Xk是时刻k的真实值

• Fk 是作用在 Xk−1 上的状态变换模型（/矩阵/矢量）。
• Bk 是作用在控制器向量uk上的输入－控制模型。
• Wk 是过程噪声，并假定其符合均值为零，协方差矩阵为Qk的多元正态分布。

公式2：Zk是时刻k的测试值

其中Hk是观测模型,它把真实状态空间映射成观测空间，vk 是观测噪声，其均值为零，协方差矩阵为Rk,且服从正态分布

初始状态以及每一时刻的噪声{x0, w1, ..., wk, v1 ... vk} 都认为是互相独立的.

卡尔曼滤波要做的是：

已知：

1. 系统的初始状态 x0

2. 每个时间的测量 Z

3. 系统模型和测量模型

求解：

状态x随着时间变化而产生的值

预测过程包括两个方面：

一、由上一个更新值 Xk-1|k-1 预测这一个预测值 Xk|k-1

二、由上一个更新值和真实值之间的误差 Pk-1|k-1 预测下一个预测值和真实值之间的误差 Pk|k-1

具体来说，就是以下两个方程：

        ···········（6）预测状态，该公式可直接由公式 1  导出

   ············（7）预测估计协方差矩阵

这里：

Xk-1|k-1 这种记法代表的是上一次的更新值，后面一个 k-1可以看做 Zk-1， 也就是上一次经过对比Zk-1(实际就是更新)之后所估计出的状态Xk-1。

Xk|k-1 这种记法代表这一次的预测值， 同理于刚才的介绍， 经过上一次Zk-1之后所估计出的状态Xk。

公式7证明：P代表估计误差的协方差

公式3,4,5属于更新方程，其中K称为kalman增益， 就像一个补偿，决定着预测值应该变化多少幅度，才能变成更新值。而公式5则是K的更新公式

更新过程实际上就是一下问题：

已知：

1. 由上一个更新值得到的当前的预测值。
2. 当前的观测值
3. 观测模型

求解：

1. 融合了预测值和观测的更新值
2. 由预测值的估计误差得到更新值的估计误差

[Math]理解卡尔曼滤波器 (Understanding Kalman Filter)有详细解释，同时有有1个小车的示例来讲解如何通过两个高斯分布相乘成为一个高斯分布，以及K，H参数的确定

高斯噪声的偏差，最优值的偏差等偏差的计算可参考 卡尔曼滤波的原理说明