1、卡尔曼滤波器简介
卡尔曼滤波器是一种由卡尔曼（Kalman）提出的用于时变线性系统的递归滤波器。这个系统可用包含正交状态变量的微分方程模型来描述，这种滤波器是将过去的测量估计误差合并到新的测量误差中来估计将来的误差。
卡尔曼滤波器具有如下特点:
（1）卡尔曼滤波处理的对象是随机信号:
（2）被处理信号无有用和干扰之分，滤波的目的正是估计出所有被处理信号
（3）系统的白噪声激励和量测噪声并不是需要滤掉的对象，它们的统计特性正是估计过程中需要利用的信息
（4）实现简单，是一个纯时域的滤波器无需变换
所以确切地说，卡尔曼滤波与其他一般的滤波器不同（如高通，低通，带通等），其是一种最优估计理论和算法。就实现形式而言，卡尔曼滤波器实质上是一种由计算机实现的递推算法，每个递推过程中包含两个过程——时间更新和状态更新；从计算过程来看，包含两个回路——滤波计算回路和增益计算回路。
2、卡尔曼滤波器的五个公式及其含义
2.1、先从卡尔曼滤波器的两个更新过程来认识卡尔曼滤波器：
其中，各个物理量的含义：
先验状态估计公式：
状态是们所观测的物理量，在已知上次状态的基础上，通过物理机理和经验推测我们可以推测出本次的状态，但由于推测过程中存在量测噪声等不确定因素，因此先验状态估计不够准确，不是最优估计。
x^−:为先验状态估计，即根据物理机理和上一次的状态所计算估计出的本次状态量 x ^ − : 为 先 验 状 态 估 计 ， 即 根 据 物 理 机 理 和 上 一 次 的 状 态 所 计 算 估 计 出 的 本 次 状 态 量
x^k−1:为上一次的最优估计量 x ^ k − 1 : 为 上 一 次 的 最 优 估 计 量
uk−1:为上一次的系统控制量 u k − 1 : 为 上 一 次 的 系 统 控 制 量
A:为状态转移矩阵，表示从上一时刻的状态来推测当前时刻的状态 A : 为 状 态 转 移 矩 阵 ， 表 示 从 上 一 时 刻 的 状 态 来 推 测 当 前 时 刻 的 状 态
B:为控制矩阵，表示控制量u如何作用于当前状态 B : 为 控 制 矩 阵 ， 表 示 控 制 量 u 如 何 作 用 于 当 前 状 态
“x^−=Ax^k−1+Buk−1”，则此公式称为先验状态估计，即x^−是我们根据物理机理和上一时刻状态推测出来的，即先通过经验得出的估计值，是不够准确的。之后需要通过观测量来修正我们的估计值 “ x ^ − = A x ^ k − 1 + B u k − 1 ” ， 则 此 公 式 称 为 先 验 状 态 估 计 ， 即 x ^ − 是 我 们 根 据 物 理 机 理 和 上 一 时 刻 状 态 推 测 出 来 的 ， 即 先 通 过 经 验 得 出 的 估 计 值 ， 是 不 够 准 确 的 。 之 后 需 要 通 过 观 测 量 来 修 正 我 们 的 估 计 值
先验协方差估计公式：
在卡尔曼滤波器中，噪声引起了状态的不确定，不确定度是用协方差来表示的，卡尔曼滤波器假设噪声是高斯（正态）分布的（也称高斯白噪声）。
P−k:为先验协方差估计，即根据状态转移矩阵，和上一次的协方差估计出本次状态中的状态协方差 P k − : 为 先 验 协 方 差 估 计 ， 即 根 据 状 态 转 移 矩 阵 ， 和 上 一 次 的 协 方 差 估 计 出 本 次 状 态 中 的 状 态 协 方 差
Pk−1:为上一次的最优协方差估计值 P k − 1 : 为 上 一 次 的 最 优 协 方 差 估 计 值
Q:为模型噪声，即我们预测模型所包含的噪声 Q : 为 模 型 噪 声 ， 即 我 们 预 测 模 型 所 包 含 的 噪 声
“P−k=APk−1AT+Q”，则此公式称为先验协方差估计，是通过状态转移矩阵和预测模型噪声来预测我们的本次的协方差 “ P k − = A P k − 1 A T + Q ” ， 则 此 公 式 称 为 先 验 协 方 差 估 计 ， 是 通 过 状 态 转 移 矩 阵 和 预 测 模 型 噪 声 来 预 测 我 们 的 本 次 的 协 方 差
计算卡尔曼增益公式：
卡尔曼系数（卡尔曼增益）是联系预测模型和观察模型的重要接口，通过先验协方差和观察模型的噪声通过计算得出这样一个系数用于对预测模型的修正，我们上面已经知道了预测模型（先验估计）是不够准确的，是需要进行修正的，就是通过卡尔曼系数进行的。
zk：为观测值与预测值相对（即可视为传感器返回值） z k ： 为 观 测 值 与 预 测 值 相 对 （ 即 可 视 为 传 感 器 返 回 值 ）
H：为状态转移矩阵，即表示的是预测值与观测值之间的关系 H ： 为 状 态 转 移 矩 阵 ， 即 表 示 的 是 预 测 值 与 观 测 值 之 间 的 关 系
R：为观测噪声，可视为传感器的测量噪声 R ： 为 观 测 噪 声 ， 可 视 为 传 感 器 的 测 量 噪 声
“Kk=P−kHT(HPk−1HT+R)−1”此公式成为卡尔曼增益公式，推导过程较为复杂，定性分析为，通过先验协方差估计与观测噪声计算出修正预测值所需要的系数 “ K k = P k − H T ( H P k − 1 H T + R ) − 1 ” 此 公 式 成 为 卡 尔 曼 增 益 公 式 ， 推 导 过 程 较 为 复 杂 ， 定 性 分 析 为 ， 通 过 先 验 协 方 差 估 计 与 观 测 噪 声 计 算 出 修 正 预 测 值 所 需 要 的 系 数
因为我们一般无法准确获得模型噪声和观测噪声，因此，在给定Q与R的值时可以参考以下准侧——
模型误差与测量误差的大小，可以看做模型预测值与测量值的加权。R固定，Q越小代表越信任模型预测值；Q固定，R越小代表越信任测量值。
后验状态估计
后验状态是我们根据先验通过观测值修正之后的最优估计值，也是我们可以利用和用于下一次迭代的值。
对于公式：“x^k=x^−k+Kk(zk−Hx^−k)”即通过卡尔曼系数将观测值与预测值想联系，估计出一个最优状态，其中(zk−Hx^−k)表示的是观测值与估计值的残差 对 于 公 式 ： “ x ^ k = x ^ k − + K k ( z k − H x ^ k − ) ” 即 通 过 卡 尔 曼 系 数 将 观 测 值 与 预 测 值 想 联 系 ， 估 计 出 一 个 最 优 状 态 ， 其 中 ( z k − H x ^ k − ) 表 示 的 是 观 测 值 与 估 计 值 的 残 差
如此说来，卡尔曼滤波可近似看做为观测量与预测量根据某种依据来加权计算。
后验协方差估计公式：
后验协方差估计是根据卡尔曼系数和先验协方差估计来更新协方差（不确定度）
I：为单位阵 I ： 为 单 位 阵
2.2、从卡尔曼滤波器的两个计算回路来认识卡尔曼滤波器
可以明显看出，增益计算回路是一个独立的计算回路，其值的迭代与计算更新完全得益于模型噪声 Q Q 与观测噪声 R R ，因此这两个参数的选取和精确测量将直接影响到卡尔曼滤波器的效果。
那么，可以看出，滤波计算回路和增益计算回路唯一的联系就是卡尔曼增益 Kk K k ，我们可以从这一个图来观察卡尔曼滤波器的结构与各个公式的关系。
待更
