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

黑龙江工程学院本科生毕业论文0第 1 章 绪 论1.1 研究的目的自从 1960 年卡尔曼滤波提出以来，它已成为控制，信号处理与通信等领域最基本最重要的计算方法和工具之一，并已成功的应用到航空，航天，工业过程及社会经济等不同领域，比如，在雷达中，人们感兴趣的是跟踪目标，但目标的位置、速度、加速度的测量值往往在任何时候都有噪声。卡尔曼滤波利用目标的动态信息,设法去掉噪声的影响，得到一个关于目标位置的好的估计。这个估计可以是对当前目标位置的估计（滤波） ，也可以是对于将来位置估计（预测） ，也可以是对过去位置的估计（差值或平滑） 。但随着微型计算机的普及应用,对卡尔曼滤波的数值稳定性、计算效率、实用性和有效性的要求越来越高，随着微型计算机时代的来临显著地提高了科学计算的能力，滤波大量复杂的计算在计算机种只需要几分钟就能算出，为此本文将对卡尔曼滤波进行研究。1.2 研究的意义卡尔曼滤波 ( Kalman , 1960) 是当前应用最广的一种动态数据处理方法 , 它具有最小无偏方差性. 把变形体视为一个动态系统 , 将一组观测值作为系统的输出 , 可以用卡尔曼滤波模型来描述系统的状态. 动态系统由状态方程和观测方程描述 , 以监测点的位置、速率和加速率参数为状态向量 ,可构造一个典型的运动模型 . 状态方程中要加进系统的动态噪声 . 其滤波方程是一组递推计算公式 ,计算过程是一个不断预测、修正的过程 , 在求解时 , 优点是不需保留用过的观测值序列 , 并且当得到新的观测数据时 , 可随时计算新的滤波值 , 便于实时处理观测成果 , 把参数估计和预报有机地结合起来. 卡尔曼滤波特别适合变形监测数据的动态处理.1.3 研究的方法1.4 课题的主要内容本文先从现代测量误差处理理论基础开始讲解，细致的写出现代测量误差都有那些函数，并详细分析讲解这些函数，在继续讲解最小二乘与卡尔曼滤波的关系，如量测值越多，只要处理得合适，最小二乘估计的均方误差就越小。采用批处理实现的最小二乘算法，需存储所有的量测值。若量测值数量十分庞大，则计算机必须具备巨大的存储容量，这显然是不经济的。递推最小二乘估计从每次获得的最小量测值中提取出被估计量信息，用于修正上一步所得的估计。获得量测的次数越多，修正的次数也越多，估计的精读也越高。这和卡尔曼滤波原理非常相似，本文在详细讲解了卡尔曼滤波，写出其原理性质，在根据 C+进行编程，使其应用于测量领域。第 2 章 现代测量误差处理理论基础黑龙江工程学院本科生毕业论文12.1 概 述在测量、通信和控制等学科中，为了求得某些未知参数，常常要进行一系列的观测由于测量上的局限性，往往只能观测未知量的某些函数，且观测值中必然含有误差(或称为噪声)这就产生了根据含有误差的观测值求定未知参数估值的问题下面举几个例子(1)为了确定平面或三维控制网中各点的坐标，对控制网的边长和方向(或坐标差)进行了观测，当然，观测值包含有误差设各点的坐标为未知参数向量 x，而包括边长和方向的观测值向量为 L，则 L 和 x 之间有函数关系 )(XFL式中 表示误差向量通过含有误差 的观测向量 L 来求定待定点坐标的最佳估值，就是一个估计问题在测量中，就是一个平差问题(2)通信理论中的一个重要问题是从接收到的信号中，提取被发送的信号设被发送的信息调制成信号 S(t)，而接收到的信号也就是信号的观测值 L(t)，由于大气噪声和电路噪声的干扰，因此有 )()(tnStL其中 n(t)是噪声，t 表示时间通信中的主要问题就是从 L(t)中将有用的信号 S(t)分离出来，也就是由 L(t)求定 S(t)的最佳估值信号 S(t)也是一种未知参数(3)生产过程的自动化可以达到高效率和高精度 在实现生产过程的控制中，需要通过对生产系统进行状态的不断测量，得到与系统运行状态有关的观测值；然后对观测值进行分析处理得到控制信号，实时地控制生产系统按要求运行但由于观测值中存在误差，所以，为了得到控制信号，就要求由观测值来估计系统的运行状态(4)卫星(或其他运动体)的轨道往往可以由如下微分方程确定 ()(),()XtftUt式中 f 表示时间；x(t)表示卫星的轨道参数，在此处称为状态向量；U(t)为控制向量；力(t)是随机的状态噪声为了精确估计或预测卫星的轨道，就需要对卫星进行观测，从而得到大量的观测数据 L(t)，然后实时地由含有误差的观测值 L(t)来估计卫星的轨道，即估计卫星的轨道参数以上例中所述的信号或状态都可以说是一种未知参数在测量平差中，通常称非随机的未知参数向量为参数，而称随机参数向量为信号，而称随时间 t 变化的动态系统中的未知参数向量为状态向量，或筒称为状态可以看到，在上面的例子中，都存在一个对未知参数进行估计的问题一般说来，若设 x 为 t 阶未知参数向量(简称为参数)，L 为 n 阶观测向量(或称观测值)，表示 n 维误差(或噪声)向量那么，所谓估计问题，就是根据含有误差 的观测值 L，构造一 个函数 ，使 成为未知参数向量 X 的最佳估计量，其具体数值称为最佳估值 (以后一般XL不区分其含义)通常将 简记为 ，并记()X ()XL称 ；为 的估计误差X()L黑龙江工程学院本科生毕业论文2可以看到，当；的数学期望等于零时， ；的方差就等于 ；而当 X 为非随机X()XTE量时，未知参数的估值工的方差 ；也就等于其误差方差 在估计理论中，通常是用估XD)D计量 的误差方差 来衡量其精度的但在经典的最小二乘平差中，由于 X 一般都是非随X()X机参数，所以习惯上都用估值(平差值)的方差衡量精度在根据观测值 L 求未知参数 x 的估值 时，总是希望所得到的估值是最优的由估计理()L论知道，最优估计量主要应具有以下几个性质：(1)一致性由观测值得到的估值 通常与其真值是不同的，我们希望当观测值个数 n 增()X加时，估计量变得更好些；当 n 无限增大时，估计量向被估计的参数趋近的概率等于 1即如果对于任意 ，有0(1-1-1)lim()1nPX则称估计量 具有一致性；若有X(1-1-2)li()()0Tn则称此估计量是均方一致的估计量的一致性是从它的极限性质来看的(2)无偏性若估计量 的数学期望等于被估计量 x 的数学期望，即(1-1-3)(XE如果丑是非随机量，上式即为(1-1-4)()则称丘为无偏估计量如果 ，则称 为渐近无偏()n(3)有效性若由观测向量 L 得到无偏估计量 的误差方差 ，小于由X)()(TXEL 得到的任何其他无偏估计量 的误差方差 ，即*X*)(TE)()(TE或写为 (1-1-5)*()XD则称 是有效估计量，也称 具有有效性或方差最小性X以不同的准则来求定未知参数的最佳估值，可得到不同的估计方法估计方法主要有极大似然估计，最小二乘估计，极大验后估计，最小方差估计和线性最小方差估计等；经典的测量平差法都是以最小二乘估计或极大似然估计为根据导出的；而滤波、配置和动态系统的卡尔曼滤波等，最初是以极大验后估计或最小方差估计为根据导出的因此，概率统计中的估计理论是广义测量黑龙江工程学院本科生毕业论文3平差的理论基础2.2 多维正态分布正态分布是测量平差理论中最常用的误差分布，是最小二乘平差误差理论的基础本节在已学过的一元正态分布的基础上，对多维正态分布做全面阐述广义测量平差理论中还涉及其他分布，则将分别在相应章节中一一介绍2.2.1 多维正态分布的定义和性质已知随机变量 X 的正态分布概率密度为(1-2-1)221()exp()2Xfx式中两个参数 和 分别为随机变量 X 的数学期望和方差当 =0， =1 时，X 为标X2 2准正态分布变量记为 ，其概率密度为(0,1)N(1-2-2)21exp2f设有 m 个互相独立的标准正态随机变量构成的随机向量 它们12TmZZ的有限个线性函数 11220nnmnXAZ为 n 维正态随机向量此时，X 的数学期望和方差阵为 ()TXEDAX 的分布函数和概率密度都简称为 n 维(或 n 元)正态分布，简记为 ，或写为(,)TnXNA(,)nXND由互相独立的标准正态随机变量组成的随机向量 Z，可写为 阶单位阵(0,).nE为多维正态分布具有以下性质：(1)正态随机向量的线性函数还是正态的.例如,设 则(,)TnXNAYBXb(,)TYNBb(2)设 ，记(,)TnXNA黑龙江工程学院本科生毕业论文4111222,TXXDA则 112(,)(,).rnrrN2.2.2 多维正态分布设有 n 维正态随机向量 XN。(p。 ，Dx)，其中方差阵 D，为可逆阵，即 det(Dx)0，则它的概率密度为 112()exp()()2TXXXfx x 式中 表示 的行列式 XDX对于二维正态随机向量 ，若它有可逆方差阵和数学期望为TY2XXY和则由(1-2-3)式可得其概率密度为 221(,)XYfxyA222()()()epYXYXYXxxyy 因相关系数 ，所以上式可写为XY2 222()()()11(,)exp()2XXYYXYXYXxxyfxy (1-2-4)这就是二维正态随机向量概率密度当 时，即当 X 和 Y 是互不相关的两个正态随机变量时，则有0=XYXY或22()()1(,)exp2XYXYyfxy2 2()()1expXYXYyA()xyf黑龙江工程学院本科生毕业论文5这就是说，当 时，x 和 y 是互相独立的所以，对于正态分布来说，随机变量的“互0XY不相关”与“互相独立”是等价的根据(1-2-4)式绘制二维正态曲面(密度曲面)如图 l-1 所示曲面在点 处取得(,)XY最大值如果用平行于 XOY 面的平面 (常数)截此曲面，即得到一族椭圆，椭圆上所0Z有点的概率密度值均相等，因此，称这些椭圆为等密度椭圆2.2.3 正态随机向量的条件概率密度设有 维正态随机向量 X，且设nt111222,XXD和 分别是由 X 的前 n 个分量和后 t 个分量构成的正态随机变量，即 ，1X2 11(,)nXND的概率密度是(,)tND图 1-1（1-2-6）111222()expTnXXxfxDD 按分块矩阵求逆公式，有（1-2-7）11122X 或为（1-2-8）111 21222XDD 其中（1-2-9）1122可将（1-2-7）和（1-2-8）两式分别写为黑龙江工程学院本科生毕业论文6（1-2-10）1 1111220X ttDDEE （1-2-11）11121 12 20nXn因 还可分解为XD（1-2-12）11 12212 200XDEDE所以， 的行列式之值为X（1-2-13）121X利用(1-2-10)、(1-2-9)式和(1-2-13)式，可将概率密度(1-2-6)式改写为12(),)fx111exp()()2n TDDx AA（1-2-14）1 12222()()()T 或 12(),)fx1 12222exp()()TDDx AA（1-2-15）1121()()()mT 其中 （1-2-16）112221()Dx根据边际概率密度和多维正态分布的性质可知（1-2-17 ）1121 1()exp()()n Tfx x A（1-2-18 ）1 122 222()()()TfDD 黑龙江工程学院本科生毕业论文7又由条件概率密度公式知（1-2-19）1221(,)()fxf（1-2-20）1212(,)()ffx而将(1-2-14)和(1-2-17)两式代人(1-2-19)式，得（1-2-21）1 1221 222()(ep()()TfxDD 而将(1-2-15)和(1-2-18)两式代人(1-2-20)式，即得（1-2-22）1 1212 1()(ex()()2mTfx x 显然，上两式仍然是正态概率密度，根据条件期望和条件方差的定义和正态概率密度的性质可得（1-2-23）1121221()()EXDxx（1-2-24）12112()D因此， （1-2-21）和（1-2-22 ）式又可写为11 12221 222112121()()exp()()()TmfxXEXxDxEXxD （1-2-25）正态分布的条件期望具有以下性质：(1)由(1-2-23)式可知， 是 的线性组合，所以 ,它是正态随机向量；当然12()EXx也是正态随机向量21()EXx(2)设 X 和 Y，为正态随机向量，且设（1-2-26）()EXyZAY则 是与 Z 互相独立的随机向量这是因为1()XYYDX由协方差传播律可得黑龙江工程学院本科生毕业论文810(,)XYXYTDDXZEA1TTA(3)设 ， 而(,)XN112212(,)(,)covN， ， 且 =0，1 200XYYD(,)=D则 有(1-2-27)1212()(,)()()XEyXyEy证 因为 112()(,)()XYYD12 1120XYy所以 1 2112 12(,)()()XYXYXEyDyDy2()E(4)设 (,),XYN且 121122,YXTY YDD令 221(y),YE则 有 212)(,)XEy(1-2-28)(X证 因为 122()YDY11 122ED所以黑龙江工程学院本科生毕业论文9(1-2-29)12122111 1222 222122()0,()(,)(,)YXYXEYDDY DXE 利用分块求逆公式和(1-2-29)式得 112(,)()XYYEyD121212XD121 11122120XYX YDEE A A12 111122 2()(XYXY