type
status
date
slug
summary
tags
category
icon
password
1. 状态转移矩阵
从当前状态到下一个状态的转换.
例子: 假设跟踪车辆形式
- 车辆的状态矩阵, 位置变量 ; 速度变量 .
- 假设车辆有加速度变量 ; 这里 是单位时间; 这里就可以从上一个单位时间的 推导出当前的 .
- 这里可以发现输出变量是输入变量的线性组合, 所以卡尔曼滤波器是线性滤波器, 只能描述状态与状态之间的线性关系. 由于是线性关系, 所以可以将公式写成矩阵的形式.
- 提取公式中带时间参数矩阵之后, 就可以简化公式获得卡尔曼滤波器中的状态预测公式.
- 这里的 表示, 这个值有 hat 符号是预测值, 并不是真实值; 右上角减号表示这个值是从 t-1 时刻的参数估计而来, 还需要 t 时刻的观测值进行修正.
2. 协方差矩阵
协方差描述的是变量之间的相关程度.
- 首先, 可以使用中心值核方差来描述一个变量的分布情况.
- 而对于存在两个变量的分布情况, 需要用到二维坐标系. 可以在两个轴分别描述两个变量的中心值和方差. 如果两个变量没有明显线性关系时, 分布情况图为左一; 两变量分布正相关时, 图为中间; 分布为负相关时, 图为右一. 可以发现这三种情况用中间值和方差是无法体现的.
- 为了描述两变量之间的相关程度, 使用了协方差. 协方差矩阵的公式如下
- 其中, 表示的就是两变量的协方差.
- 还是拿车辆速度举例, 由 来表示不确定性在各个时间段内的传递关系, 是当前时刻先验估计协方差.
- 表示的是状态转换矩阵里带时间变量的 矩阵. 之所以两边都乘, 是因为协方差矩阵的性质. 协方差的更新方法, 如果要将每一个点都乘以矩阵 , 公式如下:
- 是状态转移协方差, 用来描述状态转换矩阵和实际过程之间的误差. 这个值是可以设置的参数, 值越大, 表示我们对预测值的信任值越高. 因为这样会增大 .
3. 观测矩阵
描述系统实际状态, 与观测到的数值之间的关系.
- 例1, 地球上的汽车位置为 [经度, 纬度, 海拔], 但是地图导航软件常常默认海拔为 0, 也就是不需要观测数据中的海拔信息.
那么地图需要的观测数据 如下:
- 例2, 还是车辆速度问题, 如果有个测距仪在每个时间段进行测距, 获得标量 .
- 描述观测值和预测值的残差, 因为观测值也不一定准确. 这时候, (观测值)和 (预测值) 都是已知的, 所以可以算出:
- 测量噪声的协方差为 , 可以手动设定的参数.
这时候, 观测矩阵为 , 因为车辆的状态矩阵中有用的只有位置变量 .
获得 公式如下:
4. 状态更新
- 计算出卡尔曼系数 , 也叫滤波增益系数, 它的作用有两个
- 权衡预测状态协方差 和 观察值的残差协方差 的大小, 来决定观测值的权重. (相信预测模型还是观测值, 如果相信预测模型多一点, 那么 就会小一点; 反之则 就会大一点)
- 将残差的表现形式从观测域转换为状态域. 因为, 观察值和状态值的维度可能就是不同的, 所以需要转换. 举车辆速度的例子来说, 观测值只有车辆的位置没有速度, 但是车辆状态有速度和位置. 这时候, 中包含了协方差矩阵 的信息, 所以可以利用位置和速度的相关性, 从位置的残差 推算出速度的残差.
- 计算当前状态, 由 状态预测公式 + 卡尔曼系数 * 观测值和预测值的残差 获得.
- 更新最佳估计值的噪声协方差矩阵 , 在下一时刻作为 用于 的计算.
5. 实际使用
- 需要调试的参数
- : 其实修改的是转态转移矩阵, 不同的运动状态可以使用不同的矩阵
- : 测量噪声的协方差, 取值越小收敛越快, 取值越大收敛越慢;
- : 状态转移协方差矩阵, 值越大, 表示我们对预测值的信任值越高; 当状态转换过程为已确定时, 的取值越小越好; 转换状态不确定的时候, 可以去随机值, 随时间变化, 这时滤波器变为自适应卡尔曼滤波器.
使用测距仪获得单位时间车辆距离测距仪的距离, 预测车辆速度.
对于有车有固定加速度的情况
如果车的加速度是非线性变化 (前50s, a=1; 后50s, a=3) 的情况, 无法准确预测
- 更复杂的应用可以看这里
6. 总结
优点：
- 卡尔曼滤波器是一种最优的滤波器，具有最小均方误差，能够有效地消除噪声和抖动。
- 卡尔曼滤波器不仅可以估计状态，还可以估计状态的协方差，对于系统的不确定性有一定的鲁棒性。
- 卡尔曼滤波器可以实时地进行状态估计，响应速度较快。
缺点：
- 卡尔曼滤波器的理论基础是线性高斯系统，对于非线性、非高斯的系统效果可能不佳。
- 卡尔曼滤波器需要准确的先验信息，如果先验信息不准确，则滤波结果也会不准确。
- 卡尔曼滤波器计算量较大，对计算能力有一定要求。
参考资料
- 调参
