第七章滤波、推估及配置,kalman滤波

1、 第七章最小二乘滤波、推估与配置序贯平差、kalman滤波 一、滤波、推估及配置概述 经典平差经典平差-将参数定性为非随机变量，不考虑其随机性。 滤波滤波-参数考虑为正态随机量，有先验统计性质，且在估计这些参数时要考虑先验统计性质。滤波得到的参数称为信号。 信号分为两种：信号分为两种：1 1、滤波信号、滤波信号S S-已测点信号，与观测向量L建立了函数模型；2 2、推估信号、推估信号SS-未测点信号，与观测向量未建立函数模型。S与S统计相关。 当平差模型中同时存在随机参数（信号）随机参数（信号）和非随机参数（倾向参非随机参数（倾向参数）数），且在估计这些参数时要考虑随机参数的先验统计信息时，就

2、是最小二乘滤波、推估及配置滤波、推估及配置模型，配置也称为拟合推估拟合推估。范围外，称外推或预报在当；范围内，称内插或平滑在对静态系统而言，当的过程称为推估；值的过程称为滤波；求估求估值向量。正态随机参数观测向量和观测误差。、滤波的函数模型：SSSSSSYLSSAYAL01滤波模型对设计矩阵B是否满秩，不作要求。将将G-MG-M模型推广，可得最小二乘配置的函数模型：模型推广，可得最小二乘配置的函数模型：。号意义与滤波模型相同为随机参数。其余各符为非随机参数，YXSSAXBYAXBL01SALSXBYA0001，就成为，就成为项等于，就成为项等于上式中，当间接平差模型；间接平差模型；滤波与推估模

3、型；滤波与推估模型；滤波模型。滤波模型。虚拟观测值虚拟观测值-当未知参数当未知参数Y Y是正态随机向量时，可以将它的先验是正态随机向量时，可以将它的先验期望期望 Y Y当作虚拟观测值，虚拟观测值的当作虚拟观测值，虚拟观测值的方差方差为为Y Y 的先验方差的先验方差DDY Y，再按广义最小二乘原理（或极大验后估计原理，二者计算公式再按广义最小二乘原理（或极大验后估计原理，二者计算公式一致）求参数一致）求参数Y Y的估值。的估值。SSS二、最小二乘滤波与推估SSAYAL01滤波的函数模型： 相互独立。、与随机参数，表示观测噪声，若协方差：的先验期望和方差：）随机模型：（设的先验统计信息构成的和SS

4、DDDSDSDDDDPDYDYEYPDDEYSSSSSSSSSSYYSS00,cov,cov,011120 SSYYYSSSSSSYYSSYYYYSSLYVLDDDDDPYELPDPYE111对应的误差方程为，其权为，令：，方差为为作为虚拟观测值，其权将虚拟观测值的误差方程：最小二乘滤波的总误差方程：。，则：的权阵。约定：观测值，权阵：或写成：120100DPLPPPLLYIAVVLYVLYAVYYYYY广义最小二乘原理：为对应的权阵。及测值的改正数，分别为观测值及虚拟观及YYYYTYTPPVVVPVVPVmin SSTSTSSSSSSSTSTSSSSSTSTSSSYTYSTSTSSSSTST

5、SSYTYYTYTDADADAADDDDADADAADDDDADADAADDDDADADALDADAADSALDADAADSAADADALDLDADADAY11111111111111111111111111111111111112001协方差阵：解之得各参数的方差及再由法方程得知，解上式得：，并考虑到将矩阵反演公式用于，得法方程：并考虑入滤波的总误差方程，将广义最小二乘原理代 的解决方法。测量平差时，可参考教材广义之间存在线性关系，即与推估信号参数当滤波信号参数方法；的解决平差可参考教材广义测量时而之间有函数关系时，即当滤波信号参数协方差。的值及其它们的方差和及同理，解之可得变为时，广义最小

6、二乘原理，当的情况下成立。，中公式是假设在上一页45P444P,3min002001012211SSSSHHSSSSSSSSVVDDDDVVDDDDPPTYYYYTYTSSSS滤波的单位权方差估值：nVPVVPVDDSSTSTSS2000时，有，当最小二乘滤波的应用举例：两个相邻控制网中的公共点有两套坐标时，可利用滤波的原理，使两个控制网的所有坐标统一在一个最佳坐标状态。.00000000000000000000IBXBXLDXXXXDDDXXXXDDDXXXXXXXXXXXXXXX其中。其观测方程为的新观测值，方差是是对于甲网，。对应的方差和协方差阵、分别是和、参数估值，是非公共点的点的参数

7、估值，是甲网单独平差时公共乙网中，设。对应的方差和协方差阵、分别是和、参数估值，是非公共点的点的参数估值，是甲网单独平差时公共甲网中，设似。滤波公式与上面内容相是单位阵。其中。其观测方程为的新观测值，方差是是对于乙网，及精度：得甲网中滤波后的参数接上页。根据滤波公式IBXBXLDXXDDDDDDDDDDDDDDDDDDXXDDXXXXDDDXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX000111010001000000000000000000000000000计算结果，将两个控制网连接成一个控制网，使两网坐标统一起来。三、最小二乘配置。随机模型与滤波的相同的先验统计

8、信息构成的和YSSYAAYXLYAXBL,01为随机参数。数）；为非随机参数（倾向参为观测向量；配置的函数模型：1.最小二乘配置的误差方程 SSYYYSSSSSSYYSSSSSSLYVLDDDDDPVVYESSYAALYAXBV,011对应的误差方程为。与，其权为和其相应的改正数为作为虚拟观测值，理，将，按照广义最小二乘原式中：误差方程：同。的计算公式与滤波时相与可见式中”时的参数估计公式：，阵反演公式，得到“根据矩阵分块求逆和矩平差问题。题已转化成一般的间接此时，最小二乘配置问阵为上面误差方程的协方差将上式写为SSDADAMAXBLMADSAXBLMADSALMBMBBXDDDDDDDDDD

9、DDLSSXIIABVVVLYVLYAXBVTSSTSSSSTSSSTTSSSSSSSSSSSSSSSSYY,00000001111111111 最小二乘配置的总误差方程为的个数。为非随机参数估值为时，验后单位权方差的当其中方差及协方差阵：最小二乘配置各参数的XttnVPVVPVDDDADAMMBDADDMBDADDDAMBBDIMADDDDAMBBDIMADDDDAMBBDIMADDDMBBDxxSSTSTSSTSXTSSXSXTSXSSSTXTSSSSSSSTXTSSSSSTXTSSSTX0, 0,20111111111111四、序贯平差静态卡尔曼滤波特点：若对同一个控制网进行了分期重复观

10、测，只需将上一次平差的结果作一些修改，就可得到联合平差的效果。是一种递推的间接平差法。（也称为静态逐次滤波）未知参数不改变的情况矩阵反演公式：（要求P、R两矩阵有逆阵存在）1221122122222112111111.DQPLXBVDQPLXBVDQPLXBVkkkkkHPHPHRPHPHRHPTTT1111解法 1:1111121122211222111222211222211122211111111111111)()(:,.,)()()()(:,)(;:,kkTkkXkXkkTkkkXkXkkkTXTTXTXXTTXXTXTXBPBPQLPBXPQXLLLLBPBPBPBBPBQLPBXP

11、QLPBLPBQXLLPBPBQLPBQXL可得联合求解由可得联合求解由可得求解由解法解法 2:11111111112122122112122122122112)()()()()()(1kXkkTkkXkTkkXkXkXkkkkTkXkTkkXkkXTXTXXXTXTXQBQBQBBQQQXBLQBQBBQXXQBQBQBBQQQXBLQBQBBQXXk五、卡尔曼滤波1、kalman滤波理论特点：1） Kalman滤波是 利用观测向量来重构、估计随时间不断变化的状态向量。顺序是：预测实测修正。2） Kalman滤波得到的状态向量估值具有无偏性和最小方差性，是一种基于线性最小方差估计上的最优估计

12、方法。3） Kalman滤波的优点：是一种计算量小，存储量低，实时性高的递推解算法。4）在经历了初始滤波的过渡状态后，滤波效果会很好。Kalman滤波应用滤波应用举例举例 在在雷达跟踪目标系统中，观察到的是带有噪声的雷达跟踪目标系统中，观察到的是带有噪声的目标位置、运动速度、加速度等，目标位置、运动速度、加速度等， Kalman滤波就是滤波就是能利用目标的动态信息，减小或去除噪声的影响，得能利用目标的动态信息，减小或去除噪声的影响，得到关于目标位置的最佳到关于目标位置的最佳估计。估计。 Kalman滤波利用滤波利用过去直到当前过去直到当前的观测信息：的观测信息：1)、对目标当前位置估计（对目标

13、当前位置估计（滤波滤波）；）；2)、对目标将来位置估计（对目标将来位置估计（预测预测）；）；3)、重新、重新估计过去的状态（估计过去的状态（插值插值或称或称平滑平滑）。）。Kalman滤波分为：滤波分为：滤波、预测、滤波、预测、平滑平滑三个主要部分。三个主要部分。的形式（和、上面三部分常记为：)0jkjkXkjkXkkX 21,0,0111111, 1111kDNnrnmnnkDNppmrrmmmmkkkkkkkkkkkkkkkkUGXBLUXX维观测噪声阶系数矩阵阶系数矩阵维观测向量声维动态噪系数阵阶动态噪声或称输入向量维控制向量向量系数阵阶控制阶转移矩阵维状态向量，2、 状态方程（动态方程

14、）和观测方程 下面为下面为 基础基础Kalman滤波的滤波的状态方程状态方程(1) 和和观测方程观测方程(2) （该模（该模型对应的是离散线性系统）：型对应的是离散线性系统）：以上两方程也常称为动力学模型和测量模型。以上两方程也常称为动力学模型和测量模型。Kalman滤波的应用举例：滤波的应用举例：导航，控制，传感器数据融合甚至在军事方面的雷达系统以导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像及导弹追踪等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。边缘检测等等。随机模型 的前二阶矩

15、。和、已知）函数。是克罗内尔（的正定方差阵；是的非负定方差阵；是其中：协方差：方差。；，有且对于一切；kkkjkjkkjkjkjkxxjkkjjkkjjkkkXjkjkKronekDkDLXXkjDXXuXEkDkDEE00001kercovvar0,cov0,cov0,cov00var0000,cov,cov,cov00在完全不相关白噪声作用下，离散线性系统的随机模型为：在完全不相关白噪声作用下，离散线性系统的随机模型为：Kalman滤波的前提条件： kkkkkkkkkkssskskkkkkskkkskkkkkkkkkkkkkkkkkkkkkkZXBlXXklZXBlXXlZXBlXXlll

16、lklXXklUklUGZZXBLUXX1112222212112111110100121111, 111111111, 111111021,1:221，观测方程为：式可有、个时，观测向量有可看成是观测向量。当式中式变为则，。令由上两式可得观测方程）式中设（状态方程和观测方程统一转化为观测方程形式 的滤波公式。于上两式，得标准形式再将矩阵反演公式作用公式）：估值及其方差阵（滤波一起平差，得与上面第三个误差方程，方程为：作为虚拟观测值，误差将方差：个方程解得：、由上面第：方程转变的误差方程为。得到由观测观测值，方差阵为的先验期望看成是虚拟将随机参数，11111111111111101010101

17、0110112222221121111110010010/11/110/10/110/11/11/10/11/10/10/1000/1,00/00/10/00/0211111000/000BDBDDZlDBXDBDBDXXXXvXDDDlXXXXXXlZXBvklXXkvlZXBvlXXvlZXBvlXXvXXvDXTXXTXTXXTTXXskkkkkskkkksssssXX虚拟观测方程可见kalman滤波，就是一种变参数的逐次平差法。推导时注意：P=D-13、kalman滤波器的5个核心公式）。此时并未产生，即预测，所以动态噪声状态方程得到，因为是优滤波值。（上式可从是上一状态的最的预测值，

18、式中求即用）（，01/1,11/11/11111kkkkkkkkkkXXLLUkkXkkXkalman一步预测公式： 没有方差）传播得到。（控制项此式由状态方程经误差，kTkkkkTkkXkkXUkDkkDkkD211/11/1,1,11一步预测值的方差为： 。项去修正了预测值也可以说是用，估值，得到现在状态的最优时测量值和即用预测值1/1/1/31/1/kkXkkXBZLJkkXLkkkXkkXBZLJkkXkkXkkkkkkkkkKalman滤波公式：滤波增益矩阵： 的计算工作量。算出，减少实时处理时与观测值无关，可预先可见kTkXkTkXkJkDBkkDBBkkDJ41/1/1滤波值的方

19、差公式： 51/kkDBJIkkDXkkX 即可。，两式中令、上面则项为零时，测方程中的非随机控制注意：当动态方程和观0031kkZU 为了令卡尔曼滤波器开始工作，需要两个零时刻的初始值为了令卡尔曼滤波器开始工作，需要两个零时刻的初始值-X(0/0)和和DX(0/0)。他们的值可以任意给一个，因为随着卡尔曼的工作，。他们的值可以任意给一个，因为随着卡尔曼的工作，X会逐渐的收敛。会逐渐的收敛。但是对于但是对于DX(0/0) ，一般不要取，一般不要取0，因为这样可能会令系统完全相信你给定的，因为这样可能会令系统完全相信你给定的X(0/0)是系统最优的，从而使算法不能收敛。是系统最优的，从而使算法不能收敛。Kalman系统的可观可控性可控性输入能够控制状态的变化。 可观性状态的变化能由输出反映出来。器。近最优的滤波器和预报的值，可得到简单、渐次计算到稳态。即不再每正定矩阵，此时滤波达将趋于一个唯一确定