卡尔曼滤波器在嵌入式控制系统中的应用

1、嵌入式控制系统要用卡尔曼滤波器来观测进程中的变量，以便实现进程控制。本文将介绍卡尔曼滤波器设计和实现的基本原理，然后以汽车导航问题为实例，说明汽车位置控制过程中，如何利用卡尔曼滤波器这个有效工具对汽车的当前位置进行可靠的估计。 卡尔曼滤波器最初是专为飞行器导航而研发的，目前已成功应用在许多领域中。卡尔曼滤波器主要用来预估那些只能被系统本身间接或不精确观测的系统状态。 许多工程系统和嵌入式系统都需要滤波。例如收到受噪音干扰的无线通讯信号时，良好的滤波算法在保留有用信息的同时，还可以从电磁信号中消除噪音。又如在电源电压中，未受干扰的电源是那些为了消除不期望波动而进行线电压过滤的电源设备，如果不消除

2、这些波动，将会缩短电子设备(如计算机和打印机)的寿命。 卡尔曼滤波器适用于观测进程中的变量，从数学角度看，卡尔曼滤波器可估计线性系统的状态。卡尔曼滤波器不仅能在实践中发挥很好的作用，而且在理论上也颇引人注目，因为在各种滤波器中，卡尔曼滤波器的估计误差最小。嵌入式控制系统经常需要使用卡尔曼滤波器，因为要控制一个进程，首先必须精确地估计进程中的变量。 本文将介绍卡尔曼滤波器设计和实现的基本原理。文章首先引入卡尔曼滤波器算法并用该算法解决汽车导航问题。为了控制汽车的位置，首先必须对汽车的当前位置进行可靠的估计。卡尔曼滤波器正是具备这项功能的有效工具。线性系统 为了用卡尔曼滤波器消除信号中的噪音，被测

3、量的进程必须能用线性系统描述。许多物理进程，如路上行驶的车辆、围绕地球轨道运转的卫星、由绕组电流驱动的电机轴或正弦射频载波信号，均可用线性系统来近似。线性系统是指能用如下两个方程描述的简单进程： 状态方程： 输出方程： 在上述方程中，A、B和C均为矩阵，k是时间系数，x称为系统状态，u是系统的已知输入，y是所测量的输出。w和z表示噪音，其中变量w称为进程噪音，z称为测量噪音，它们都是向量，因此包含多个元素。x中包含系统当前状态的所有信息，但它不能被直接测量。因此要测量向量y，y是受到噪音z干扰的系统状态x的函数。我们可以利用y获取x的估计，但不一定必须藉由y的测量值来获得x的估计，因为y受到了

4、噪音的干扰。 例如在沿直线运动的汽车模型中，其状态由汽车的位置p和速度v构成。输入u是控制加速度，而输出y则是测量位置。假定每隔T秒时间都能改变加速度并测量汽车的位置，根据物理学基本定律，速度v将由下述方程控制： 也就是说，下一时刻(T秒之后)的速度将等于当前速度加上控制加速度与T的乘积。但前面的方程并未给出vk+1时刻的精确值，因为实际上该速度将受到阵风及其它意外噪音的干扰。这些速度噪音是随时间变化的随机变量，因此下述方程能更好地反映v的实际情况： 其中(方程17)表示速度噪音。类似地，可以推导出位置p的方程： 其中(方程18)表示位置噪音。下式提出了由位置和速度构成的状态向量x： 最后，由

5、于测量输出是汽车的位置，可以得到线性系统的状态方程如下： zk表示由仪器误差等因素带来的测量噪音。如果希望藉由反馈系统控制汽车，则需要得到位置p和速度v的精确估计，换句话说，即需要对状态x进行估计。卡尔曼滤波器正是状态估计的有效工具。卡尔曼滤波器的原理和算法 假定线性系统的模型如前所述，我们希望利用测量得到的y来估计系统x的状态。由于系统的行为遵循其状态方程，而且可以获得系统的位置测量，那么如何确定状态x的最佳估计呢？我们需要能对实际状态进行精确估计的预估器，尽管并不能直接测量该状态。那么预估器又应当满足哪些准则呢？显然必须满足以下两个条件。 首先，状态估计的平均值应等于实际状态的平均值。换言

6、之，我们并不希望估计出现这样或那样的偏差。在数学上，人们总希望估计的期望值应等于状态的期望值。 其次，我们希望状态估计与实际状态之间的偏差尽可能小。也就是说，不仅希望状态估计的平均值等于实际状态的平均值，而且希望预估器的状态估计与实际状态之间的偏差尽可能达到最小。在数学上，人们总希望预估器的误差方差尽可能最小。 卡尔曼滤波器正是能满足以上两条准则的预估器，但卡尔曼滤波器解决方案也必须满足特定的噪音条件。在系统模型中，w表示进程噪音，而z表示测量噪音。因此，必须假定w和z的平均值均为0且w与z不相关。这样，在任何时刻k、wk和xk均为不相关的随机变量，而噪音协方差矩阵Sw和Sz分别为： 进程噪音

7、协方差： 测量噪音协方差： 其中wT和zT分别表示随机噪音向量w和z的转置，E()表示期望值。 现在就可以开始研究卡尔曼滤波器方程了。首先，可以选择不同的等价方程来描述卡尔曼滤波器，其中一种表述如下： 卡尔曼滤波器方程由3个方程组成，每个方程都包含矩阵运算。在上述方程中，a-1表示矩阵求逆，而aT上表示矩阵转置。K矩阵称为卡尔曼增益，而P矩阵表示估计误差的协方差。 上述状态估计方程相当直观。方程中用来推导k + 1时刻状态估计的第一项正好等于A与k时刻状态估计的乘积加上B与k时刻已知输入的乘积。如果不再进行测量，那么该值就是系统的状态估计。换言之，状态估计可以像系统模型中的状态向量一样传递至后

8、续时刻。方程中第二项称为校正项，表示在测量条件下，用于对传递的状态估计进行修正的校正量。 K方程的校验表明：测量噪音较大，Sz也将较大，因此K值应当较小，而且在计算下一时刻的测量值y时，不应过分信赖该值；另一方面，测量噪音较小，Sz也将较小，这样K值应当较大，而且在计算下一时刻的测量值时，可以充分信赖该值。 汽车导航应用实例 现在考虑先前提出的汽车导航问题。汽车沿直线行驶，位置测量的误差为10英尺(一个标准偏差)；命令加速度是常数，其值为1英尺/秒2；加速度噪音为0.2英尺/秒2 (一个标准偏差)；位置测量的周期为0.1秒(T=0.1)。那么如何得到运动汽车位置的最佳估计？由于存在较大的测量噪

9、音，可以肯定计算结果比测量值更好。因为T=0.1，表征系统的线性模型可用本文提出的系统建模方法推导得到： 因为测量噪音的标准偏差为10英尺，Sz矩阵可简单地取值为100。 现在推导Sw矩阵。因为位置与加速度的0.005倍成正比并且加速度噪音为0.2英尺/秒2，因此位置噪音的偏差为(0.005)2×(0.2) 2= 10-6。类似地，因为速度与加速度的0.1倍成正比，因此速度噪音的偏差为(0.1)2×(0.2)2=4×10-4。最后，位置噪音和速度噪音的协方差等于位置噪音的标准方差与速度噪音的标准方差的乘积，计算结果为(0.005×0.2)×(0

10、.1×0.2)=2×10-5。将以上计算结果相结合，即可得到矩阵Sw： 接着，为位置和速度设置最佳的初始值，并将(方程16)初始化为最初估计的误差。然后每隔一个时间步长执行一次卡尔曼滤波器算法。 我们使用Matlab模拟卡尔曼滤波器，模拟结果参见以下各图。图1显示了车辆的实际位置和估计位置，两条平滑曲线表示实际位置和估计位置，由于相隔太近而难以区分。较为粗糙的曲线表示测量位置。 图2显示了实际位置和测量位置之间的误差以及实际位置与卡尔曼滤波器估计位置之间的误差。测量误差的标准偏差约为10英尺，偶尔也会出现30英尺(3)的峰值。估计位置的误差则一直保持约为2英尺。 图3显示了

11、卡尔曼滤波器的优势。因为汽车的速度是状态x的一部份，因此我们得到位置估计的同时也得到了速度估计。图4显示了实际速度和卡尔曼滤波器估计速度之间的误差。 得到上述结果的Matlab程序如表1所示。Matlab是一种很容易理解的语言，很像伪码(pseudocode)，但具有内置矩阵运算功能。使用Matlab执行程序时，每次得到的结果会有所不同，这是因为模拟中存在随机噪音，但总体上与这?给出的各图都很相近。实际问题及其扩展 卡尔曼滤波器的基本原理很简单，但滤波器方程对矩阵代数的依赖性很强。表2显示了用C语言描述的卡尔曼滤波器修正方程。表2中的矩阵代数参考可见。该网站列出的程序非常全面，如果问题足够小，

12、程序还能相应简化。例如，矩阵的转置： 等于： 因此，如果需要对矩阵进行转置，就可使用上述方程。矩阵运算的补充C代码和卡尔曼滤波可参见。具有超过3个状态的系统将显著增加程序的代码长度和计算量。与矩阵转置相关的计算量正比于n3(这?n表示矩阵的维数)。这意味着，如果卡尔曼滤波器的状态数目增加1倍，计算量将增至原来的8倍。对于一个维数适中的卡尔曼滤波器，几乎所有的处理工作都花在矩阵运算上。但稳态卡尔曼滤波器在提供良好预估性能的同时，还能极大地削减运算量，因此也不必过于担心。在稳态卡尔曼滤波器中，矩阵Kk和Pk均为常量，因此在代码中可定义为常值，而卡尔曼滤波器方程中唯一需要实时实现的是(方程15)方程

13、，由简单的乘积和加法运算组成(如果使用的是DSP，则由乘积和累加运算组成)。 我们已经讨论了线性系统的状态估计，但如何估计非线性系统的状态呢？事实上，在工程中几乎所有的进程都是非线性的。一些非线性系统非常接近线性系统，而另一些则差别甚大。这一点早在卡尔曼滤波器发展初期阶段就引起了重视，因而推出了“扩展卡尔曼滤波器”。扩展卡尔曼滤波器只是简单地将线性卡尔曼滤波原理推广到非线性系统。 到目前为止，我们已经逐步讨论了藉由测量对状态进行估计的步骤。获得整个测量时序记录后，如果希望将状态作为时间的函数进行预估，又应当如何处理呢？例如在上述示例中，如何在获得必要信息的基础上，重构汽车的行驶轨迹呢？我们似乎

14、完全可以比采用卡尔曼滤波器做得更好，因为在k时刻预估系统状态时，我们不仅能测量k时刻之前(包括k时刻)的状态，还能测量k时刻之后的状态。为此必须修正卡尔曼滤波器，这就是卡尔曼平滑滤波器的由来。 卡尔曼滤波器不仅很实用，其工作原理也很有吸引力。卡尔曼滤波器可减小估计误差的偏差，但如果我们更关心最坏情况下的估计误差，或者希望减小“最坏”的估计误差，而不是“平均”误差时，问题又该如何解决呢？H滤波器可以解决这个问题。H滤波器(读作“H无限大”滤波器，有时写作H )是上世纪八十年代开发出的卡尔曼滤波替代方案。H滤波器应用不如卡尔曼滤波器广泛，因此也不像卡尔曼滤波器那样广为人知，但在某些情况下更为有效。

15、 卡尔曼滤波理论假定进程噪音w与测量噪音z互不相关。如果在系统中进程噪音和测量噪音相关，那么应当如何处理呢？这就是相关噪音问题，同样可以藉由修正卡尔曼滤波器来解决这个问题。此外，卡尔曼滤波器还需要明确的噪音协方差Sw和Sz。如果Sw和Sz未知，又该如何处理呢？我们如何能够获得状态的最佳估计？这需要再次求助于H滤波器。 卡尔曼滤波涉及的领域很广泛，不是本文就能尽述的。自1960年问世以来，业界已经发表了数千篇有关卡尔曼滤波的论文，出版了数十本教材。 发展历史及其展望 卡尔曼滤波器最初由Rudolph Kalman开发设计，其研究成果发表在一家著名的期刊上，因为它比其它人的研究成果更为通用也更完整

16、，因而被命名为卡尔曼滤波器，有时也被称为Kalman-Bucy滤波器，因为Richard Bucy早期曾与Kalman共同研究该滤波器。 卡尔曼滤波算法的起源可以追溯到1795年由年仅18岁的Karl Gauss提出的最小二乘理论。卡尔曼滤波算法与许多新技术一样，也是致力于解决特定问题，例如阿波罗空间计划中的航天飞机导航问题。从此，卡尔曼滤波器逐渐应用到许多领域，包括各种导航(航空太空、陆地和海洋)、核电站设备、人口统计建模、制造业、地层放射性探测以及模糊逻辑和神经网络学习。 Dan Simon是Cleveland大学电子工程和计算机工程系的教授和工业顾问。他的教学和研究方向包括滤波、控制理论

17、、嵌入式系统、模糊逻辑和神经网络。目前，他正试图利用卡尔曼滤波器实现基于DSP的电机控制器，可以藉由电子邮件：与他联系。 参考文献 1. Gelb, A. Applied Optimal Estimation. Cambridge, MA: MIT Press, 1974。这是一本“过时但仍然叫座”的参考书，它由麻省理工大学出版社出版，内容浅显易懂，不仅从介绍最基本的原理入手，而且还非常注重实际应用。 2. Anderson, B. and J. Moore. Optimal Filtering. Englewood Cliffs, NJ: Prentice-Hall, 1979。这是一本纯数学参考书，非常深奥，但有助于理解卡尔曼滤波算法及相关问题的基本原理。 3. Grewal, M. and A. Andrews. Kalman Filtering Theory and Practice. Englewood Cliffs, NJ: Prentice-Hall, 1993。本书介于以上两本参考书之间，在理论和实践之间架起了一座桥梁。本书的一大特色是磁盘中包含卡尔曼滤波算法的原始码，不足之处在于原始码是用Fortran语言编写的。 4. Sorenson, H