本期概述
卡尔曼滤波器的含金量还在不断上升！
状态预测公式
我们假设在道路上有一辆小汽车在行驶，他现在时刻的状态矩阵简单由位置和速度来表示：
欢迎加入自动驾驶实战群
那么位置以及速度就不难推导：
其中，是当前车辆的加速度（理解为给的油门，表示线性系统的控制量）。
由于卡尔曼滤波器是一个线性滤波器，所以我们可以用矩阵来表示上面这个方程组：
一切都很简单对嘛！现在再用一个简单的字母去代替矩阵，替换公式为：
最后代入，恭喜~我们得到了卡尔曼滤波器的第一个公式，状态预测公式：
是状态转移矩阵，表示从上一时刻的状态推导至当前时刻状态。
是控制矩阵，表示当前的控制量如何作用于当前时刻状态
那我问你，为啥的头顶为啥尖尖的呢？
这个表示状态是预测得到的，并非实打实的真实值！同时，还有一个?上标，这表示这个值不能直接使用，下面我们会要对他增加一些修正。
协方差矩阵
真实的世界并非是0和1的，一定会有0-1的叠加态，即”不确定”的情况。我们的观测很有可能带有噪声，噪声越大，这种”不确定”就越强。
假设蓝色是采样点，噪声是x,y两个方向上提供的，再加上两个噪声有可能耦合，这几个参数共同组成了噪声协方差矩阵
同时，不同的噪声很有可能互相耦合，为了表达不同噪声之间的相关性，就有了协方差矩阵：
矩阵的对角线元素表示状态变量 估计值的不确定性或误差方差。方差越大，表示对该状态的估计越不确定。
矩阵的非对角线元素表示状态变量之间的相关性。如果两个变量的误差高度相关，这个协方差值就会很大。
协方差矩阵的传递
根据协方差矩阵的性质（这里不再证明）：
再把上面计算得到的状态预测公式带入，就有协方差矩阵传递公式的简单形式：
但是由于我们的预测模型仍然有不确定性，所以加一个尾巴加以修正：
我们得到了第二个公式。
观测矩阵
观测矩阵把系统的内部状态（通常是难以直接测量的变量）转换成能够被传感器测量的形式，将系统的状态向量 映射到观测空间的矩阵。
卡尔曼滤波器的数据融合功能就是从这里实现的。
简单来说，观测矩阵 用于从状态向量中提取出我们能直接测量的部分。
例如，小汽车的位置以及速度都是状态量，但是我们的传感器（假设为RTK）只能给出位置的测量值，那么观测矩阵就会将状态矩阵中的干掉：
最后，由于观测也并非完全可靠，所以加上一个补偿：
状态更新
还记得嘛，我们说还有一个?上标，需要加上补偿量才可以使用！
啥是补偿量呢？
就是评估预测值与实际观测值之间的偏差！考虑上实际观测后，状态矩阵可以去掉?上标，使其更加接近实际观测值：
这里又引入了一个卡尔曼系数，推导它是一个复杂的事情，我们先记住他的表达：
卡尔曼系数的作用相当于一个权衡值，如果模型更相信预测值，则会比较小；如果模型更相信测量值，则会比较大。
NOTE：卡尔曼系数还会把测量的残差从观察域转换回状态域，和观测矩阵的作用相反。
噪声协方差矩阵的更新
这个是留给下一轮迭代时使用的。
表示了如何将卡尔曼增益应用到预测误差协方差上。
同样，在较大时，系统对观测数据更为信任，更新的协方差矩阵会变得较小，表示系统对状态的估计更精确；反之，则会变得较大，表示不确定性增加。
总结
我们得到了所有的五个公式
预测公式：
更新公式：
最后总结一下整体流程：
1.初始化：
给定初始状态估计 和初始估计误差协方差矩阵 。
2.预测阶段:
预测当前时刻的状态 。
预测当前时刻的误差协方差矩阵 ，其中 是过程噪声协方差矩阵。
3.更新阶段：
计算卡尔曼增益 ，其中 是观测噪声协方差矩阵。
更新状态估计。
更新估计误差协方差矩阵。
实验
这个图片是李小毛从网络上扒下来的！
数据来源： https://www.bilibili.com/video/BV1vs411z7PX这个实验中，车辆从1运动到100，速度为1m/s，每1s采样一次，观测噪声为1的高斯噪声。
X：系统的状态向量，包含位置和速度。初始化时，假设物体的初始位置和速度都是 0。
P：状态协方差矩阵，表示对状态估计的不确定性。在初始时，位置和速度的估计不确定性是 10。
F：状态转移矩阵。在这个例子中，假设位置随时间增加（假设速度是恒定的），所以位置和速度的状态转移矩阵是这样设定的。
Q：过程噪声协方差矩阵，表示系统动态过程中的噪声影响。在这个程序中，假设过程噪声是很小的，因此设置一个非常小的值。
H：观测矩阵，它将系统的状态（位置和速度）映射到观测空间。由于程序中的观测值 Z 只包含位置，因此观测矩阵只有对位置的映射。
R：观测噪声协方差矩阵，描述观测数据中噪声的大小。在这里，设定为 1，表示观测值的噪声是单位方差的高斯噪声。
实验作图
最后别忘了，帮忙点“在看”。
您的点赞，在看，是我创作的动力。
AiFighing是全网第一且唯一以代码、项目的形式讲解自动驾驶感知方向的关键技术。
长按扫描下面二维码，加入知识星球。
