🪴 l4rk's Quartz

卡尔曼滤波与拓展卡尔曼滤波

12分钟阅读

卡尔曼滤波


其中， 为状态 (转移) 矩阵（Transition Matrix）， 记为 时刻的状态向量。
为 时刻输入, 称为控制矩阵（Control Matrix），反映了系统输入到系统状态的映射关系。
是过程噪声，我们假定其符合均值为 0，协方差矩阵为 的高斯噪声。
为测量矩阵（Measurement Matrix），描述了从系统状态到测量值的转换关系
是测量噪声，我们假定其符合均值为 0，协方差为 的高斯噪声。

状态预测方程是取得是每一项的概率最大值处对应的自变量的值，噪声 是均值为 0 的高斯噪声，因此最大概率对应的值为 0。

对系统状态向量上面加上一个帽子符号表示这是一个估计量，也就是我们实际输出的量。

表示 时刻时的卡尔曼滤波后的状态向量，也称为状态向量的后验估计。

表示当前根据系统状态方程计算预测得到的状态向量，也称为状态向量的先验估计。

测量估计方程同样忽略噪声项，变成对此时测量量的估计

递归卡尔曼滤波器所有公式（统一前面卡尔曼滤波的介绍和推导一二中的符号）有

  1. 状态预测方程
  2. 先验估计误差协方差更新公式
  3. 卡尔曼增益计算公式
  4. 最优状态估计迭代更新公式
  5. 最优 (后验) 估计误差协方差更新公式

第一行公式没有代入卡尔曼增益的 计算公式，是最优估计误差协方差的一般形式，不要求 必须为卡尔曼增益。
假设 为真实值，那么卡尔曼滤波就是找到一个合适的 使得下面式子中的 最小。
我们根据测量方程可以反推出状态向量的测量反推值


上述式子中，为了展示 的融合过程，将 定义为 。
卡尔曼滤波的目标函数为

这里，我们尝试简化一下

这里 为真实值，表示一个常数，我们假设在初始状态下（即 k=0）时， 可以准确的描述 ，即 。
这里我们尝试化简一下



在上述公式中， 和 需要理解为一个常量，而不是服从某一个分布的随机变量。

有上面的公式可以看出，对于上述线性系统的建模后， 取任意值，都可以保证无偏性的传递。
同时也可以看出，若后验估计保持了无偏性的传递性，先验估计 也是无偏的。

一般的材料中，常说卡尔曼滤波的估计值是无偏估计，实际上这是由条件的，其中要求系统噪声和量测噪声为不相关、零期望的白噪声，且是线性系统，初始时刻的状态估计是无偏的。当这些条件不能满足时，Kalman滤波的估计结果是有偏的。 我们这里是先给出了卡尔曼滤波的一般公式，然后再证明其无偏性。 实际上也可以要求其无偏，设 $x^+_k = K_1 \hat x^-_{k} + K_2 x^{measure}_k$ ，从无偏性中推出 $K_1$ 与 $K_2$ 的关系。

再回到原来的公式

由此可以看出，估计值与真实值之间的均方误差就是估计误差的方差。

我们继续化简

这里用用上了无偏估计的条件， ，目标函数可以化简为

这里又得出另外一个重要结论，在卡尔曼滤波中，最优估计值是无偏估计的条件下，最小化估计值与真实值之间的均方误差等价于最小化估计值的方差。

因此，卡尔曼滤波的思想即线性融合所有数据来源的分布，求各个分布的系数 K 使融合后的分布方差最小。

拓展卡尔曼滤波与卡尔曼滤波

普通的卡尔曼滤波仅仅适用于线性系统
而拓展卡尔曼滤波不仅可以用于线性系统, 也可以用于非线性系统

Q: 为什么拓展卡尔曼滤波可以使用在非线性系统中?
A: 与求导的思想类似
如果你放大到非线性曲线上的任意一点,你会发现它看起来非常像一条直线.使用扩展卡尔曼滤波器 (Extended Kalman Filter), 我们通过一个称为雅可比矩阵 (Jacobian) 的特殊矩阵将非线性方程转换为线性化形式 (State Space Model i.e. Transition Function). 然后, 我们使用这个线性化形式的方程来完成卡尔曼滤波过程.

State Space Model i.e. Transition Function

reference
三维俯瞰

为线性速度, 为角速度, 为 yaw angle 偏航角
将一个非线性的变换转换为两个线性变换的和

均通过雅克比行矩阵得到, 将非线性方程转换为线性化形式

Observation Model

reference

为传感器数值向量, 为状态向量, 为噪声向量, 为观测矩阵
估计传感器向量=观测矩阵*状态向量+噪声向量
同样也是由雅克比行列式推断得到

Q: 传感器数值矩阵为 3*1, 状态矩阵为 3*1
什么时候观测矩阵为单位矩阵?
A: 观测矩阵形状为 3*3
当传感器矩阵与状态矩阵一一对应的时候, 例如传感器 1 对应状态 1

State Space Model 与 Observation Model 的联系

State Space Model 使用 时刻的传感器数据, 来预测 时刻的状态
Observation Model 使用 时刻的状态, 来预测 时刻的状态, 再利用预测状态推断 时刻的传感器数据

Kalman 滤波

Kalman 滤波做的工作是将 observation model 得到的传感器数值与实际上的传感器数值联系起来, 得到一个最优解

Q: 为什么不能够信任实际上的传感器数值?
A: 因为传感器并非 100%完美, 存在或大或小的噪声
为了减小噪声, 使得数据尽量贴近事实, 选择使用 kalman 滤波

关系图谱