资源描述
目 录序言- 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_GUI概述- 8 -第3章 Kalman滤波GUI和程序的设计- 10 -3.1 Kalman滤波器的应用实例- 10 -3.2 Kalman滤波GUI设计所实现的任务- 11 -3.3 GUI界面设计- 11 -第4章 在MATLAB_GUI下运行程序- 18 -4.1 通过MATLAB_GUI运行结果- 18 -4.2运行结果分析- 18 -参考文献- 20 -1 Kalman R.E.A New Approach to Linear filtering and Prodiction Theory.Trans.ASME.Journal of Basic Eng,1960,82D:35-46.- 20 -2- 20 -4- 20 -致 谢- 21 - 21 -序言最佳线性滤波理论起源于40年代美国科学家Wiener和前苏联科学家等人的研究工作,后人统称为维纳滤波理论。从理论上说,维纳滤波的最大缺点是必须用到无限过去的数据,不适用于实时处理。为了克服这一缺点,60年代Kalman把状态空间模型引入滤波理论,并导出了一套递推估计算法,后人称之为卡尔曼滤波理论。1960年, R.E. Kalman 出版了他的著名论文,描述了一个离散线性系统滤波问题的递归的解决方案。Kalman 滤波器是一组数学等式,它提供了一个有效的计算(回归)方法,来估计过程的状态,在这种方法中,将均方差最小化。该滤波器功能非常强大,它支持过去、现在、甚至将来状态的估计,并当模型系统的精确特性未知的情况下进行估计。卡尔曼滤波器(Kalman Filter)是一个最优化自回归数据处理算法(optimal recursive data processing algorithm)。对于解决很大部分的问题,他是最优,效率最高甚至是最有用的。他的广泛应用已经超过30年,包括机器人导航,控制,传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理,例如头脸识别,图像分割,图像边缘检测等等。目前Kalman滤波器的设计有许多现成的高级语言设计程序,但他们都存在设计效率低,不具有可视图形,不便于参数修改等缺点,而MATLAB_GUI为Kalman滤波器的研究和应用提供了一个直观、高效、便捷的利器。它以矩阵运算为基础,把计算、可视化、仿真以及设计融合到一个交互式的工作环境中。本文基于MATLAB_GUI对Kalman滤波器进行设计和仿真。第1章 Kalman滤波理论1.1 Kalman滤波理论的应用背景信号是传递和运载信息的时间或空间函数。信号有两类,即确定性信号和随即信号。确定性信号的变化规律是既定的,可以表示为一确定的时间函数或空间函数,具有确定的频谱特性,如阶跃信号、脉宽固定的矩形脉冲信号,正余弦函数等,它们对于指定的某一时刻,可确定一相应的函数值。随即信号没有既定的变化规律,不能给出确定的时间或空间函数,在相同的初始条件和环境条件下,信号每次实现都不一样,如陀螺漂移、惯性导航系统的导航误差、GPS的SA误差、海浪等,随即信号尽管没有确定的频谱特性,但是可以知道它的统计特性,即具有确定的功率谱。信号在传输与检测过程中不可避免地要受到外来干扰与设备内部噪声的影响,使接收端收到的信号具有随机性。为获取所需信号,排除干扰,就要对信号进行滤波。所谓滤波,是指从混合在一起的诸多信号中提取出所需信号的过程。信号的性质不同,获取的方法就不同,即滤波的手段不同。对于确定性信号,由于其具有确定的频谱特性,可根据各信号所处频带的不同,设置具有相应频率特性的滤波器,如低通滤波器、高通滤波器、带通滤波器及带阻滤波器等,使有用信号无衰减地通过,而干扰信号收到抑制。这类滤波器可用物理的方法实现,即模拟滤波器,亦可用计算机通过算法实现,即数字滤波器。对确定性信号的滤波处理通常称为常规滤波。随即信号具有确定的功率谱特性,可根据有用信号和干扰信号的功率谱设计滤波器。美国学者维纳(N.Wiener)等人提出了Wiener滤波,它通过做功率谱分解设计滤波器,在对信号做抑制和选通这一点同常规滤波是相似的。由于在频域进行Wiener滤波器设计需要求解维纳-霍普方程,且计算量较大,需要大量的存储空间,妨碍了Wiener滤波的应用。Kalman滤波是卡尔曼于1960年提出的从与被提取信号有关的观测量中通过算法估计出所需信号的一种滤波算法1。他把状态空间的概念引入到随即估计理论中,把信号过程视为白噪声作用下的一个线性系统的输出,用状态方程来描述这种输入-输出关心,估计过程中利用系统状态方程、观测方程和白噪声激励(系统噪声和观测噪声)的统计特性形成滤波算法,由于所用的信息都是时域内的量,所以不但可以对平稳的一维的随机过程进行估计,也可以对非平稳的、多维随机过程进行估计。这就完全避免了Wiener滤波在频域内设计时遇到的限制,适用范围比较广泛。实际上,Kalman滤波是一套由计算机实现的实时递推算法,它所处理的对象是随机信号,利用系统噪声和观测噪声的统计特性,以系统的观测量作为滤波器的输入,以所要估计值(系统的状态或参数)作为滤波器的输出,滤波器的输入与输出之间是由时间更新和观测更新算法联系在一起的,根据系统方程和观测方程估计出所有需要处理的信号。所以,此处所谈的Kalman滤波与常规滤波的涵义与方法完全不同,实质上是一种最优估计方法。1.2 卡尔曼滤波的基本思想卡尔曼滤波器是线性无偏最小均方误差递推滤波器。与维纳滤波相比,在平稳条件下,它们所得到的稳态结果是一致的。然后,它们解决的方法有很大区别。维纳滤波是根据全部过去的和当前的观察数据来估计信号的当前值,它的解是以均方误差最小条件下所得到的系统的传递函数H(z)或单位样本响应h(n)的形式给出的,因此称这种系统为最佳线性过滤器或滤波器。而卡尔曼过滤是用前一个估计值和最近一个观察数据来估计信号的当前值,是用状态方程和递推的方法进行估计的,其解是以估计形式给出。因此称这种系统为线性最有估计器或滤波器。卡尔曼滤波过程实际上是获取维纳解的递推运算过程2,卡尔曼过滤中信号和噪声是状态方程和测量方程表示的,因此设计卡尔曼滤波器要求已知状态方程和测量方程。标准卡尔曼滤波器是在最小均方误差准则下的最佳线性过滤器,就是说,它使系统的状态向量和状态向量的预测值之间的均方误差达到最小,它用状态方程和递推方法进行估计,它的解是以估计值形式给出的。由于它能够对物体的运动建立某种模型,因此在跟踪中经常被用到。当观测方程不是线性时,上述标准卡尔曼方程不再适用。总的来说,卡尔曼滤波器是一个线性的估计器,能够有效地跟踪物体的运动和形状变化,但它基于两个假设:一是背景相对干净;二是运动参数服从高斯分布,因而适用范围有限。1.3 卡尔曼滤波理论基础在这一部分,我们就来描述源于Dr Kalman的卡尔曼滤波器。下面的描述,会涉及一些基本的概念知识,包括概率(Probability),随机变量(Random Variable),高斯或正态分配(Gaussian Distribution)还有State-space Model等等。但对于卡尔曼滤波器的详细证明,这里不能一一描述。(1)在介绍卡尔曼滤波算法之前,先简要介绍白噪声的概念。所谓高斯白噪声中的高斯是指概率分布是正态函数,而白噪声是指它的二阶矩不相关,一阶矩为常数,是指先后信号在时间上的相关性。这是考查一个信号的两个不同方面的问题。高斯白噪声:如果一个噪声,它的幅度分布服从高斯分布,而它的功率谱密度又是均匀分布的,则称它为高斯白噪声。 (2)Kalman滤波器算法首先,我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程(Linear Stochastic Difference equation)来描述: (1.1)再加上系统的测量值: (1.2)上两式子中,X(k)是k时刻的系统状态,U(k)是k时刻对系统的控制量。A和B是系统参数,对于多模型系统,他们为矩阵。Z(k)是k时刻的测量值,H是测量系统的参数,对于多测量系统,H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise),他们的协方差分别是Q,R(这里我们假设他们不随系统状态变化而变化)。对于满足上面的条件(线性随机微分系统,过程和测量都是高斯白噪声),卡尔曼滤波器是最优的信息处理器。下面我们来用他们结合他们的covariances来估算系统的最优化输出。首先我们要利用系统的过程模型,来预测下一状态的系统。假设现在的系统状态是k,根据系统的模型,可以基于系统的上一状态而预测出现在状态: (1.3)式(1.3)中,X(k|k-1)是利用上一状态预测的结果,X(k-1|k-1)是上一状态最优的结果,U(k)为现在状态的控制量,如果没有控制量,它可以为0。到现在为止,我们的系统结果已经更新了,可是,对应于X(k|k-1)的covariance还没更新。我们用P表示covariance: (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个公式当中的前两个,也就是对系统的预测。现在我们有了现在状态的预测结果,然后我们再收集现在状态的测量值。结合预测值和测量值,我们可以得到现在状态(k)的最优化估算值X(k|k): (1.5)其中Kg为卡尔曼增益(Kalman Gain): (1.6)到现在为止,我们已经得到了k状态下最优的估算值X(k|k)。但是为了要使卡尔曼滤波器不断的运行下去直到系统过程结束,我们还要更新k状态下X(k|k)的covariance: (1.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 卡尔曼滤波参数的估计和调整在卡尔曼滤波的使用中,通常首先要测量噪声协方差R。可以通过离线测量实验确定。但是确定过程噪声协方差Q一般更困难,因为无法直接观察过程。当对一个相对简单的过程模型加入足够的不确定性时(通过选择合适的Q)得到的结果是可信的。无论如何,通过运行中调整Q和R可以得到较好的滤波效果。调整通常是离线的,通常借助于过程的另一个卡尔曼滤波进行系统辨识来实现。在Q和R为常数的情况下,估计误差协方差Pk 和卡尔曼增益Kk 都会迅速镇定,然后保持为常量。如果是这样的话,可以通过离线运行滤波器来预先确定这些参数。然而通常的情况是测量误差不会保持常量。例如,当观察光电跟踪器面板上的亮点时,对附近的亮点的测量误差要小于远处的。有时过程噪声Q 在滤波器运行中是动态变化的,变成了Qk,以调整符合不同的动态特点。例如在跟踪三维虚拟环境中的用户头部时,如果用户慢速移动,就要减小Qk 值,如果动态变化加快就增加Qk 值。在这类情况下,通过选择来适应用户意图的不确定性以及模型中的不确定性3。(1)测量方程协方差的确定测量方程是由测量设备可以实现的测量量来确定形式的,例如激光测距仪可以进行目标位置径向位置和角度位置的测量(极坐标测量),因此这两个量就是测量方程的输出。测量方程的协方差可以通过对激光测距仪的试验来确定。方法是利用在测距仪和目标都固定的情况下,激光测距仪对已知位置的目标进行多次测量,得到一组测量数据,然后利用MATLAB 软件求出这组数据的协方差就可以作为测量方程的协方差了。当然测距仪在测量不同距离的目标时,测量的误差可能不同,所以就有不同的协方差,可以取一个中间位置对应的协方差为仪器的协方差。或者取测量范围内最大的协方差。(2)状态方程(过程方程)的协方差确定过程方程的协方差比较难以确定,一种方法是对于比较容易观察过程变化的系统,通过经验设定协方差。另一种需要通过仿真来调整。对于给定的过程方程,先预设一个协方差,通过设定一系列标准的测量值,仿真观察卡尔曼滤波是否很快收敛,如果不收敛或者收敛慢则可以调整过程协方差来达到较好的效果。1.5 小结Kalman滤波器是一个最优化递归处理算法(optimal recursive data processing algorithm)。(1)最优(optimal)依赖于评价性能的判据。Kalman滤波器充分利用如下信息估计感兴趣变量当前取值:a.系统和测量装置的动态特性;b.系统噪声、测量误差和动态模型的不确定性的统计描述;c.感兴趣变量的初始条件的相关信息。(2)递归(recursive)是指Kalman不需要保存先前的数据,当进行新的测量时也不需要对原来数据进行处理。(3)filter(DPA)实际上是数据处理算法,只不过是计算中处理的程序,因此能处理离散时间测量样本,而不是连续时间输入。基本假设:采用线性模型是合理的;这是典型工程模型在某些主要点或轨迹是线性的,线性模型比非线性模型更简单。因此用线性模型来近似。白噪声意味着噪声值和时间不相关;白噪声指在整个频率上都有相同强度的频率特性的噪声。实际应用中将频率设为常值,带宽大大超过系统带宽的噪声称为白噪声,用高斯白噪声来模拟,可以大大简化模型。采用高斯密度函数在实践上是可行的。因为采用高斯函数在数学上容易处理。当缺少高阶统计量时,除了假定高斯密度外,没有更好的可以表示的函数形式。用一阶和二阶统计量完全可以描述高斯白噪声。第2章 MATLAB及GUI编程概述2.1 MATLAB简介作为具有科学计算、符号运算和图形处理等多种功能的强有力实现工具,近年来MATLAB这一软件已得到了业界的普遍认可,应用领域已拓展到数值计算、数据处理、统计分析、工程等各个方面,在各大公司、科研机构和高校得到广泛应用,其自身也因此得到迅速发展,功能不断扩充。MATLAB 是一种高效的工程计算语言,它在数值计算、数据处理、自动控制、图像处理、神经网络、小波分析、金融分析等方面有着广泛的应用。MATLAB 系统不但提供大量涉及各个工程领域的工具箱来简化科学计算、工程设计和分析等工作,而且提供具有自身特点的编程语言,可以轻松地实现大量数据的分析、处理及显示任务。一般而言,用户总希望将数据或设计结果用图形来表示,以使数据的特征或性能能够直观地体现出来。对于一般的高级语言程序来说,绘制图形,尤其是根据计算结果所得的不规则图形是一项较为复杂的工作,用户只有在对该语言有了较为深入的了解后才能迅速准确地绘制所需的图形,而MATLAB 面向对象的图形技术使用户可以轻松实现自身数据或处理后数据的绘制任务。使用MATLAB 提供的大量图形设计技术,用户无需了解图形实现的细节内容,有时甚至只需一条简单的指令就可以绘制非常复杂的图形。另外,用户还可以根据需要来规划MATLAB 的图形外观,使得绘图结果完全符合用户的需求。从当前的软件趋势来看,友好的图形界面已经成为应用程序的基本交互入口,为此,MATLAB 提供了对用户图形界面(GUI)的支持,使MATLAB 开发的程序可以为更多的用户所接受。2.2 MATLAB_GUI概述图形用户界面(Graphical User Interface,简称 GUI,又称图形用户接口)是指采用图形方式显示的计算机操作用户界面。与早期计算机使用的命令行界面相比,图形界面对于用户来说在视觉上更易于接受。图形用户界面(GUI)是用户与计算机程序之间的交互方式,是用户与计算机进行信息交流的方式。计算机在屏幕显示图形和文本,若有扬声器还可产生 声音。用户通过输入设备,如:键盘、鼠标、跟踪球、绘制板或麦克风,与计算机通讯。用户界面设定了如何观看和如何感知计算机、操作系统或应用程序。通常, 多是根据悦目的结构和用户界面功能的有效性来选择计算机或程序。图形用户界面或GUI是包含图形对象,如:窗口、图标、菜单和文本的用户界面。以某种方式 选择或激活这些对象,通常引起动作或发生变化。最常见的激活方法是用鼠标或其它点击设备去控制屏幕上的鼠标指针的运动。按下鼠标按钮,标志着对象的选择或 其它动作。MATALB 可以创建图形用户界面GUI ( GraphicalUser Interface) ,它是用户和计算机之间交流的工具。MATLAB 将所有GUI支持的用户控件都集成在这个环境中并提供界面外观、属性和行为响应方式的设置方法,随着版本的提高,这种能力还会不断加强。而且具有强大的绘图功能,可以轻松的获得更高质量的曲线图。第3章 Kalman滤波GUI和程序的设计3.1 Kalman滤波器的应用实例假设我们要研究的对象是一个房间的温度。根据你的经验判断,这个房间的温度是恒定的,也就是下一分钟的温度等于现在这一分钟的温度(假设我们用一分钟来做时间单位)。假设你的经验不是100%的可信,可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声(White Gaussian Noise),也就是这些偏差跟前后时间是没有关系的而且符合高斯分配(Gaussian Distribution)。另外,我们在房间里放一个温度计,但是这个温度计也不准确的,测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。于是现在对于某一分钟我们有两个有关于该房间的温度值:你根据经验的预测值(系统的预测值)和温度计的值(测量值)。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。假如我们要估算k 时刻的是实际温度值。首先你要根据k-1 时刻的温度值,来预测k 时刻的温度。因为你相信温度是恒定的,所以你会得到k 时刻的温度预测值是跟k-1 时刻一样的,假设是23 度,同时该值的高斯噪声的偏差是5 度(5 是这样得到的:如果k-1 时刻估算出的最优温度值的偏差是3,你对自己预测的不确定度是4 度,他们平方相加再开方,就是5)。然后,你从温度计那里得到了k 时刻的温度值,假设是25 度,同时该值的偏差是4 度。由于我们用于估算k 时刻的实际温度有两个温度值,分别是23 度和25 度。究竟实际温度是多少?相信自己还是相信温度计?究竟相信谁多一点,我们可以用他们的covariance 来判断。因为Kg2=52/(52+42),所以Kg=0.78,我们可以估算出k 时刻的实际温度值是:23+0.78*(25-23)=24.56 度。可以看出,因为温度计的covariance 比较小(比较相信温度计),所以估算出的最优温度值偏向温度计的值。现在我们已经得到k 时刻的最优温度值了,下一步就是要进入k+1 时刻,进行新的最优估算。到现在为止,好像还没看到什么自回归的东西出现。对了,在进入k+1 时刻之前,我们还要算出k 时刻那个最优值(24.56 度)的偏差。算法如下:(1-Kg)*52)0.5=2.35。这里的5 就是上面的k 时刻你预测的那个23度温度值的偏差,得出的2.35 就是进入k+1 时刻以后k 时刻估算出的最优温度值的偏差(对应于上面的3)。就是这样,卡尔曼滤波器就不断的把covariance 递归,从而估算出最优的温度值。他运行的很快,而且它只保留了上一时刻的covariance。上面的Kg,就是卡尔曼增益(Kalman Gain)。他可以随不同的时刻而改变他自己的值。3.2 Kalman滤波GUI设计所实现的任务Kalman滤波GUI设计所实现的任务如下:(1)能够实现人机交互。(2)当单击“运行”按钮的时候,经过Kalman滤波后的曲线出现在GUI的axes上。3.3 GUI界面设计GUI设计面板是GUI设计工具应用的平面,面板上部提供了菜单和常用工具按钮,左边提供了多种如命令按钮、单选按钮、可编辑文本框、静态文本框、弹出式菜单等。进行设计时, 首先单击面板左边所需的控件, 然后在右边的图形界面编辑区中再次单击某一恰当位置, 这时将在该位上为图形界面添加相应的控件。一个图形界面的完成, 除了设计其外观外, 还有相当的一部分是通过属性的设来完成的。因此在设置这些属性时, 要注意下面几个常用又很重要的属性设置:(1) 控件风格和外观 BackgroundColor:设置控件背景颜色,使用R G B或颜色定义。 CData:在控件上显示的真彩色图像,使用矩阵表示。 ForegroundColor:文本颜色。 String属性:控件上的文本,以及列表框和弹出菜单的选项。 Visible:控件是否可见。 (2)对象的常规信息 Enable属性: 表示此控件的使能状态,设置为on”,表示可选,为“off”时则表示不可选。 Style:控件对象类型。 Tag:控件表示(用户定义)。 TooltipString属性:提示信息显示。当鼠标指针位于此控件上时,显示提示信息。 UserData:用户指定数据。 Position:控件对象的尺寸和位置。 Units:设置控件的位置及大小的单位。 有关字体的属性,如 FontAngle, FontName等。(2) 控件回调函数的执行 BusyAction:处理回调函数的中断。有两种选项:即Cancel:取消中断事件,queue:排队(默认设置)。 ButtonDownFcn属性:按钮按下时的处理函数。 CallBack属性:是连接程序界面整个程序系统的实质性功能的纽带。该属性值应该为一个可以直接求值的字符串,在该对象被选中和改变时,系统将自动地对字符串进行求值。 CreateFcn:在对象产生过程中执行的回调函数。 DeleteFcn:删除对象过程中执行的回调函数。 Interruptible属性:指定当前的回调函数在执行时是否允许中断,去执行其他的函数。(4)控件当前状态信息 ListboxTop:在列表框中显示的最顶层的字符串的索引。 Max:最大值。 Min:最小值。 Value:控件的当前值。应用MATLAB制作这样一个过程是非常方便的, 我们可以通过GUI操作来看到。该环境下要设计一个界面友好的仿真软件,一般应完成以下两个步骤:(1)GUI界面设计。主要是通过不同的文本框、按钮等许多工具的使用,设计出一个图形用户界面。要清楚这个图形界面的功能是什么,即在图形界面上的操作会引发什么样的结果。(2)回调函数的设计。用户应根据设计好的图形界面的功能,针对各个不同的图形对象来编写出能够实现该功能的函数代码,确保这个图形界面能够完成所预定的功能。在matlab命令行窗口中输入guide或点击matlab界面上面的菜单中的图标,打开gui设计的图形界面如图3.1 。然后拖入所要的图形控件,按需要修改外观和空间属性,直至满足要求,如图3.2。设置属性点击guide界面上方的Run按钮,会生成一个fig文件,一个m文件,其中fig文件就是界面的图形,m文件是界面的回调函数,在m文件里每个控件的回调函数都已经自动生成,控件要做的工作就是在文件框架下定义某些特殊要求的状态并补充完整回调函数场, 使单击控件时激活回调程序完成一定的功能。图3.1 GUI设计的图形界面图3.2 Kalman滤波的GUI界面设计3.4 Kalman滤波程序回调函数的编写过程基于上面的例子,首先,我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程(Linear Stochastic Difference equation)来描述: (3.1)再加上系统的测量值: (3.2)上两式子中,是k 时刻的系统状态,是k 时刻对系统的控制量。A 和B 是系统参数,对于多模型系统,他们为矩阵。是k 时刻的测量值,H 是测量系统的参数,对于多测量系统,H 为矩阵。和分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise),他们的covariance 分别是Q,R(这里我们假设他们不随系统状态变化而变化)。对于满足上面的条件(线性随机微分系统,过程和测量都是高斯白噪声),卡尔曼滤波器是最优的信息处理器。下面我们来用他们结合他们的covariances 来估算系统的最优化输出(类似上一节那个温度的例子)。首先我们要利用系统的过程模型,来预测下一状态的系统。假设现在的系统状态是k,根据系统的模型,可以基于系统的上一状态而预测出现在状态: (3.3)式(3.3)中,是利用上一状态预测的结果,是上一状态最优的结果,为现在状态的控制量,如果没有控制量,它可以为0。到现在为止,我们的系统结果已经更新了,可是,对应于的covariance 还没更新。我们用P表示covariance: (3.4)式(3.4)中,是对应的covariance,是对应的covariance,表示A 的转置矩阵,Q 是系统过程的covariance。式子3.3,3.4 就是卡尔曼滤波器5 个公式当中的前两个,也就是对系统的预测。现在我们有了现在状态的预测结果,然后我们再收集现在状态的测量值。结合预测值和测量值,我们可以得到现在状态(k)的最优化估算值: (3.5)其中Kg 为卡尔曼增益(Kalman Gain): (3.6)到现在为止,我们已经得到了k 状态下最优的估算值。但是为了要另卡尔曼滤波器不断的运行下去直到系统过程结束,我们还要更新k 状态下的covariance: (3.7)其中I 为1 的矩阵,对于单模型单测量,I=1。当系统进入k+1 状态时,就是式子(3.4)的。这样,算法就可以自回归的运算下去。卡尔曼滤波器的原理基本描述了,式子3.3、3.4、3.5、3.6和3.7就是他的5个基本公式。根据这5个公式,可以很容易的实现计算机的程序。结合以上例子和描述,把房间看成一个系统,然后对这个系统建模。当然,我们见的模型不需要非常地精确。我们所知道的这个房间的温度是跟前一时刻的温度相同的,所以A=1。没有控制量,所以=0。因此得出: (3.8)式子(3.4)可以改成: (3.9)因为测量的值是温度计的,跟温度直接对应,所以 H=1。式子3.5,3.6,3.7 可以改成以下: (3.10) (3.11) (3.12)现在我们模拟一组测量值作为输入。假设房间的真实温度为 25 度,我模拟了100 个测量值,这些测量值的平均值为25 度,但是加入了标准偏差为几度的高斯白噪声。为了令卡尔曼滤波器开始工作,我们需要告诉卡尔曼两个零时刻的初始值,是 X(0|0)和P(0|0)。他们的值不用太在意,随便给一个就可以了,因为随着卡尔曼的工作,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=25+0.5*randn(1,100);p1_=2;p1=p1_;Q=0.25;R=0.25;A=1;H=1;f1_=1;f1=f1_;for n=1:100f_pri=A*f1;P_pri=A*p1*A+Q;K=P_pri*H*inv(H*P_pri*H+R); f_post=f_pri+K*(tempr_n(n)-H*f_pri);P_post=(1-K*H)*P_pri; p1=P_post;f1=f_post;f_denoise(n)=f_post;end;f_denoise(1)=f1_;grid on;plot(f_denoise,r:);grid on;第4章 在MATLAB_GUI下运行程序4.1 通过MATLAB_GUI运行结果图4.1 运行结果图4.2运行结果分析图4.1的横轴表示模拟的100个时刻,纵轴表示温度。由此可以得出,我们通过Kalman算法求出的曲线(红线)最终在25度(真实温度)上下波动,并且十分接近真实温度。我们假设初始的预测值是1度,在图4.1中可以看出,在温度轴上最优曲线从1度左右开始回归。卡尔曼滤波的实质是由量测值重构系统的状态向量。它以“预测实测修正”的顺序递推,根据系统的量测值来消除随机干扰,再现系统的状态,或根据系统的量测值从被污染的系统中恢复系统的本来面目。卡尔曼滤波由于其在求解时不需要贮存大量的观测数据,并且当得到新的观测数据时,可随时算得新的参数滤波值,便于实时地处理观测成果,因此,卡尔曼滤波被越来越多地应用于动态数据处理中。本文以 MATLAB7.6.0 为例,介绍卡尔曼滤波器的设计方法, 目的是为了熟悉卡尔曼滤波器算法及实现,用 MATLAB 进行卡尔曼滤波器仿真、对比卡尔曼滤波器的预测效果。Kalman滤波器的最优估算具有广泛的应用背景,除了在对系统状态进行估计外,也可以在一些智能控制算法中的参数调整上起到加快速度的作用。在算法的继续发展上可以做的工作还很多,这一算法理论将能在实际控制中发挥更大的作用。参考文献1 Kalman R.E.A New Approach to Linear filtering and Prodiction Theory.Trans.ASME.Journal of Basic Eng,1960,82D:35-46.2Crochiere, Rabiner. Optimum FIR Digital Filter Implementations for Decimation, Interpolation, and Narrow-band FilteringJ,IEEE Trans.Acoustics,Speech,and Signal Processing. 2005, Vol.23, No.5,pp.444-456.3 Greg Welch and Gary Bishop. An introduction to the Kalman filter. Department of Computer Science, University of North Carolina at Chapel Hill, Chapel Hill, NC 27599-3175.4 李勇.MATLAB 辅助现代工程数字信号处理M. 西安:西安电子科技大学出版.2002 年10月.
展开阅读全文