卡尔曼滤波原理及公式推导

本文参考了b站视频教程：
https://www.bilibili.com/video/av75068018?from=search&seid=8021704495196825336
一、卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。
二、算法的核心思想是：
根据当前的仪器"测量值" 和上一刻的 “预测量” 和 “误差”,计算得到当前的最优量，再预测下一刻的量。
三、举个例子并顺便推导卡尔曼滤波方程

首先，有一辆小车，理想状态下他不受任何外力作用并做匀速直线运动。
它的状态是符合正态分布的：
用上一时刻的状态来表现这一时刻的状态（运动学表达）：

方程1：

但事实上小车并不总是按照匀速直线运动，假设它有一个加速度为a。

用运动学公式表达这一状态（得到的位置和速度的值可能不准确）：

方程2：
引入矩阵描述：
把方程1和2改写为矩阵形式：

方程3：

方程4：

引入一个表示噪声的方程得到具有噪声的预测方程：

方程5：


给小车加上传感器来测量位置和速度的测量值（这个值可能也不是准确的）：

由于运动学公式的计算和测量值并都不能确定是准确的，卡尔曼滤波出场了：
黑色圈代表预测状态，红色圈代表真实状态，蓝色圈则为卡尔曼滤波所得到的最优估计。
推广为一般形式，并将Xpredicted和Ymeasurement代入：

得到如下方程：
将方程进行简化：

至此我们已经得到了卡尔曼滤波的方程：

将它推广为一般形式：