无人驾驶感知篇之融合（六）

         今天上海新增二万左右，感觉解封的日期遥遥无期。最近家里快没粮了，希望疫情早点结束吧！祈祷，祈祷！今天咱们主要讲讲融合算法中的卡尔曼滤波法。卡尔曼滤波法的主要思想是主要用于融合低层次实时动态多传感器冗余数据。该方法用测量模型的统计特性递推，决定统计意义下的最优融合和数据估计。

1.卡尔曼滤波的由来

        卡尔曼滤波器最早要追寻到1960年卡尔曼先 生发表的关于利用递归算法来求解离散线性滤波器 问题的学术论文——《A New Approach to Linear Filtering and Prediction Problems》(线性滤波与预 测问题的新方法)，在卡尔曼先生的这篇学术论文中 首次提出了针对维纳滤波器缺点的全新解决方案， 这种方案就是时至今日被人们所熟知的卡尔曼滤波 方法。卡尔曼滤波方法功能非常强大，应用也十分 广泛，利用卡尔曼滤波方法不仅可以预测信号当前、 过去的状态，还可以预测信号的下一步即将来的状态，这种预测和估计可以在不知道系统确切模型的条件下完成。

2.卡尔曼滤波算法原理

       直奔主题吧，咱们可以有以下假设：（1）假设物理系统的状态更新过程为一个离散时间的随机过程；（2）假设被控制对象的输入对物理系统的状态会产生影响；（3）假设噪声对物理系统的观测过程产生影响；（4）假设物理系统的状态是非直接可观测的。

在以上假设前提 下，得到系统的状体方程和观测方程：

 式中：Xk为状态向量；Lk为观测向量；为状态 转移矩阵；Uk-1为控制向量，一般不考虑；为系数矩阵；为系统动态噪声向量；为观测 噪声向量，其随机模型为：

 

 卡尔曼滤波递推公式为：