摄像头与激光雷达微波雷达的融合算法之四----卡尔曼滤波

1、卡尔曼滤波的五大公式

系统的状态方程为：

这个状态方程是根据上一时刻的状态和控制变量来推测此刻的状态，wk-1是服从高斯分布的噪声，是预测过程的噪声，它对应了 xk 中每个分量的噪声，是期望为 0，协方差为 Q 的高斯白噪声wk-1~N(0,Q),Q即下文的过程激励噪声Q.

观测方程为：

vk是观测的噪声，服从高斯分布，vk~N(0,R),R即下文的测量噪声R。

卡尔曼滤波算法有两个基本假设: ( 1) 信息过程的足够精确的模型，是由白噪声所激发的线性( 也可以是时变的) 动态系统; ( 2) 每次的测量信号都包含着附加的白噪声分量 。当满足以上假设时，可以应用卡尔曼滤波算法。

卡尔曼滤波算法分为两步:预测和更新

预测：根据上一时刻