卡尔曼滤波对测量数据处理的程序实现方法

1、黑龙江工程学院本科生毕业论文第1章绪论1.1 研究的目的自从 1960 年卡尔曼滤波提出以来，它已成为控制，信号处理与通信等领域最基本最重要的计算方法和工具之一，并已成功的应用到航空，航天，工业过程及社会经济等不同领域，比如，在雷达中，人们感兴趣的是跟踪目标，但目标的位置、速度、加速度的测量值往往在任何时候都有噪声。卡尔曼滤波利用目标的动态信息, 设法去掉噪声的影响，得到一个关于目标位置的好的估计。这个估计可以是对当前目标位置的估计（滤波） ，也可以是对于将来位置估计（预测） ，也可以是对过去位置的估计（差值或平滑） 。但随着微型计算机的普及应用 , 对卡尔曼滤波的数值稳定性、计算效率、实用性

2、和有效性的要求越来越高，随着微型计算机时代的来临显著地提高了科学计算的能力，滤波大量复杂的计算在计算机种只需要几分钟就能算出，为此本文将对卡尔曼滤波进行研究。1.2 研究的意义卡尔曼滤波( Kalman , 1960)是当前应用最广的一种动态数据处理方法,它具有最小无偏方差性 .把变形体视为一个动态系统,将一组观测值作为系统的输出,可以用卡尔曼滤波模型来描述系统的状态.动态系统由状态方程和观测方程描述,以监测点的位置、速率和加速率参数为状态向量, 可构造一个典型的运动模型.状态方程中要加进系统的动态噪声.其滤波方程是一组递推计算公式, 计算过程是一个不断预测、修正的过程,在求解时,优点是不需保

3、留用过的观测值序列,并且当得到新的观测数据时,可随时计算新的滤波值,便于实时处理观测成果,把参数估计和预报有机地结合起来.卡尔曼滤波特别适合变形监测数据的动态处理.1.3 研究的方法1.4 课题的主要内容本文先从现代测量误差处理理论基础开始讲解，细致的写出现代测量误差都有那些函数，并详细分析讲解这些函数，在继续讲解最小二乘与卡尔曼滤波的关系，如量测值越多，只要处理得合适，最小二乘估计的均方误差就越小。采用批处理实现的最小二乘算法，需存储所有的量测值。若量测值数量十分庞大，则计算机必须具备巨大的存储容量，这显然是不经济的。递推最小二乘估计从每次获得的最小量测值中提取出被估计量信息，用于修正上一步

4、所得的估计。获得量测的次数越多，修正的次数也越多，估计的精读也越高。这和卡尔曼滤波原理非常相似，本文在详细讲解了卡尔曼滤波，写出其原理性质，在根据C+ 进行编程，使其应用于测量领域。第 2 章现代测量误差处理理论基础1黑龙江工程学院本科生毕业论文2.1概述在测量、通信和控制等学科中，为了求得某些未知参数，常常要进行一系列的观测由于测量上的局限性，往往只能观测未知量的某些函数，且观测值中必然含有误差( 或称为噪声 ) 这就产生了根据含有误差的观测值求定未知参数估值的问题下面举几个例子(1)为了确定平面或三维控制网中各点的坐标，对控制网的边长和方向( 或坐标差 ) 进行了观测，当然，观测值包含有误

5、差设各点的坐标为未知参数向量x，而包括边长和方向的观测值向量为 L，则 L 和 x 之间有函数关系LF(X)式中表示误差向量通过含有误差的观测向量L 来求定待定点坐标的最佳估值，就是一个估计问题在测量中，就是一个平差问题(2) 通信理论中的一个重要问题是从接收到的信号中，提取被发送的信号设被发送的信息调制成信号 S(t) ，而接收到的信号也就是信号的观测值L(t)，由于大气噪声和电路噪声的干扰，因此有L(t )S(t )n(t )其中 n(t)是噪声， t 表示时间通信中的主要问题就是从L(t)中将有用的信号S(t) 分离出来，也就是由 L(t) 求定 S(t) 的最佳估值信号S(t) 也是一

6、种未知参数(3)生产过程的自动化可以达到高效率和高精度在实现生产过程的控制中，需要通过对生产系统进行状态的不断测量，得到与系统运行状态有关的观测值；然后对观测值进行分析处理得到控制信号，实时地控制生产系统按要求运行但由于观测值中存在误差，所以，为了得到控制信号，就要求由观测值来估计系统的运行状态(4) 卫星 ( 或其他运动体 ) 的轨道往往可以由如下微分方程确定&f ( X (t ),U (t ),(t)X (t )式中 f 表示时间； x(t) 表示卫星的轨道参数，在此处称为状态向量；U(t) 为控制向量；力 (t)是随机的状态噪声为了精确估计或预测卫星的轨道，就需要对卫星进行观测，

7、从而得到大量的观测数据 L(t) ，然后实时地由含有误差的观测值L(t) 来估计卫星的轨道，即估计卫星的轨道参数以上例中所述的信号或状态都可以说是一种未知参数在测量平差中，通常称非随机的未知参数向量为参数，而称随机参数向量为信号，而称随时间t 变化的动态系统中的未知参数向量为状态向量，或筒称为状态可以看到，在上面的例子中，都存在一个对未知参数进行估计的问题一般说来， 若设 x 为 t 阶未知参数向量 ( 简称为参数 ) ，L 为 n 阶观测向量 ( 或称观测值 ) ，表示 n 维误差 ( 或噪声 ) 向量那么， 所谓估计问题， 就是根据含有误差的观测值 L，构造一个函数?的最佳估计量，其具体数

8、值称为最佳估值( 以后一般不区分X (L) ，使 X (L) 成为未知参数向量 X?，并记其含义 ) 通常将 X ( L) 简记为 X?X?X(L) XXX称? ；为 X (L ) 的估计误差X可以看到，当；的数学期望等于零时，? ；的方差就等于 E( ?T? ) ；而当 X 为非随机量XXX2黑龙江工程学院本科生毕业论文时，未知参数的估值工的方差D ? ；也就等于其误差方差D ( ? ) 在估计理论中，通常是用估计XX?的误差方差 D ( X? ) 来衡量其精度的 但在经典的最小二乘平差中，由于 X 一般都是非随机参量 X数，所以习惯上都用估值 ( 平差值 ) 的方差衡量精度在根据观测值 L

9、 求未知参数?由估计理论x 的估值 X ( L) 时，总是希望所得到的估值是最优的知道，最优估计量主要应具有以下几个性质：(1)一致性由观测值得到的估值?n 增X ( L) 通常与其真值是不同的，我们希望当观测值个数加时，估计量变得更好些；当n 无限增大时，估计量向被估计的参数趋近的概率等于1即如果对于任意0 ，有lim P( X?X) 1(1-1-1)Xn则称估计量?X 具有一致性；若有lim( X?T) 0(1-1-2)X)(XX )n则称此估计量是均方一致的估计量的一致性是从它的极限性质来看的(2)?x 的数学期望，即无偏性若估计量 X 的数学期望等于被估计量?E(X)(1-1-3)E(

10、X)如果丑是非随机量，上式即为?X(1-1-4)E(X )则称丘为无偏估计量如果E(?X (n) ，则称?为渐近无偏X )X(3)有效性 若由观测向量L 得到无偏估计量? T) ，小于由 LX 的误差方差 E( XX)(XX )得到的任何其他无偏估计量X *的误差方差 E( X X * )( XX*)T) ，即? T) <* TE(X X)(X X)E(X X)(X X)或写为D(? )D(X * )(1-1-5)X则称?具有有效性或方差最小性X 是有效估计量，也称X以不同的准则来求定未知参数的最佳估值，可得到不同的估计方法估计方法主要有极大似然估计，最小二乘估计，极大验后估计，最小方差

11、估计和线性最小方差估计等；经典的测量平差法都是以最小二乘估计或极大似然估计为根据导出的；而滤波、配置和动态系统的卡尔曼滤波等，最初是以极大验后估计或最小方差估计为根据导出的因此，概率统计中的估计理论是广义测量平差的理论基础3黑龙江工程学院本科生毕业论文2.2 多维正态分布正态分布是测量平差理论中最常用的误差分布，是最小二乘平差误差理论的基础本节在已学过的一元正态分布的基础上，对多维正态分布做全面阐述广义测量平差理论中还涉及其他分布，则将分别在相应章节中一一介绍2.2.1 多维正态分布的定义和性质已知随机变量X的正态分布概率密度为f ( x)1exp12 ( x X ) 2(1-2-1)22式中

12、两个参数 X 和2 分别为随机变量 X 的数学期望和方差当X =0，2=1 时，X为标准正态分布变量记为X N (0,1)，其概率密度为f ( x)1exp1 x2(1-2-2)22设有 m个互相独立的标准正态随机变量构成的随机向量ZZ1 Z2LTZm 它们的有限个线性函数X1Z1XX 2AZ2A0MMn 1n mn 1X nZm为 n 维正态随机向量此时， X 的数学期望和方差阵为E(X )D XAATX 的分布函数和概率密度都简称为n维 ( 或 n元 ) 正态分布，简记为X N n (, AAT ) ，或写为X N n ( , D X ) 由互相独立的标准正态随机变量组成的随机向量Z，可写

13、为 Z N (0, En ).En为 n 阶单位阵多维正态分布具有以下性质：(1) 正态随机向量的线性函数还是正态的. 例如 , 设 X N n (,AAT),YBXb 则YN(Bb, BAAT BT )(2) 设 X N n ( , AAT ), ，记4黑龙江工程学院本科生毕业论文XX1 ,1 ,DXAATD11D12X 22D21D22则X1 N r ( 1,D11), X 2 N (2,D22).r 1nr2.2.2 多维正态分布设有 n 维正态随机向量X N。 (p 。， Dx) ，其中方差阵D，为可逆阵，即det(Dx) 0，则它的概率密度为2()2DX11 ( xX)TDX1 (

14、xf (x)2 expX )2式中 DX 表示 DX 的行列式对于二维正态随机向量XY T ，若它有可逆方差阵和数学期望为2XXXY和2YXYY则由 (1-2-3) 式可得其概率密度为f (x, y)1g2222XYXY( xX )222( xX )( yY ) XY( yY )22expYX2222XYXY因相关系数XYXY，所以上式可写为XYf ( x, y)11( xX )2( xX )( yY ) ( y Y )22exp22222X Y 12(1XY )XX YYXY(1-2-4)这就是二维正态随机向量概率密度当XY0或XY =0 时，即当 X 和 Y 是互不相关的两个正态随机变量时

15、，则有f (x, y)1exp2X Y1exp2Xf x ( x) fy ( y)( xX ) 2( yY )22X22 Y2( xX )21( yY )22X2g2exp2 Y2Y5黑龙江工程学院本科生毕业论文这就是说，当XY0 时， x 和 y 是互相独立的所以，对于正态分布来说，随机变量的“互不相关”与“互相独立”是等价的根据 (1-2-4)式绘制二维正态曲面( 密度曲面 ) 如图 l-1所示曲面在点 (X ,Y ) 处取得最大值如果用平行于XOY面的平面 ZZ0 ( 常数 ) 截此曲面，即得到一族椭圆，椭圆上所有点的概率密度值均相等，因此，称这些椭圆为等密度椭圆2.2.3 正态随机向量

16、的条件概率密度设有 n t 维正态随机向量X，且设XX1, X1, D XD11D12X 22D21D22X1和 X2 分别是由X 的前 n 个分量和后 t个分量构成的正态随机变量，即X1 N n ( 1 , D11 ) ，X 2 N t (2 , D22 ) 的概率密度是图 1-1n111x1T1 x111f ( x) (2 ) 2DX2 expDX2x2x222按分块矩阵求逆公式，有11%111%11D11D11 D12 D22D21D11D11D12D22DX%11%1D22D21D11D22（ 1-2-6）（ 1-2-7 ）或为1%1%11D11%1D11D12D22（1-2-8 ）D

17、X111%11D22 D21D11D22D22 D21D11D12D22其中%D111D21D11D12 D22（1-2-9 ）%D221D12D22D21D11可将（ 1-2-7 ）和（ 1-2-8 ）两式分别写为6黑龙江工程学院本科生毕业论文1D111 D12%11EtD1110DXEtD22D21D11001En%11D2100DXD221D21D11EnD220D221因 DX 还可分解为D110ED1D%DE011D11DX%1212D210E0 D221D2ED22D22所以， D X 的行列式之值为D XD11%D22%D22D11利用 (1-2-10)、 (1-2-9)式和 (

18、1-2-13)式，可将概率密度(1-2-6) 式改写为f (x)f (x1, x2 )n11T122 gg(2)D11( x11 ) D11 (x11 )exp2111T %12 %2%(2)D22gexp( x22 )D22 (x22 )2或f (x)f (x1, x2 )111T122 gg(2 )D22( x22 )D22 ( x22 )exp2m%11%T%221(2)D11g( x11 )D11 ( x11)exp2%112 )其中1D12D22 ( x2%1%22D21D11(x11 )根据边际概率密度和多维正态分布的性质可知n11T122 gf1 ( x1 )(2)D11( x1

19、1)D11 ( x11 )exp2111 (x22)T D221( x22 D22f 2 ( x2 )(2)2 gexp2 )2（ 1-2-10）（ 1-2-11）（ 1-2-12）（ 1-2-13 ）（ 1-2-14）（ 1-2-15）（ 1-2-16 ）（ 1-2-17 ）（ 1-2-18 ）7黑龙江工程学院本科生毕业论文又由条件概率密度公式知f ( x2x1)f ( x1 , x2 )f1 ( x1)f ( x1x2 )f ( x1 , x2 )f2 ( x2 )而将 (1-2-14)和 (1-2-17)两式代人 (1-2-19)式，得1%11T%122%f ( x2 x1)(2)D22

20、exp( x2D 22 ( x22 )2而将 (1-2-15)和 (1-2-18)两式代人 (1-2-20)式，即得m11T %1f (x1 x2 )(2)2 %2exp%1)( x1D11(x1D112%)2%1)（ 1-2-19 ）（ 1-2-20 ）（ 1-2-21 ）（ 1-2-22）显然，上两式仍然是正态概率密度，根据条件期望和条件方差的定义和正态概率密度的性质可得E( X1x)%1D12D 1(x22)2122（1-2-23 ）E( X2 x1 )%11 )22D21D11 ( x1%D111D21D ( X1 x2 ) D11D12D22（ 1-2-24 ）%D221D ( X

21、2 x1 ) D22D21D11D12因此，（ 1-2-21 ）和（ 1-2-22 ）式又可写为111T1( X2f ( x2x1 )(2) 2D(X2x1)2expx2E(X 2x1)Dx1 ) x2E(X 2x1)2m11T1( X1f ( x1x2 )(2) 2D(X1x2 )2expx1E( X1x2 )Dx2 ) x1E( X1x2 )2（ 1-2-25）正态分布的条件期望具有以下性质：(1) 由 (1-2-23) 式可知， E( X1 x2 ) 是 x2 的线性组合， 所以 , 它是正态随机向量； 当然 E( X 2 x1 )也是正态随机向量(2)设 X和 Y，为正态随机向量，且设

22、%E( X y)X XZ（ 1-2-26）AY%则 X 是与 Z 互相独立的随机向量这是因为%XXXDXY DY1(YY )XDXY DY1YDXY DY1 Y由协方差传播律可得8黑龙江工程学院本科生毕业论文%E1DXDXY0D( X ,Z)DXY DYDYXDYATDXY ATDXY DY1DY AT0(3)设X N( X,DX)，Y1N(1,D1)，Y2 N(2 , D 2 )，且 cov(Y1,Y 2 )=0 ， 而D(X,Y1 )=D XY10, D(X,Y2 )=D XY20, 则有E (Xy)E( Xy1 , y2 )E( Xy1 )E (Xy2 )X(1-2-27)证因为E( X

23、 y) E( X y1 , y2 )XD XY DY 1( yY )XDXY1DXY2D110y1101y2D22所以E( X y1 , y2 )XD XY D1 1( y1 1 )D XY D2 1( y22 )XX12E( X y1) E(X y2 )X(4) 设 X N(X,DX),Y N( Y,DY),且1, DYD11D12, DXYDY1XYD21D 22D XYT2DY2 X%Y2E(Y2 y1), 则有令 Y2E( Xy , y )E( X y , y%)1212E ( X y )E( Xy%)(1-2-28)12X证因为%Y221(Y11 )Y2D 21D11D21D111E

24、Y12 D21D1111Y2所以9黑龙江工程学院本科生毕业论文%0,E(Y2)%1ED11D12D111D121D12%D(Y2)D 21D11D 21D22ED22 D21D11D22%D(Y2 ,Y1 ) 0%1EDY1 XDY X1DY X(1-2-29)D(Y2,X )D21D11DY2 XD21D1121利用分块求逆公式和 (1-2-29)式得E( X y1 , y2 )XDXY DY1(YY )D XYDXYD11D12Y11XD21D22Y2122D1D%11D 10Y11D XYD XY1112D11 D21E11gXEgD2200Y2122(1D12DXY2 )D1%1EDX

25、Y110Y11XDXY1 D11g(Y2 ) gD11D21D11Y22%1 %11) Y21(Y11 )XD( X ,Y2)D(Y2 ) D21D11 (Y12D XY1 D11XX%XE( X y1) E( X y2 )2.2.4 矩阵反演公式由于正定矩阵的逆阵唯一，故由（1-2-7）、（ 1-2-8 ）两式直接可得：( D11D12 D221 D21 ) 1D111D111D12 (D22D 21D111D12 ) 1 D21 D111(1-2-30)D111D12 (D22 D21D111 D12 ) 1( D11D12 D221D 21 ) 1 D12 D221(1-2-31)由此可

26、知， 对于任意矩阵A、B 和任意可逆阵C、D，只要在下式中它们可以相乘，就有上两式关系，一般形式为( DABC) 1D 1A(C 1BD 1A) 1BD 1(1-2-32)CB (DABC) 1(C 1BD 1A) 1BD 1(1-2-33)通常称 (1-2-32) 、 (1-2-33) 两式为矩阵反演公式，是两个非常重要的关系式，在测量平差推导公式时常要用到矩阵反演公式也可直接证明令H(DABC ) 1，则有( D ABC) H E, 或DH+ACBH=E(1-2-34)，10黑龙江工程学院本科生毕业论文HD 1D 1ACBH将上式左乘B，得BD 1(C 1BD 1A)CBH ，或(C 1B

27、D 1A) BD 1CBH此即 (1-2-33)式，代入 (1-2-34)式，即得 (1-2-32)式2.3 极大似然估计设有参数向量 X ，它可以是未知的非随机量，也可以是随机向量，为了估计X，进行了 n 次t 1观测得到了观测向量L 的观测值l ，又假定对 X 的所有可能取值为x ，在 X=x 的条件下得到的n 1n 1观测向量 L 的条件概率密度为f (lx) 容易理解， f (lx) 是 x 和 l 的函数， 但对具体的观测值l 来说，f (l x)可以认为只是 x 的函数因此，如果xf (l x)f (l x)中的最大?是 x 中的一个，而? 是值，那么，?是 x 的准确值的可能性最大此时把?( L)?X 叫做 X 的极大似然估值，并记作X或XxMLML这就是说，极大似然估计是以f (lx)max(1-3-1)为准则求最佳估值 x 的方法显然，它满足于f (l x)0(1-3-2)x?xXML (L)由于对数是单调增加函数，因此ln f (lx) 与 f (lx)