卡尔曼滤波算法在运动控制中的应用

1.    系统模型介绍

       (1) 系统离线型状态方程

       首先我们需要用方程来描述被观测的系统，因为后面滤波器的方程要用到这里的一些参 数。卡尔曼滤波器适用于线性系统，由 k-1 时刻到k 时刻，系统状态预测方程：

x(k) = A ·x(k- 1) + B ·u(k) + w(k)

       系统状态观测方程

z(k) = H ·x(k) + y(k)

       (2) 变量说明

                       x(k) —— k 时刻系统的状态

                       u(k) ——  控制量

                        w(k) —— 符合高斯分布的过程噪声，其协方差在下文中为 Q

                       z(k) —— k 时刻系统的观测值

                       y(k) —— 符合高斯分布的测量噪声，其协方差在下文中为 R

                       A——状态转移矩阵

                       B——输入增益矩阵

                       H——量测矩阵

                        A 、B 、H ——  系统参数，多输入多输出时为矩阵，单输入单输出时就是几个常数注意在后面滤波器的方程中我们将不会再直接面对两个噪声 w(k)和 y(k)，而是用到他  们的协方差Q 和R。至此，A 、B 、H 、Q 、R 这几个参数都由被观测的系统本身和测量过程 中的噪声确定了。

2.   卡尔曼滤波器

       卡尔曼估计实际由两个过程组成：预测与校正，在预测阶段，滤波器使用上一状态的估计，做出对当前状态的预测。在校正阶段，滤波器利用对当前状态的观测值修正在预测阶段 获得的预测值，以获得一个更接进真实值的新估计值。

(1) 卡尔曼滤波计算公式

预测：

x(k|k-1) =A ·x(k-1|k-1) + B·u(k)

P(k|k-1) =A ·P(k-1|k-1) ·A^T + Q

校正：

z(k) = H ·x(k) + y(k)

K(k) = P(k|k-1) ·H^T ·(H ·P(k|k-1) ·H^T + R)^(-1)

x(k|k) = x(k|k-1) + K(k)·(z(k) - H ·x(k|k-1))

更新协方差估计：

P(k|k) = (I - K(k) ·H) ·P(k|k-1)

(2) 推导过程简述

       工程中用到的卡尔曼滤波是一个迭代的过程，每得到一个新的观测值迭代一次，来回来 去地更新两个东西：“系统状态”(x)和“误差协方差”(P)。由于每次迭代只用到上一次迭代的 结果和新的测量值， 这样滤波对计算资源的占用是很小的。每一次迭代， 两个步骤：预测和修正。预测是根据前一时刻迭代的结果，即 x(k-1|k-1) 和 P(k-1|k-1)，来预测这一时刻的系统状态和误差协方差，得到 x(k|k-1)和 P(k|k-1)：

x(k|k-1) =A ·x(k-1|k-1) + B ·u(k)

P(k|k-1) =A ·P(k-1|k-1) ·AT + Q

(这里用到的A、B、H、Q、R 就是从前面的状态/观测方程中拿来的)

       然后计算卡尔曼增益 K(k)，和这一次的实际测量结果 z(k)一起，用于修正系统状态 x(k|k-1)及误差协方差 P(k|k-1)，得到最新的 x(k|k)和 P(k|k)：

K(k) = P(k|k-1) ·HT ·(H ·P(k|k-1) ·HT + R)-1

x(k|k) = x(k|k-1) + K(k) ·(z(k) - H ·x(k|k-1))

P(k|k) = (I - K(k) ·H) ·P(k|k-1)

       至此这次迭代就算结束了，x(k|k)就是我们要的滤波后的值，它和 P(k|k)将会作为 x(k-1|k-1)和 P(k-1|k-1)用在下一时刻的迭代里。别看现在公式还是一大堆，在我们所谓的“简单场景”下，系统参数一定下来，就能简化很多很多。

先看状态方程和观测方程。假设我们是在测温度、加速度或者记录蓝牙 RSSI，此时控 制量是没有的， 即：

另外，参数A和H也简单地取1。现在滤波器的预测方程简化为：

① x(k|k-1) = x(k-1|k-1)

② P(k|k-1) = P(k-1|k-1) + Q

同时修正方程变成：

③ K(k) = P(k|k- 1) / (P(k|k- 1) + R)

④ x(k|k) = x(k|k- 1) + K(k) ·(z(k) - x(k|k- 1))

⑤ P(k|k) = (1 - K(k)) ·P(k|k- 1)

①对②、③无影响，于是④可以写成：

x(k|k) = x(k- 1|k- 1) + K(k) ·(z(k) - x(k- 1|k- 1))

在程序中，该式写起来更简单：

x = x + K * (新观测值 - x);

       观察②，对这一时刻的预测值不就是上一时刻的修正值+Q 嘛，不妨把它合并到上一次迭代 中，即⑤改写成：

P(k+1|k) = (1 - K(k)) ·P(k|k- 1) + Q

       这一时刻的 P(k+1|k)，会作为下一时刻的 P(k|k- 1)，刚好是③需要的。于是整个滤波过程只 用这三个式子来迭代即可：

K(k) = P(k|k- 1) / (P(k|k- 1) + R)

x(k|k) = x(k- 1|k- 1) + K(k) ·(z(k) - x(k- 1|k- 1))

P(k+1|k) = (1 - K(k)) ·P(k|k- 1) + Q

3.    卡尔曼滤波算法的实现