![74faa239258bde229858b3481b386b54.png](https://i-blog.csdnimg.cn/blog_migrate/498a1ee13dc9e46390e8c51dab96e641.jpeg)
本文使用 Zhihu On VSCode 创作并发布
0 前言
卡尔曼滤波(Kalman Filter,KF)以贝叶斯滤波为理论基础,并通过假设状态量随机变量(以下简称状态量)、观测量均服从正态分布,假设过程噪声、观测噪声均服从均值为 0 的正态分布,以及假设状态转移函数和观测函数均为线性函数,实现对连续型随机过程的递推状态估计。简言之,卡尔曼滤波是在贝叶斯滤波框架下求解线性高斯问题。
1 贝叶斯滤波的三大概率密度函数
在此前的文章
石鹏:从概率到贝叶斯滤波zhuanlan.zhihu.com![1cb6862405ed82c28298699b97d72aae.png](https://i-blog.csdnimg.cn/blog_migrate/c9baf5faf9631dceee3be8641bf4d847.jpeg)
中,已经讲到贝叶斯滤波的先验概率密度函数、似然概率密度函数和后验概率密度函数:
(1) 先验概率密度函数
(2) 似然概率密度函数
(3) 后验概率密度函数
其中,后验概率密度函数中的归一化常数
2 卡尔曼滤波的假设
卡尔曼滤波以贝叶斯滤波为理论基础,并作了六个前提假设:
(1) 假设一:状态量服从正态分布
(2) 假设二:观测量服从正态分布
(3) 假设三:过程噪声服从均值为 0 的正态分布
(4) 假设四:观测噪声服从均值为 0 的正态分布
(5) 假设五:状态转移函数为线性函数
其中,
(6) 假设六:观测函数为线性函数
3 卡尔曼滤波的公式推导
3.1 预测步的两个公式
根据假设一,
结合假设三与假设五,k 时刻状态量
对于
- Mathematica 软件做符号推导
- 复变函数留数定理
- 傅里叶变换 + 卷积
这里使用 Mathematica 软件做符号推导,创建 WolframScript(.wls)脚本文件,创建
![34d24e7c2d7876321506fa1f1fd9ac13.png](https://i-blog.csdnimg.cn/blog_migrate/cb66a67348f012fe42fa49b048c4512e.png)
其中,
运行代码进行化简,得到如下条件表达式结果:
![de6fe0e608decce4e1747b4e1257b845.png](https://i-blog.csdnimg.cn/blog_migrate/f66c0a7cb79e40ad7b1120fb22f6bec1.png)
条件表达式中的条件显然成立,整理表达式形式可知,先验概率密度函数
3.2 更新步的三个公式
结合假设四、假设六、公式 (3.1)、公式 (3.2),可知,k 时刻状态量
其中,归一化常数
创建 WolframScript(.wls)脚本文件,创建
![cfb403bd3816ffba0891440c87bc8492.png](https://i-blog.csdnimg.cn/blog_migrate/38abe3deaead03a0d6bc9e7cef484b0f.png)
其中,
Simplify[]
函数的使用是为了简化结果。
运行代码进行化简,得到如下条件表达式结果:
![44716a5dc714822d5eea2f6bf86dd89f.png](https://i-blog.csdnimg.cn/blog_migrate/b07759eb703b8ae49c034b98c74b9abc.png)
条件表达式中的条件显然成立,整理表达式形式可知后验概率密度函数
卡尔曼增益系数
当
此时,后验估计结果倾向于预测;
当
点击这里下载我的 WolframScript(.wls)脚本文件。
4 矩阵形式的卡尔曼滤波
上文内容所描述的是一维的卡尔曼滤波,当状态量和观测量不再是单一的随机变量而是由多个随机变量组成的序列时,卡尔曼滤波中各个量的维数也将随之改变:
- 状态量
由随机变量演变为随机向量,随机向量中的每一个分量为一个状态量随机变量。维数为
- 状态转移比例项
演变为矩阵,维数为
- 控制量
演变为矩阵,维数为
- 控制比例项
演变为矩阵,维数为
- 状态量概率密度函数均值
演变为矩阵,维数为
- 状态量概率密度函数方差
演变为协方差矩阵,用
表示,维数为
- 过程噪声方差
演变为协方差矩阵,用
表示,维数为
- 观测量
由随机变量演变为随机向量,随机向量中的每一个分量为一个观测量随机变量。维数为
- 观测值
由单一值演变为由单一值组成的值矩阵,维数为
- 观测比例项
演变为矩阵,维数为
- 观测噪声方差
演变为协方差矩阵,用
表示,维数为
- 卡尔曼增益系数
演变为矩阵,维数为
对应的五个公式演变为:
公式 (4.3) 中
从结果中还可以发现,外部控制项
5 应用实例
卡尔曼滤波的应用实例可参考此前的学习文章
(十三)手把手教你写卡尔曼滤波器blog.shipengx.com![bc22d8b9dfa295cab2e58f2a6eb73d09.png](https://i-blog.csdnimg.cn/blog_migrate/99ad35bfc655eeb02642b5b241023eee.jpeg)
文章中的应用场景是使用激光雷达(Lidar)对匀速直线运动(Constant Velocity,CV)目标的测距过程。
博主工作中使用卡尔曼滤波对路口前的停止线(来自单目前向视觉)纵向距离进行了估计,系统模型抽象为匀加速直线运动(Constant Acceleration,CA),此时的状态转移函数
6 参考
- b站忠实的王大头《贝叶斯滤波与卡尔曼滤波》第七讲:卡尔曼滤波
- 无人驾驶技术入门(十三)| 手把手教你写卡尔曼滤波器
- How a Kalman filter works, in pictures
- 详解卡尔曼滤波原理
- Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation
如果觉得文章对你有所帮助的话,欢迎点赞/评论/收藏/关注,相互交流,共同进步~