基于MATLABGUI的Kalman滤波程序

1、序百-1 -第1章 Kalman滤波理论.-2 -1.1 Kalman滤波理论的应用背景 -2 -1.2 卡尔曼滤波的基本思想 -3 -1.3 卡尔曼滤波理论基础 -4 -1.4 卡尔曼滤波参数的估计和调整 -5 -1.5 小结-7 -第2章 MATLAB及GUI编程概述 -8 -2.1 MATLAB 简介-8 -2.2 MATLAB_GU 概述-8 -第3章 Kalman滤波GUI和程序的设计 -10-3.1 Kalman滤波器的应用实例 -10 -3.2 Kalman滤波GUI设计所实现的任务 -11 -3.3 GUI界面设计-11 -第4章在MATLAB_GUIT运行程序.-18 -4.

2、1 通过 MATLAB_GU运行2果-18 -4.2 运行结果分析 -18 -参考文献-20 -1 Kalman R.E.A New Approach to Linear filtering and ProdictionTheory.Trans.ASME.Journal of Basic Eng,1960,82D:35-46-.20-2 - 20 -4 - 20 -致词小错误！未定义书签。序言最佳线性滤波理论起源于40年代美国科学家 Wiener和前苏联科学家K o ji m o r等队的研究工作，后人统称为维纳滤波理论。从理论上说，维纳滤波 的最大缺点是必须用到无限过去的数据，不适用于实时处

3、理。为了克服这一缺点， 60年代Kalman把状态空间模型引入滤波理论，并导出了一套递推估计算法，后 人称之为卡尔曼滤波理论。1960年，R.E. Kalman出版了他的著名论文，描述了一个离散线性系统滤波 问题的递归的解决方案。Kalman滤波器是一组数学等式，它提供了一个有效的 计算（回归）方法，来估计过程的状态，在这种方法中，将均方差最小化。该滤 波器功能非常强大，它支持过去、现在、甚至将来状态的估计，并当模型系统的 精确特性未知的情况下进行估计。卡尔曼滤波器（Kalman Filter）是一个最优化自回归数据处理算法（optimal recursive data processing

4、algorithnm 。对于解决彳艮大部分的问题，他是最优，效 率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制， 传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。目前Kalman滤波器的设计有许多现成的高级语言设计程序，但他们都存在 设计效率低，不具有可视图形，不便于参数修改等缺点，而 MATLAB_GUI为 Kalman滤波器的研究和应用提供了一个直观、高效、便捷的利器。它以矩阵运 算为基础，把计算、可视化、仿真以及设计融合到一个交互式的工作环境中。本 文基于MATLAB_GUI对Kal

5、man滤波器进行设计和仿真。第1章Kalman滤波理论1.1 Kalman 滤波理论的应用背景信号是传递和运载信息的时间或空间函数。信号有两类，即确定性信号和随 即信号。确定性信号的变化规律是既定的，可以表示为一确定的时间函数或空间 函数，具有确定的频谱特性，如阶跃信号、脉宽固定的矩形脉冲信号，正余弦函 数等，它们对于指定的某一时刻，可确定一相应的函数值。随即信号没有既定的 变化规律，不能给出确定的时间或空间函数，在相同的初始条件和环境条件下， 信号每次实现都不一样，如陀螺漂移、惯性导航系统的导航误差、GPS的SA误差、海浪等，随即信号尽管没有确定的频谱特性，但是可以知道它的统计特性， 即具有

6、确定的功率谱。信号在传输与检测过程中不可避免地要受到外来干扰与设备内部噪声的影 响，使接收端收到的信号具有随机性。为获取所需信号，排除干扰，就要对信号 进行滤波。所谓滤波，是指从混合在一起的诸多信号中提取出所需信号的过程。 信号的性质不同，获取的方法就不同，即滤波的手段不同。对于确定性信号，由 于其具有确定的频谱特性，可根据各信号所处频带的不同，设置具有相应频率特 性的滤波器，如低通滤波器、高通滤波器、带通滤波器及带阻滤波器等，使有用 信号无衰减地通过，而干扰信号收到抑制。这类滤波器可用物理的方法实现，即 模拟滤波器，亦可用计算机通过算法实现，即数字滤波器。对确定性信号的滤波 处理通常称为常规

7、滤波。随即信号具有确定的功率谱特性，可根据有用信号和干扰信号的功率谱设计 滤波器。美国学者维纳（N.Wiener）等人提出了 Wiener滤波，它通过做功率谱 分解设计滤波器，在对信号做抑制和选通这一点同常规滤波是相似的。由于在频域进行Wiener滤波器设计需要求解维纳-霍普方程，且计算量较大，需要大量的 存储空间，妨碍了 Wiener滤波的应用。Kalman滤波是卡尔曼于1960年提出的 从与被提取信号有关的观测量中通过算法估计出所需信号的一种滤波算法1o他把状态空间的概念引入到随即估计理论中， 把信号过程视为白噪声作用下的一个 线性系统的输出，用状态方程来描述这种输入 -输出关心，估计过程

8、中利用系统 状态方程、观测方程和白噪声激励(系统噪声和观测噪声)的统计特性形成滤波 算法，由于所用的信息都是时域内的量，所以不但可以对平稳的一维的随机过程 进行估计，也可以对非平稳的、多维随机过程进行估计。这就完全避免了 Wiener 滤波在频域内设计时遇到的限制，适用范围比较广泛。实际上，Kalman滤波是一套由计算机实现的实时递推算法，它所处理的对 象是随机信号，利用系统噪声和观测噪声的统计特性，以系统的观测量作为滤波 器的输入，以所要估计值(系统的状态或参数)作为滤波器的输出，滤波器的输 入与输出之间是由时间更新和观测更新算法联系在一起的，根据系统方程和观测 方程估计出所有需要处理的信号

9、。所以，此处所谈的Kalman滤波与常规滤波的涵义与方法完全不同，实质上是一种最优估计方法。1.2 卡尔曼滤波的基本思想卡尔曼滤波器是线性无偏最小均方误差递推滤波器。与维纳滤波相比，在平稳条件下，它们所得到的稳态结果是一致的。然后，它们解决的方法有很大区别。 维纳滤波是根据全部过去的和当前的观察数据来估计信号的当前值，它的解是以均方误差最小条件下所得到的系统的传递函数H(z)或单位样本响应h(n)的形式给出的，因此称这种系统为最佳线性过滤器或滤波器。 而卡尔曼过滤是用前一个 估计值和最近一个观察数据来估计信号的当前值，是用状态方程和递推的方法进 行估计的，其解是以估计形式给出。因此称这种系统为

10、线性最有估计器或滤波器。 卡尔曼滤波过程实际上是获取维纳解的递推运算过程 2,卡尔曼过滤中信号和噪 声是状态方程和测量方程表示的，因此设计卡尔曼滤波器要求已知状态方程和测 量方程。标准卡尔曼滤波器是在最小均方误差准则下的最佳线性过滤器，就是说，它使系统的状态向量和状态向量的预测值之间的均方误差达到最小，它用状态方程和递推方法进行估计，它的解是以估计值形式给出的。由于它能够对物体的运动 建立某种模型，因此在跟踪中经常被用到。当观测方程不是线性时，上述标准卡 尔曼方程不再适用。总的来说，卡尔曼滤波器是一个线性的估计器， 能够有效地 跟踪物体的运动和形状变化，但它基于两个假设：一是背景相对干净；二是

11、运动 参数服从高斯分布，因而适用范围有限。1.3 卡尔曼滤波理论基础在这一部分，我们就来描述源于 Dr Kalman的卡尔曼滤波器。下面的描述， 会涉及一些基本的概念知识，包括概率(Probability),随机变量(Random Variable),高斯或正态分配(Gaussian Distribution)还有 State-space Mode等等。 但对于卡尔曼滤波器的详细证明，这里不能一一描述。(1)在介绍卡尔曼滤波算法之前，先简要介绍白噪声的概念。所谓高斯白噪声中的高斯是指概率分布是正态函数， 而白噪声是指它的二阶 矩不相关，一阶矩为常数，是指先后信号在时间上的相关性。这是考查一个信

12、号 的两个不同方面的问题。高斯白噪声:如果一个噪声，它的幅度分布服从高斯分 也，而它的功率谱密度又是均匀分布的，则称它为高斯白噪声。(2) Kalman滤波器算法首先，我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微 分方程 (Linear Stochastic Difference equation 来描述：Xk = AXk-1 BUk Wk(1.1)再加上系统的测量值：Z k = HX k V k(1.2)上两式子中，X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。A 和B是系统参数，对于多模型系统，他们为矩阵。Z(k)是k时刻的测量值，H是测量系统的参数，对于多测

13、量系统，H为矩阵。W(k)和V(k)分别表示过程和测 量的噪声。他们被假设成高斯白噪声(White Gaussian Noise ,他们的协方差分别 是Q, R (这里我们假设他们不随系统状态变化而变化)。对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡 尔曼滤波器是最优的信息处理器。下面我们来用他们结合他们的 covariances来 估算系统的最优化输出。首先我们要利用系统的过程模型，来预测下一状态的系统。假设现在的系统 状态是k,根据系统的模型，可以基于系统的上一状态而预测出现在状态：(1.3)X k |k -1 = AX k -1| k -1 BU k式(1.3)中

14、，X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)是上一状态最优的结果，U(k)为现在状态的控制量，如果没有控制量，它可以为00到现在为止，我们的系统结果已经更新了，可是，对应于X(k|k-1)的covariance 还没更新。我们用P表示covariance：P k | k _1)=AP k -1| k -1 A Q(1.4)式(1.4)中，P(k|k-1)是 X(k|k-1)对应的 covariance, P(k-1|k-1)是 X(k-1|k-1)对 应的covariance, A表示A的转置矩阵，Q是系统过程的covariance式子1.3、 1.4就是卡尔曼滤波器5个公

15、式当中的前两个，也就是对系统的预测。现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。结 合预测值和测量值，我们可以得到现在状态(k)的最优化估算值X(k|k)：X k|k =Xk|k-1 Kg k Z k -HXk|k-1(1.5)其中Kg为卡尔曼增益(Kalman Gain)：Kg k )=P k| k -1 H / HP k | k-1 H R (1.6)到现在为止，我们已经得到了 k状态下最优的估算值X(k|k)。但是为了要使 卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下X(k|k) 的 covariance：Pk|k =I-KgkHPk|k-1(1.

16、7)其中I为1的矩阵，对于单模型单测量I=1。当系统进入k+1状态时，P(k|k) 就是式子(1.4)的P(k-1|k-1)。这样，算法就可以自回归的运算下去。卡尔曼滤波器的原理基本描述了，式子1.3 , 1.4 , 1.5 , 1.6和1.7就是他的5个基本公式。根据这5个公式，可以很容易的实现计算机的程序。由上面可以看出，滤波过程是以不断地“预测一修正”的递推方式进行计算， 先进行预测值计算，再根据观测值得到的新信息和Kalman增益(加权项)，对预测值进行修改。由滤波值可以得到预测，又由预测可以得到滤波，其滤波和预 测相互作用，并不要求存储任何观测数据，可以进行实时处理。1.4卡尔曼滤波

17、参数的估计和调整在卡尔曼滤波的使用中，通常首先要测量噪声协方差 R。可以通过离线测量实验确定。但是确定过程噪声协方差 Q一般更困难，因为无法直接观察过程。当 对一个相对简单的过程模型加入足够的不确定性时（通过选择合适的Q）得到的结果是可信的。无论如何，通过运行中调整 Q和R可以得到较好的滤波效果。调 整通常是离线的，通常借助于过程的另一个卡尔曼滤波进行系统辨识来实现。在Q和R为常数的情况下，估计误差协方差Pk和卡尔曼增益Kk都会迅速镇定，然 后保持为常量。如果是这样的话，可以通过离线运行滤波器来预先确定这些参数。然而通常的情况是测量误差不会保持常量。 例如，当观察光电跟踪器面板上 的亮点时，对

18、附近的亮点的测量误差要小于远处的。有时过程噪声Q在滤波器运行中是动态变化的，变成了 Qk,以调整符合不同的动态特点。例如在跟踪三 维虚拟环境中的用户头部时，如果用户慢速移动，就要减小 Qk值，如果动态变 化加快就增加Qk值。在这类情况下，通过选择来适应用户意图的不确定性以及 模型中的不确定性3。（1）测量方程协方差的确定测量方程是由测量设备可以实现的测量量来确定形式的，例如激光测距仪可以进行目标位置径向位置和角度位置的测量（极坐标测量），因此这两个量就是 测量方程的输出。测量方程的协方差可以通过对激光测距仪的试验来确定。方法是利用在测距仪和目标都固定的情况下， 激光测距仪对已知位置的目标进行多

19、次 测量，得到一组测量数据，然后利用 MATLAB软件求出这组数据的协方差就可 以作为测量方程的协方差了。当然测距仪在测量不同距离的目标时， 测量的误差 可能不同，所以就有不同的协方差，可以取一个中间位置对应的协方差为仪器的 协方差。或者取测量范围内最大的协方差。（2）状态方程（过程方程）的协方差确定过程方程的协方差比较难以确定，一种方法是对于比较容易观察过程变化的 系统，通过经验设定协方差。另一种需要通过仿真来调整。对于给定的过程方程， 先预设一个协方差，通过设定一系列标准的测量值，仿真观察卡尔曼滤波是否很 快收敛，如果不收敛或者收敛慢则可以调整过程协方差来达到较好的效果。1.5小结Kalm

20、an滤波器是一个最优化递归处理算法 (optimal recursive data processing algorithm)。(1)最优(optimal)依赖于评价性能的判据。Kalman滤波器充分利用如下 信息估计感兴趣变量当前取值：a.系统和测量装置的动态特性；b.系统噪声、测 量误差和动态模型的不确定性的统计描述；c.感兴趣变量的初始条件的相关信 息。(2)递归(recursive)是指Kalman不需要保存先前的数据，当进行新的测 量时也不需要对原来数据进行处理。(3) filter (DPA)实际上是数据处理算法，只不过是计算中处理的程序， 因此能处理离散时间测量样本，而不是连续时

21、间输入。基本假设：采用线性模型是合理的；这是典型工程模型在某些主要点或轨迹是线性的， 线性模型比非线性模型更简单。因此用线性模型来近似。白噪声意味着噪声值和时间不相关；白噪声指在整个频率上都有相同强度的 频率特性的噪声。实际应用中将频率设为常值，带宽大大超过系统带宽的噪声称 为白噪声，用高斯白噪声来模拟，可以大大简化模型。采用高斯密度函数在实践上是可行的。因为采用高斯函数在数学上容易处 理。当缺少高阶统计量时，除了假定高斯密度外，没有更好的可以表示的函数形 式。用一阶和二阶统计量完全可以描述高斯白噪声。第2章MATLAB及GUI编程概述2.1 MATLAB 简介作为具有科学计算、符号运算和图形

22、处理等多种功能的强有力实现工具，近 年来MATLABi一软件已得到了业界的普遍认可，应用领域已拓展到数值计算、 数据处理、统计分析、工程等各个方面，在各大公司、科研机构和高校得到广泛 应用，其自身也因此得到迅速发展，功能不断扩充。MATLAB是一种高效的工程计算语言，它在数值计算、数据处理、自动控 制、图像处理、神经网络、小波分析、金融分析等方面有着广泛的应用。MATLAB 系统不但提供大量涉及各个工程领域的工具箱来简化科学计算、工程设计和分析等工作，而且提供具有自身特点的编程语言，可以轻松地实现大量数据的分析、 处理及显示任务。一般而言，用户总希望将数据或设计结果用图形来表示，以使数据的特征

23、或性能能够直观地体现出来。对于一般的高级语言程序来说，绘制图形，尤其是根 据计算结果所得的不规则图形是一项较为复杂的工作，用户只有在对该语言有了较为深入的了解后才能迅速准确地绘制所需的图形，而 MATLAB面向对象的图 形技术使用户可以轻松实现自身数据或处理后数据的绘制任务。使用MATLAB提供的大量图形设计技术，用户无需了解图形实现的细节内容，有时甚至只需一 条简单的指令就可以绘制非常复杂的图形。另外，用户还可以根据需要来规划 MATLAB的图形外观，使得绘图结果完全符合用户的需求。从当前的软件趋势 来看，友好的图形界面已经成为应用程序的基本交互入口，为此， MATLAB提 供了对用户图形界

24、面（GUI）的支持，使MATLABff发的程序可以为更多的用户 所接受。2.2 MATLAB_GUI 概述图形用户界面（Graphical User Interface,简称 GUI,又称图形用户接口） 是指采用图形方式显示的计算机操作用户界面。与早期计算机使用的命令行界面 相比，图形界面对于用户来说在视觉上更易于接受。图形用户界面(GUI)是用户与计算机程序之间的交互方式，是用户与计算 机进行信息交流的方式。计算机在屏幕显示图形和文本，若有扬声器还可产生声 音。用户通过输入设备，如：键盘、鼠标、跟踪球、绘制板或麦克风，与计算机通讯。用户界面设定了如何观看和如何感知计算机、操作系统或应用程序。

25、通常, 多是根据悦目的结构和用户界面功能的有效性来选择计算机或程序。图形用户界面或GUI是包含图形对象，如：窗口、图标、菜单和文本的用户界面。以某种方 式 选择或激活这些对象，通常引起动作或发生变化。最常见的激活方法是用鼠 标或其它点击设备去控制屏幕上的鼠标指针的运动。按下鼠标按钮，标志着对象 的选择或其它动作。MATALB 可以创建图形用户界面 GUI ( GraphicalUser Interface)，它是用户 和计算机之间交流的工具。MATLAB将所有GUI支持的用户控件都集成在这个 环境中并提供界面外观、属性和行为响应方式的设置方法，随着版本的提高，这种能力还会不断加强。而且具有强大

26、的绘图功能，可以轻松的获得更高质量的曲线 图。第3章Kalman滤波GUI和程序的设计3.1 Kalman滤波器的应用实例假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一 分钟来做时间单位）。假设你的经验不是100%勺可信，可能会有上下偏差几度。 我们把这些偏差看成是高斯白噪声（ White Gaussian Noise）,也就是这些偏差 跟前后时间是没有关系的而且符合高斯分配（Gaussian Distribution ）。另外， 我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏 差

27、。我们也把这些偏差看成是高斯白噪声。于是现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他 们各自的噪声来估算出房间的实际温度值。假如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻的温度值， 来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预 测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5 是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3,你对自己预测的 不确定度是4度，他们平方相加再开方，就是5）。然后，你从温度计那里得到 了k时刻的温度值，假设是

28、25度，同时该值的偏差是4度。由于我们用于估算k时刻的实际温度有两个温度值，分别是 23度和25度。 究竟实际温度是多少？相信自己还是相信温度计？究竟相信谁多一点， 我们可以 用他们的covariance 来判断。因为KgA2=5A2/（5A2+4A2），所以Kg=0.78,我们可 以估算出k时刻的实际温度值是：23+0.78*（25-23）=24.56 度。可以看出，因为温度计的covariance比较小（比较相信温度计），所以估 算出的最优温度值偏向温度计的值。现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进 行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现

29、。对了，在 进入k+1时刻之前，我们还要算出k时刻那个最优值（24.56度）的偏差。算法如下：(1-Kg)*5A2)A0.5=2.35。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的2.35就是进入k+1时刻以后k时刻估算出的最优温度值 的偏差(对应于上面的3)。就是这样，卡尔曼滤波器就不断的把 covariance递归，从而估算出最优的 温度值。他运行的很快，而且它只保留了上一时刻的 covariance 。上面的Kg,就 是卡尔曼增益(Kalman Gain)。他可以随不同的时刻而改变他自己的值。3.2 Kalman滤波GUI设计所实现的任务Kalman滤波GUI设计所实现

30、的任务如下：(1)能够实现人机交互。(2)当单击“运行”按钮的时候，经过 Kalman滤波后的曲线出现在GUI的 axes。3.3 GUI界面设计GUI设计面板是GUI设计工具应用的平面，面板上部提供了菜单和常用工 具按钮，左边提供了多种如命令按钮、单选按钮、可编辑文本框、静态文本框、 弹出式菜单等。进行设计时，首先单击面板左边所需的控件，然后在右边的图形 界面编辑区中再次单击某一恰当位置，这时将在该位上为图形界面添加相应的 控件。一个图形界面的完成，除了设计其外观外，还有相当的一部分是通过属性 的设来完成的。因此在设置这些属性时，要注意下面几个常用又很重要的属性设 置：(1)控件风格和外观B

31、ackgroundColor：设置控件背景颜色，使用R G B或颜色定义。CData：在控件上显示的真彩色图像，使用矩阵表示。 ForegroundColor：文本颜色。String属性：控件上的文本，以及列表框和弹出菜单的选项。Visible：控件是否可见。(2)对象的常规信息Enable属性：表示此控件的使能状态，设置为 on”，表示可选，为“off 时则表示不可选。Style：控件对象类型。Tag:控件表示（用户定义）。TooltipString属性：提示信息显示。当鼠标指针位于此控件上时，显示提 示信息。UserData用户指定数据。Position：控件对象的尺寸和位置。Units：

32、设置控件的位置及大小的单位。 有关字体的属性，如 FontAngle, FontName等。（2）控件回调函数的执行BusyAction ：处理回调函数的中断。有两种选项：即Cancel：取消中断事件，queue排队（默认设置）。ButtonDownFcn属性：按钮按下时的处理函数。CallBack属性：是连接程序界面整个程序系统的实质性功能的纽带。该属性值应该为一个可以直接求值的字符串，在该对象被选中和改变时，系统将自动地对字符串进行求值。CreateFcn在对象产生过程中执行的回调函数。DeleteFcn：删除对象过程中执行的回调函数。Interruptible属性：指定当前的回调函数在执

33、行时是否允许中断，去执行 其他的函数。（4）控件当前状态信息ListboxTop：在列表框中显示的最顶层的字符串的索引。Max：最大值。Min ：最小值。Value：控件的当前值。应用MATLAB制作这样一个过程是非常方便的，我们可以通过GUI操作 来看到。该环境下要设计一个界面友好的仿真软件，一般应完成以下两个步骤：（1） GUI界面设计。主要是通过不同的文本框、按钮等许多工具的使用，设计出一个图形用户界面。要清楚这个图形界面的功能是什么，即在图形界面上 的操作会引发什么样的结果。(2)回调函数的设计。用户应根据设计好的图形界面的功能，针对各个不 同的图形对象来编写出能够实现该功能的函数代码

34、，确保这个图形界面能够完成 所预定的功能。在matlab命令行窗口中输入guide或点击matlab界面上面的菜单中的 哲 图 标，打开gui设计的图形界面如图3.1 。然后拖入所要的图形控件，按需要修改 外观和空间属性，直至满足要求，如图 3.2。设置属性点击guide界面上方的Run 按钮，会生成一个fig文件，一个mt件，其中fig文件就是界面的图形，mt件是 界面的回调函数，在mt件里每个控件的回调函数都已经自动生成，控件要做的 工作就是在文件框架下定义某些特殊要求的状态并补充完整回调函数场，使单击控件时激活回调程序完成一定的功能。图3.1 GUI设计的图形界面图3.2 Kalman滤

35、波的GUI界面设计3.4 Kalman滤波程序一一回调函数的编写过程基于上面的例子，首先，我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程(Linear Stochastic Difference equation )来描述：X k)= AX k -1 BU k Wk(3.1)再加上系统的测量值：Z k = HX k V k(3.2)上两式子中，X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。A 和B是系统参数，对于多模型系统，他们为矩阵。Z(k )是k时刻的测量值，H是 测量系统的参数，对于多测量系统，H为矩阵。W(k)和V(k)分别表示过程和测 量的噪声。他们

36、被假设成高斯白噪声(White Gaussian Noise),他们的covariance分别是Q, R (这里我们假设他们不随系统状态变化而变化)对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡 尔曼滤波器是最优的信息处理器。下面我们来用他们结合他们的covariances来 估算系统的最优化输出(类似上一节那个温度的例子)。首先我们要利用系统的过程模型，来预测下一状态的系统。假设现在的系统 状态是k,根据系统的模型，可以基于系统的上一状态而预测出现在状态：X k | k -1V-AX k -1| k -1BU k(3.3)式(3.3)中，X(k|k1 )是利用上一状态预

37、测的结果，X(k1|k1)是上一状态最优的结果，U(k)为现在状态的控制量，如果没有控制量，它可以为00到现在为止，我们的系统结果已经更新了，可是，对应于X(k|k1)的covariance 还没更新。我们用P表示covariance :P k | k -1 ：| AP k -1| k -1 A Q(3.4)式(3.4)中，P(k|k1)是 X(k|k-1 闷应的 covariance , P(k-1|k-1)是X(k-1|k-1阳应(covariance, A表示A的转置矩阵，Q是系统过程的covariance 。式子3.3, 3.4就是卡尔曼滤波器5个公式当中的前两个，也就是 对系统的预测

38、。现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。结 合预测值和测量值，我们可以得到现在状态(k)的最优化估算值X(k|k)：X k | k )= X k | k -1Kg k Z k - HX k |k -1(3.5)其中Kg为卡尔曼增益(Kalman Gain):Kg k = Pk|k-1H /HPk|k-1H R(3.6)到现在为止，我们已经得到了 k状态下最优的估算值X(k|k)0但是为了要另卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新 k状态下X (k | k )白covariance :P k|k = I -Kg k H P k|k -1(3.7)其中I

39、为1的矩阵，对于单模型单测量,I=1当系统进入k+1状态时,P(k | k 蹴是式子(3.4)的 P(k 1| k -1 )。这样，算法就可以自回归的运算下去。卡尔曼滤波器的原理基本描述了，式子3.3、3.4、3.5、3.6和3.7就是他的5个基本公式。根据这5个公式，可以很容 易的实现计算机的程序。结合以上例子和描述，把房间看成一个系统，然后对这个系统建模。当然， 我们见的模型不需要非常地精确。我们所知道的这个房间的温度是跟前一时刻的 温度相同的，所以A=1。没有控制量，所以U k =00因此得出：X k |k -1 =X k -1|k -1(3.8)式子(3.4)可以改成：P k | k

40、-1 =Pk-1|k-1 Q(3.9)因为测量的值是温度计的，跟温度直接对应，所以H=1。式子3.5 , 3.6 , 3.7 可以改成以下：Xk|k =Xk|k-1 Kg k Zk -Xk|k-1(3.10)Kg k = Pk|k-1/Pk|k-1 R(3.11)P k|k = 1-Kg k P k|k -1(3.12)现在我们模拟一组测量值作为输入。 假设房间的真实温度为25度，我模拟 了 100个测量值，这些测量值的平均值为25度，但是加入了标准偏差为几度的 高斯白噪声。为了令卡尔曼滤波器开始工作，我们需要告诉卡尔曼两个零时刻的 初始值，是X(0|0)和P(0|0)。他们的值不用太在意，随

41、便给一个就可以了，因 为随着卡尔曼的工作，X会逐渐的收敛。但是对于P, 一般不要取0,因为这样可 能会令卡尔曼完全相信你给定的X(0|0)是系统最优的，从而使算法不能收敛。我 选了 X(0|0)=1 度,P(0|0)=2。该系统的真实温度为25度，红线是卡尔曼滤波器输出的最优化结果(该结 果在算法中设置了 Q=0.25, R=0.25)。下面我就可以来编写“运行”按钮的回调 函数了。“运行”按钮的回调函数 pushbutton1_Callback根据选定的参数显示设计的 Kalman滤波曲线，其程序如下：%点击“运行”按钮立即运行程序，实现Kalman滤波器设计程序function pushbutton1_Callback(hObject, eventdata, handles)% hObject handle to pushbutton1 (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA) clear all;tempr=25*ones(1,100);tempr_n=