卡尔曼滤波简明教程

卡尔曼滤波

前置知识

首先搞清楚两个过程：状态获取 和 状态转移

状态获取就是通过 传感器 或者 测量工具 来得到系统当前的状态的过程

在这个过程中，存在一个噪声 叫做 测量噪声

状态转移就是 根据运动定律或者约束方程 计算得到的系统下一时刻的状态 这个过程

在这个过程中，存在一个噪声 叫做 过程噪声

从测量 更新 预测的角度看，也叫做预测噪声

那么卡尔曼滤波是用来干什么的，下面这句话解释的很好

由于测量噪声和过程噪声的存在，直接根据运动方程估计目标位置可能会与真实位置 大相径庭 ，所以 为了对抗测量和模型的不确定性， 提出卡尔曼滤波算法

进一步思考，为什么说 KF 可以对抗测量和模型的不确定性

详细参考 建模和公式推导 部分。简单来说就是把状态估计量看作一个随机变量, 其存在一个方差p，通过取一个合适的卡尔曼增益，使得状态估计量的方差最小，也就是最优估计

建模和公式推导

变量定义

• r 测量噪声方差
• q 过程噪声方差
• p 状态估计方差（就是估计值和真实值残差值的方差，KF的目的就是让p最小）
• x 状态值，z测量值
• 下标 n,n-1 表示根据上一时刻预测的当前时刻值，下标 n,n 表示当前时刻估计值（也就是更新过程的输出），下标 n+1,n 表示当前时刻预测下一时刻值（也就是预测过程的输出）

直接给出公式（著名的五个方程）

理一下公式的逻辑和流程（语言描述下伪代码）

1.首先输入最初时间n的状态值和状态估计方差p（也就是初始化参数）

2.预测下一时间n+1的状态值和状态估计方差p（预测阶段）（两个公式）

3.获取n+1测量值和测量噪声方差r（测量阶段）

4.根据n+1的预测p和n+1的实际r 计算卡尔曼增益K**（一个公式）**

5.根据卡尔曼增益K，n+1测量和n+1状态预测更新当前状态估计值，根据卡尔曼增益和n+1p的预测值计算n+1时间的p（更新阶段）（两个公式）

公式解释

公式4，5是状态转移方程，取决于系统，无需解释

首先明确公式2是卡尔曼滤波的思想（预测值和测量值的加权），是事先规定好的，无需解释

关于公式1也就是卡尔曼增益的计算

比较简单，把p都代入公式2，为了让p取最小值，让p对K求导，使导数为0即可

关于公式3也就是协方差更新

将公式1代入公式2整理，消消消，最终得出了这么一个比较简约的公式

整体系统框架

PS

在迭代过程中所计算出的卡尔曼增益大小反映了测量过程的准确性

高卡尔曼增益表示测量系统比较准确

参考内容：卡尔曼滤波教程 (kalmanfilter.net)