卡尔曼滤波学习心得

卡尔曼滤波学习心得

卡尔曼原理简介

​ 卡尔曼滤波法是一种时域方法，对于具有高斯分布的噪声的线性系统，应用该方法可以得到系统状态的递推最小均方差估计，该方法用状态方程（表示一个状态的转化）描述动力学系统的动态模型（即状态空间转移模型），用观测方程描述动力学的系统观测模型，可处理动态时变系统、非平稳信号和多为信号。

​ （以上名词多为现代控制理论中的专业名词）

卡尔曼的推导

预备知识

1. 协方差定义

X、Y 是两个随机变量，X、Y 的协方差 cov (X, Y) 定义为：

$$\mathrm{cov}(X,Y)=\frac{{\sum }_{i=1}^n\left(X_i-\bar{X}\right)\left(Y_i-\bar{Y}\right)}{n-1}$$

2. 协方差矩阵定义

协方差矩阵是由数据集中两两变量的协方差组成。矩阵的第*(i,j)(i,j)*个元素是数据集中第 ii 和第 jj 个元素的协方差。例如，三维数据的协方差矩阵如下所示：
$$C=\left[\begin{array}{lll}\mathrm{cov}\left(x_1,x_1\right)& \mathrm{cov}\left(x_1,x_2\right)& \mathrm{cov}\left(x_1,x_3\right)\\ \mathrm{cov}\left(x_2,x_1\right)& \mathrm{cov}\left(x_2,x_2\right)& \mathrm{cov}\left(x_2,x_3\right)\\ \mathrm{cov}\left(x_3,x_1\right)& \mathrm{cov}\left(x_3,x_2\right)& \mathrm{cov}\left(x_3,x_3\right)\end{array}\right]$$
​ （x₁ ,x₂ ，…通常是一个数组）

系统模型与变量说明

1、系统离散型状态方程如下

由k-1时刻到k时刻，系统状态预测方程：

​ X_k = AX_k-1 + Bu_k + w_k

​ 系统状态观测方程

Z_k = HX_k + v_k

2、变量说明如下

​ A ：状态转移矩阵

u_k ：系统输入向量

​ B ：输入增益矩阵

w_k ：均值为０，协方差矩阵为Q ,且服从正态分布的过程噪声

H ：测量矩阵

v_k ：均值为０，协方差矩阵为R ,且服从正态分布的测量噪声

卡尔曼的实例说明

以一个普通的二维平面运动的机器人为例，它的状态有：

$$\vec{x}=\left[\begin{array}{l}p\\ v\end{array}\right]$$

预测方程

我们基于高斯分布来建立状态变量，所以在时刻 k 需要两个信息：最佳估计 [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-F4yHkB6f-1570248068212)(http://latex.codecogs.com/png.latex?%5Cwidehat%7Bx_%7Bk%7D%7D)]（即均值，其它地方常用 μ 表示），以及协方差矩阵 [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-jMj4pO1z-1570248068213)(http://latex.codecogs.com/png.latex?P_%7Bk%7D)] 。
$$\widehat{x_k}=\left[\begin{array}{l}\text{position}\\ ve\mathrm{locity}\end{array}\right],P_k=\left[\begin{array}{ll}{\Sigma }_{pp}& {\Sigma }_{pv}\\ {\Sigma }_{vp}& {\Sigma }_{vv}\end{array}\right]$$
​ 通常系统中是有外部控制量以及外部干扰的，将对系统自身状态没有相关性的改变。所以此时可得到预测的完整表达式：
$$\begin{array}{rl}{\hat{\mathbf{x}}}_k& ={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{B}}_k{\overrightarrow{\mathbf{u}}}_k\\ {\mathbf{P}}_k& ={\mathbf{F}}_{\mathbf{k}}{\mathbf{P}}_{k-1}{\mathbf{F}}_k^T+{\mathbf{Q}}_k\end{array}$$
　　由上式可知，新的最优估计是根据上一最优估计预测得到的，并加上已知外部控制量的修正。

而新的不确定性由上一不确定性预测得到，并加上外部环境的干扰。

(我们是将这些干扰当做协方差为Q_k 的噪声(这里以及之后我们都假设这些噪声是服从正态分布的)来处理，虽然模型的方程中最后是加上了噪声，但是在实际处理时是通过协方差来解决预测方程中的噪声，毕竟噪声就是对该预测方程可信度的挑战，所以这一部分可以通过协方差体现)。

测量方程

我们可能会有多个传感器来测量系统当前的状态，哪个传感器具体测量的是哪个状态变量并不重要，也许一个是测量位置，一个是测量速度，每个传感器间接地告诉了我们一些状态信息。但传感器测得的数据或多或少也会不可靠的，所以也要加入测量噪声。

测量的作用主要是与预测值融合，使得到一个最优估计： H 为测量矩阵，R 为协方差矩阵。

状态更新

卡尔曼最重要的部分也是这一部分了。更新方程为：
$$\begin{array}{rl}{\hat{\mathbf{x}}}_k^{\prime }& ={\hat{\mathbf{x}}}_k+{\mathbf{K}}^{\prime }\left({\overrightarrow{\mathrm{z}}}_k-{\mathbf{H}}_k{\hat{\mathbf{x}}}_k\right)\\ {\mathbf{P}}_k^{\prime }& ={\mathbf{P}}_k-{\mathbf{K}}^{\prime }{\mathbf{H}}_k{\mathbf{P}}_k\\ {\mathbf{K}}^{\prime }=& {\mathbf{P}}_k{\mathbf{H}}_k^T{\left({\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T+{\mathbf{R}}_k\right)}^{-1}\end{array}$$
　　（具体卡尔曼增益${\mathbf{K}}^{\prime }$是怎么算的，这个这里就不推了，建议看这篇link）

${\hat{\mathbf{x}}}_k^{\prime }$是新的最优估计，也是和${\mathbf{P}}_k^{\prime }$参与下一次的预测与更新的迭代的。（${\hat{\mathbf{x}}}_k$是根据上一时刻的信息所获得先验估计）

我们由第一个方程可以看出，k 时刻系统的最优估计值等于 k-1 时刻状态估计值（由上一状态的最优估计值加上过程误差）+带卡尔曼增益权值项的偏差。如果观测误差远远大于估计误差，那么K就很小，k时刻的预测值约等于k时刻的状态估计值，如果对i时刻的状态估计值误差远远大于观测误差，此时相应的q较大，K较大，i时刻的状态估计值更倾向于观察的数据。

总结

卡尔曼滤波也就是最后这五个公式，只要理解这几个公式，代码实现过程中，其实看起来并不是那么麻烦。而且可以看出，因为每一个时刻的测量值都通过乘积被记录在下一时刻的后验估计值之中，也就是说，虽然某一时刻的后验估计看起来只是由前一时刻的后验估计和当前的测试值乘积得到的概率最大值，但其实通过不停地乘积和迭代这当前时刻的后验估计值已经包含了之前所有测量值所提供的参考信息的。这在节约工作内存的同时，还很好的运用了之前的信息，毕竟间接的用了了之前的信息来对整体进行最优估计，所以这样能使系统在稳定之后波动很小。