卡尔曼滤波器是一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法。而且由于观测包含系统的噪声和干扰的影响,所以最优估计也可看做是滤波过程。
卡尔曼滤波器的核心内容就是5条公式,计算简单快速,适合用于少量数据的预测和估计。
下面我们用一个例子来说明一下卡尔曼算法的应用。
假设我们想在有一辆小车,在 t 时刻其速度为 Vt ,位置坐标为 Pt,ut 表示 t 时刻的加速度,那么我们可以用Xt表示 t 时刻的状态,如下:
则我们可以得到,由t-1 时刻到 t 时刻,位置以及速度的转换如下:
用向量表示上述转换过程,如下:
如下图:
那么我们可以得到如下的状态转移公式:
(1)
其中矩阵 F 为状态转移矩阵,表示如何从上一状态来推测当前时刻的状态,B 为控制矩阵,表示控制量u如何作用于当前矩阵,上面的公式 x 有顶帽子,表示只是估计值,并不是最优的。
有了状态转移公式就可以用来推测当前的状态,但是所有的推测都是包含噪声的,噪声越大,不确定越大,协方差矩阵用来表示这次推测带来的不确定性
协方差矩阵
假设我们有一个一维的数据,这个数据每次测量都不同,我们假设服从高斯分布,那么我们可以用均值和方差来表示该数据集,我们将该一维数据集投影到坐标轴上,如下图:
可以看到,服从高斯分布的一维数据大部分分布在均值附近。
现在我们来看看服从高斯分布的二维数据投影到坐标轴的情况,如下图:
二维数据比一维数据稍微复杂一点,投影后有3种情况,分别是:
左图:两个维的数据互不相关;
中图:两个维的数据正相关,也就是 y 随着 x 的增大而增大(假设两个维分别为 x 和 y)
右图:两个维的数据负相关,也就是 y 随着 x 的增大而减小。
那怎么来表示两个维的数据的相关性呢?答案就是协方差矩阵。
状态协方差矩阵传递
在公式(1)之中,我们已经得到了状态的转移公式,但是由上面可知,二