MATLAB在卡尔曼滤波器中应用的理论与实践Kalman
MATLAB在卡尔曼滤波器中应用的理论与实践KalmanKALMAN FILTERINGTheory and Practice Using MATLABThird editionMOHINDER S GREWALCalifornia State University at FullertonANGUS P. ANDREWSRockwell Science Center (retired)WILEYA JOHN WILEY & SONS, INC. PUBLICATIONCopyright 2008 by John Wiley sons, Inc. All rights reservedPublished by John Wiley sons, InC, Hoboken, New JerseyPublished simultaneously in CanadaNo part of this publication may be reproduced, stored in a retrieval system, or transmitted in any form or byany means, electronic, mechanical, photocopying, recording, scanning, or otherwise, except as permittedunder Section 107 or 108 of the 1976 United States Copyright Act, without either the prior writtenpermission of the Publisher, or authorization through payment of the appropriate per-copy fee to theCopyright Clearance Center, Inc, 222 Rosewood Drive, Danvers, MA 01923,(978)750-8400, fax(978)750-4470,oronthewebatwww.copyright.com.RequeststothePublisherforpermissionshouldbe addressed to the Permissions Department, John Wiley Sons, Inc, lll River Street, Hoboken, NJ07030,(201)748-6011,fax(201)748-6008,oronlineathttp://www.wiley.com/go/permissionimit of liability Disclaimer of Warranty: While the publisher and author have used their best efforts inpreparing this book, they make no representations or warranties with respect to the accuracy or completeness of the contents of this book and specifically disclaim any implied warranties of merchantability orfitness for a particular purpose. No warranty may be created or extended by sales representatives orwritten sales materials. The advice and strategies contained herein may not be suitable for your situationYou should consult with a professional where appropriate. Neither the publisher nor author shall be liablefor any loss of profit or any other commercial damages, including but not limited to special, incidentalconsequential, or other damagesFor general information on our other products and services or for technical support, please contact ourCustomer Care Department within the United States at(800)762-2974, outside the United States at(317)572-3993 or fax(317)572-4002Wiley also publishes its books in a variety of electronic formats. Some content that appears in print maynot be available in electronic format. For more information about wiley products, visit our web site atwww.wiley.comLibrary of Congress Cataloging- in-Publication DataGrewal. Mohinder sKalman filtering: theory and practice using MATLAB/Mohinder S. GrewalAngus p. andrews. 3rd edIncludes bibliographical references and indexISBN978-0-470-17366-4( cloth)1. Kalman filtering. 2. MATLAB. I. Andrews, Angus P. II. TitleQA402.3.G69520086298312—dc22200803733Printed in the United States of america10987654321CONTENTSPrefaceAcknowledgmentsXIIIList of abbreviationsXV1 General Information1.1 On Kalman Filtering1.2 On Optimal Estimation Methods, 51. 3 On the notation Used In This book 231. 4 Summary, 25Problems. 262 Linear Dvnamic Systems2. 1 Chapter focus, 312.2 Dynamic System Models, 362. 3 Continuous Linear Systems and Their Solutions, 402.4 Discrete Linear Systems and Their Solutions, 532.5 Observability of Linear Dynamic System Models, 552.6 Summary, 61Problems. 643 Random Processes and Stochastic Systems3.1 Chapter Focus, 673.2 Probability and random Variables (rvs), 703.3 Statistical Properties of RVS, 78CONTEN3.4 Statistical Properties of Random Processes(RPs),803.5 Linear rp models. 883.6 Shaping Filters and State Augmentation, 953.7 Mean and Covariance propagation, 993.8 Relationships between Model Parameters, 1053.9 Orthogonality principle 1143.10 Summary, 118Problems. 1214 Linear Optimal Filters and Predictors1314.1 Chapter Focus, 1314.2 Kalman Filter. 1334.3 Kalman-Bucy filter, 1444.4 Optimal Linear Predictors, 1464.5 Correlated noise Sources 1474.6 Relationships between Kalman-Bucy and wiener Filters, 1484.7 Quadratic Loss Functions, 1494.8 Matrix Riccati Differential Equation. 1514.9 Matrix Riccati Equation In Discrete Time, 1654.10 Model equations for Transformed State Variables, 1704.11 Application of Kalman Filters, 1724.12 Summary, 177Problems. 1795 Optimal Smoothers5.1 Chapter Focus, 1835.2 Fixed-Interval Smoothing, 1895.3 Fixed-Lag Smoothing, 2005.4 Fixed-Point Smoothing, 2135.5 Summary, 220Problems. 226 Implementation Methods2256. 1 Chapter Focus, 2256.2 Computer Roundoff, 2276.3 Effects of roundoff errors on Kalman filters 2326.4 Factorization Methods for Square-Root Filtering, 2386. 5 Square-Root and UD Filters, 2616.6 Other Implementation Methods, 2756.7 Summary, 288Problems. 2897 Nonlinear Filtering2937.1 Chapter Focus, 2937.2 Quasilinear Filtering, 296CONTENTS7.3 Sampling Methods for Nonlinear Filtering, 3307.4 Summary, 345Problems. 3508 Practical Considerations3558.1 Chapter Focus. 3558.2 Detecting and Correcting Anomalous behavior, 3568.3 Prefiltering and Data Rejection Methods, 3798.4 Stability of Kalman Filters, 3828. 5 Suboptimal and reduced- Order Filters, 3838.6 Schmidt-Kalman Filtering, 3938.7 Memory, Throughput, and wordlength Requirements, 4038.8 Ways to Reduce Computational requirements 4098.9 Error Budgets and Sensitivity Analysis, 4148.10 Optimizing Measurement Selection Policies, 4198.11 Innovations analysis, 4248.12 Summary, 425Problems. 4269 Applications to Navigation4279.1 Chapter focus, 4279.2 Host vehicle dynamics, 4319.3 Inertial Navigation Systems(INS), 4359. 4 Global Navigation Satellite Systems(GNSS), 4659.5 Kalman Filters for GNSS. 4709.6 Loosely Coupled GNSS/INS Integration, 4889.7 Tightly Coupled GNSS /INS Integration, 4919. 8 Summary, 507Problems. 508Appendix A MATLAB Software511A 1 Notice. 511A 2 General System Requirements, 511A 3 CD Directory Structure, 512A 4 MATLAB Software for Chapter 2, 512A. 5 MATLAB Software for Chapter 3, 512A6 MATLAB Software for Chapter 4, 512A. 7 MATLAB Software for Chapter 5, 513A 8 MATLAB Software for Chapter 6, 513A 9 MATLAB Software for Chapter 7, 514A10 MATLAB Software for Chapter 8, 515A 11 MATLAB Software for Chapter 9, 515A 12 Other Sources of software 516CONTENAppendix b A Matrix Refresher519B. 1 Matrix Forms. 519B 2 Matrix Operations, 523B 3 Block matrix Formulas. 527B 4 Functions of Square Matrices, 531B 5 Norms. 538B6 Cholesky decomposition, 541B7 Orthogonal Decompositions of Matrices, 543B 8 Quadratic Forms, 545B 9 Derivatives of matrices. 546Bibliography549Index565PREFACEThis book is designed to provide familiarity with both the theoretical and practicalaspects of Kalman filtering by including real-world problems in practice as illustrativeexamples. The material includes the essential technical background for Kalman filter-ing and the more practical aspects of implementation: how to represent the problem ina mathematical model, analyze the performance of the estimator as a function ofsystem design parameters, implement the mechanization equations in numericallystable algorithms, assess its computational requirements, test the validity of resultsitor the filteThetant attributes ofthe subject that are often overlooked in theoretical treatments but are necessary forapplication of the theory to real-world problemsIn this third edition, we have included important developments in the implemen-tation and application of Kalman filtering over the past several years, including adaptations for nonlinear filtering, more robust smoothing methods, and develelopingapplications in navigationWe have also incorporated many helpful corrections and suggefrom ourreaders, reviewers, colleagues, and students over the past several years for theoverall improvement of the textbookAll software has been provided in MatLab so that users can take advantage ofits excellent graphing capabilities and a programming interface that is very close tothe mathematical equations used for defining Kalman filtering and its applicationsSee Appendix a for more information on MATLAB softwareThe inclusion of the software is practically a matter of necessity because Kalmanfiltering would not be very useful without computers to implement it. It provides aMATLAB is a registered trademark of The Mathworks, IncEFACEbetter learning experience for the student to discover how the Kalman filter works byobserving it in actionThe implementation of Kalman filtering on computers also illuminates some of thepractical considerations of finite-wordlength arithmetic and the need for alternativealgorithms to preserve the accuracy of the results. If the student wishes to applywhat she or he learns, then it is essential that she or he experience its workingsand failings--and learn to recognize the differenceThe book is organized as a text for an introductory course in stochastic processes atthe senior level and as a first-year graduate-level course in Kalman filtering theory andapplicationIt can also be used for self-instruction or for purposes of review by practi-cing engineers and scientists who are not intimately familiar with the subject. Theorganization of the material is illustrated by the following chapter-level dependencygraph, which shows how the subject of each chapter depends upon material in otherchapters. The arrows in the figure indicate the recommended order of study. Boxesabove another box and connected by arrows indicate that the material represented bythe upper boxes is background material for the subject in the lower boxAPPENDIX B: A MATRIX REFRESHERGENERAL INFORMATION2. LINEAR DYNAMIC SYSTEMSRANDOM PROCESSES AND STOCHASTIC SYSTEMS4. OPTIMAL LINEAR FILTERS AND PREDICTORS5. OPTIMAL SMOOTHERS6. IMPLEMENTATIONMETHODS7. NONLINEAR8. PRACTICAL9. APPLICATIONSFILTERINGCONSIDERATIONSTO NAVIGATIONAPPENDIX A: MATLAB SOFTWAREChapter l provides an informal introduction to the general subject matter by wayof its history of development and application. Chapters 2 and 3 and Appendix b coverthe essential background material on linear systems, probability, stochastic processesand modeling. These chapters could be covered in a senior-level course in electricalcomputer, and systems engineeringChapter 4 covers linear optimal filters and predictors, with detailed examples ofapplications. Chapter 5 is a new tutorial-level treatment of optimal smoothing
- 2020-12-01下载
- 积分：1
opencv2.4.9源码分析——SIFT
详细介绍SIFT算法，opencv的SIFT源码分析，以及应用实例SIFT算法进行了改进,通过对两个相邻高斯尺度空间的图像相减,得到个DoG(高斯差分,Difference of gaussians)的响应值图像Dx,y,σ)来近似LoGD(x,y, o)=(G(x,y, ko)-G(x,y,o)O1(x,y)=L(x,y, ko)-L(x,y,a(5)其中,k为两个相邻尺度空间倍数的常数。可以证明DoG是对LoG的近似表示,并且用DoG代替LoG并不影响对图像斑点位賀的检测。而且用DoG近似LoG可以实现下列好处:第一是LoG需要使用两个方向的高斯二阶微分卷积核,而DoG直接使用晑斯卷积核,省去了卷积核生成的运算量;第二是DoG保留了个高斯尺度空间的图像,因此在生成某一空间尺度的特征时,可以直接使用公式1(或公式3)产生的尺度空间图像,而无需重新再次生成该尺度的图像:第三是DoG具有与LoG相同的性质,即稳定性好、抗干扰能力强。为了在连续的尺度下检测图像的特征点,需要建立DoG金宇塔,而DoG金宁塔的建立又离不开髙斯金字塔的建立,如下图所小,左侧为高斯金字塔,右侧为DoG金字塔:(nextoctave)Scale(firstoctave)Difference ofaussianGaussian(DOG)图1高斯金字塔和DoG金字塔高斯金字塔共分O组( Octave),每组又分S层( Layer)。组内各层图像的分辨率是相同的,即长和宽相同,但尺度逐渐增加,即越往塔顶图像越模糊。而下·组的图像是由上组图像按照隔点降采样得到的,即图像的长和宽分别减半。高斯金字塔的组数O是由输入图像的分辨牽得到的,因为要进行隔点降采样,所以在执行降釆样生成高斯金字塔时,一直到不能降采样为止,但图像太小又亳无意义,因此具体的公式为:0=| log2 min(x,y)-2」(6)其中,X和Y分别为输入图像的长和宽,L」衣示向下取整。金字塔的层数S为:(7)LoWe建议s为3。需要注意的是,除了公式7中的第一个字母是大写的S外,后面出现的都是小写的s髙斯金字塔的创建是这样的:设输入图像的尺度为0.5,由该图像得到高斯金字塔的第0组的第0层图像,它的尺度为m,我们称m为基准层尺度,再由第0层得到第1层,它的尺度为ko,第2层的尺度为k2o,以此类推。这里的k为:(8)我们以s=3为例,第0组的6(s+3=6)幅图像的尺度分别为:0,ko0,k2∞,k3o0,k∞o,k5o(9)写成更一般的公式为:d=or∈[0,,s+2](10)第0组构建完成后,再构建第1组。第1组的第0层图像是由第0组的倒数第3层图像经过隔点采样得到的。由公式10可以得到,第0组的倒数第3层图像的尺度为k∞o,k的值代入公式8,得到了该层图像的尺度正好为2∞,因此第1组的第0层图像的尺度仍然是2∞。但由于第1组图像是由第0组图像经隔点降采样得到的,因此相对于第1组图像的分辨率来说,第θ层图像的尺度为ω,即尺度为2σ是相对于输入图像的分辨率来说的,而尺度为∞是相对丁该组图像的分辨率来说的。这也就是为什么我们称0为基准层尺度的原因(它是每组图像的基准层尺度)。第1组其他层图像的生成与第0组的相同。因此可以看出,第1组各层图像的尺度相对于该组分辨率来说仍然满足公式10。这样做的好处就是编程的效率会提高,并且也保证∫高斯金字塔尺度空间的连续性。而之所以会出现这样的结果,是因为在参数选择上同吋满足公式7、公式8以及对上·组倒数第3层图像降釆样这三个条件的原因。那么第1组各层图像相对」输入图像来说,它们的尺度为:=2k00r∈[0,,S-2该公式与公式10相比较可以看出,第1组各层图像的尺度比第0组相对应层图像的尺度人了一倍。高斯金字塔的其他组的构建以此类推,不再赘述。下面给出相对于输入图像的各层图像的尺度公式:o,)=2k∞O∈[0,O-1l,r∈[0,,+2(12)其中,O表示组的坐标,r表示层的坐标,a为基准层尺度。k用公式8代入,得:2O∈[0,…0-1],r∈[0,…,s+2](13)在高斯金字塔中,第0组第∂层的图像是输入图像经髙斯模糊后的结果,模糊后的图像的高频部分必然会减少,因比为了最大程度的保留原图的信息量,LoWe建议在创建尺度空间前首先对输入图像的长宽扩展一倍,这样就形成了高斯金字塔的第-1组。设输入图像的尺度为0.5,那么相对于输入图像,分辨率护人一倍后的尺度应为1,由该图像依次进行高斯平滑处理得到第-1组的各个层的尺度图像,方法与其他组的一样。由于增加」第-1组,因此公式13重新写为(0∈[-1,0,…,0-1],r∈[0,…,s+2](14)DoG金字塔是由高斯金字塔得到的,即高斯金宁塔组内相邻两层图像相减得到DoG金字塔。如髙斯金字塔的第0组的筼0层和第1层相减得到DoG金字塔的第0组的箅0层图像,高斯金字塔的第0组的第1层和第2层相减得到υσG金字塔的第θ组的第1层图像以此类推。需要注意的是,高斯金字塔的组内相邻两层相减,而两组间的各层是不能相减的因此高斯金字塔每组有s+3层图像,而DoG金宁塔每组则有s+2层图像。极值点的搜索是在DoG金字塔内进行的,这些极值点就是候选的特征点。在搜索之前,我们需要在DoG金字塔内剔除那些像素值过小的点,因为这些像素具有较低的对比度,它们肯定不是稳定的特征点。极值点的搜索不仅需要在它所在尺度空间图像的邻域内进行,还需要在它的相邻尺度空间图像内进行,如图2所示。每个像素在它的尺度图像中一共有8个相邻点,而在它的下一个相邻尺度图像和上个相邻尺度图像还各有9个相鸰点(图2中绿色标注的像素),也就是说,该点是在3×3×3的立方体内被包围着,因此该点在DoG金字塔内一共有26个相邻点需要比较,来判断其是否为极大值或极小值。这里所说的相邻尺度图像指的是在同个组内,因此在DoG金字塔内,每一个组的第0层和最后一层各只有一个相邻尺度图像,所以在搜索极值点时无需在这两层尺度图像内进行,从而使极值点的搜索就只在每组的中间s层尺度图像内进行。搜索的过程是这样的:从每组的第1层开始,以第1层为当前层,对第1层的DoG图像中的每个点取·个3×3×3的立方体,立方体上下层分别为第0层和第2层。这样,搜索得到的极值点既有位置坐标(该点所在图像的空间坐标),又有尺度空间坐标(该点所在层的尺度)。当第1层搜索完成后,再以第2层为当前层,其过程与第1层的搜索类似,以此类推。Scale图2DoG中极值点的搜索2、特征点的定位通过上一步,我们得到了极值点,但这些极值点还仅仅是候选的特征点,因为它们还存在一些不确定的因素。首先是极值点的搜索是在离散空间内进行的,并且这些离散空间还是经过不断的降采样得到的。如果把采样点拟合成由面后我们会发现,原先的极值点并不是真正的极值点,也就是离散空间的极值点并不是连续空间的极值点。在这里,我们是需要精确定位特征点的位置和尺度的,也就是要达到亚像素精度,因此必须进行拟合处。我们使用泰勒级数展开式作为拟合函数。如上所述,极值点是·个三维矢量,即它包括极值点所在的尺度,以及它的尺度图像坐标,即=(x,y,o),因此我们需要三维函数的泰勒级数展开式,设我们在=(x0,y,)处进行泰勒级数展开,则它的矩阵形式为:602f02f02fdxax day dao02f02f02faxdy ayay ayaallly-yol2f02f02fOrdo aydo dodo(15)公式15为舍去高阶项的形式,而它的矢量表示形式为f(X)=f(X0)+o￥(X-x0)+7(x-x0)a F(X-Xo(16)在这里表示离散空间卜的插值中心(在离散空问内也就是采样点)坐标,表示拟合后连续空间下的插值点坐标,设ⅹ=Ⅹ-Xn,则X表示相对于插值中心,插值后的偏移量。因此公式16绎过变量变换后,又可写成:f(x)=f(X0)+yX+XTⅩX20X2(17)对上式求导,得af (x a02f0ox ox+2 ax2+axa80f.02fXaxaX2(18)让公式17的导数为0,即公式18为0,就可得到极值点下的相对于插值中心的偏移量:aX2 ax(19)把公式19得到的极值点带入公式17中,就得到了该极值点下的极值Tf(X)=f(X0)+af02f10f)a2f/02f-1of2 8X2 0X/0X28X2dXf(X0)+H打×1ora2Ta2f-ra2fa2f-1 af2 dx dx2dx2dx2 dXa f02f-10f∫(X0)+dF×f7a22 ax ax2 axaflf(Xo)+xx+2 0X(-X)18Ff(X0)+2 aX(20)对于公式19所求得的偏移量如果大」0.5(只要x、y和σ任意一个量大于0.5),则表明插值点已偏移到了它的临近的插值中心,所以必须改变当前的位置,使其为它所偏移到的插值中心处,然后在新的位置上重新进行泰勒级数插值拟合,直到偏移量小于0.5为止(x、y和σ都小于0.5),这是一个迭代的工程。当然,为了避免无限次的迭代,我们还需要设置个最人迭代次数,在达到了迭代次数但仍然没有满足偏移量小于0.5的情况下,该极值点就要被剔除掉。另外,如果由公式20所得到的极值f(X过小,即f(X1,则Tr(H)2(a+β)2(+β)2(y+1)2Det(h)2(25)上式的结果只与两个特征值的比例有关,而与具体的特征值无关。我们知道,当某个像系的矩阵的两个特征值相差越大,即γ很大,则该像素越有可能是边缘。对于公式25,当两个特征值相等时,等式的值最小,随着γ的增加,等式的值也增加。所以,要想检查主曲率的比值是否小于某一阈值y,只要检査下式是否成立即可:Tr(H)(y+1)Det(h)(26)对于不满足上式的极值点就不是特征点,因此应该把它们剔除掉。Lowe给出γ为10在上面的运算中,需要用到有限差分法求偏导,在这里我们给出具体的公式。为方便起见我们以图像为例只给出二元函数的实例。与二元函数类似,三元函数的偏导可以很容易的得到设f(i,是ν轴为i、x轴为j的图像像素值,则在(j点处的一阶、二阶及二阶混合偏导af f(i, j+1)-f(i, j0ff(i+1,j)-f(-1,ax2h2h(27)ff(+1)+f(-1)-2f(,j)a2ff(+1,j+f(-1,j)-2f(i,j)hh(28)2ff(-1,j-1)+f(i+1,j+1)-f(i-1,+1)-f(i+1,-1)dx d(29)由丁在图像中,相邻像素之问的间隔都是1,所以这里的h3、方向角度的确定经过上面两个步骤,一幅图像的特征点就可以完全找到,而且这些特征点是具有尺度不变性。但为了实现旋转不变性,还需要为特征点分配一个方向角度,也就是需要根据检测到的特征点所在的高斯尺度图像的局部结构求得一个方向基准。该高斯尺度图像的尺度a是已知的,并且该尺度是相对于高斯金字塔所在组的基准层的尺度,也就是公式10所表示的尺度。而所谓局部结构指的是在高斯尺度图像中以特征点为中心,以r为半径的区域内计算所有像素梯度的幅角和幅值,半径r为(30)其中a就是上面提到的相对于所在组的基准层的高斯尺度图像的尺度。像素梯度的幅值和幅角的计算公式为:m(xy)=√(x+1,y)-L(x-1,y)2+(L(x,y+1),L(x,y-1)2(31)L(x,y+1)-L(x,y-1)o(x, y)=arctanL(x+1,y)-L(x-1,y)(32)因为在以〃为半径的区域内的像素梯度幅值对圆心处的特征点的贡献是不同的,因此还需要对幅值进行加权处理,这里采用的是高斯加权,该高斯函数的方差Cm为:Om=1.50(33)其中,公式中的σ也就是公式30中的σ在完成特征点邻域范围内的梯度计算后,还要应用梯度方向直方图来统计邻域內内像素的梯度方向所对应的幅值大小。具体的做法是,把360°分为36个柱,则每10°为一个柱,即0°~9为第1柱,10°~19为第2柱,以此类推。在以r为半径的区域内,把那些梯度方向在0~9°范围内的像索找出来,把它们的加权后的梯度嘔值相加在一起,作为第1柱的柱高;求第2柱以及其他柱的高度的方法相同,不再赘述。为了防止某个梯度方向角度因受到噪声的干扰而突变,我们还需要对梯度方向直方图进行平滑处理。 Opencv2.4.9所使用的平滑公式为:H()~h(-2)+h(+2)4×(h(-1)+h(+1)),6×h()i=0...15161616(34)其中h和H分别表示平滑前和平滑后的直方图。由于角度是循环的,即0°=360°,如果出现h(),j超出了(0,…,15)的范围,那么可以通过圆周循环的方法找到它所对应的、在0°~360°之间的值,如h(-1)-h(15)这样,直方图的主峰值,即最高的那个柱体所代表的方向就是该特征点处邻域范围内图像棁度的主方向,也就是该特征点的上方向。由于柱体所代表的角度只是一个范围,如第1柱的角度为0~9°,因此还需要对离散的梯度方向直方图进行插值拟合处理,以得到更精确的方向角度值。例如我们凵经得到了第i柱所代表的方向为特征点的主方向,则拟合公式为:H(i-1)-H(i+1)B=i+=0,…152×(H(-1)+H(i+1)-2×H()(35)O=360-10xB(36)其中,H为由公式34得到的直方图,角度6的单位是度。同样的,公式35和公式36也存在着公式34所遇到的角度问题,处理的方法同样还是利用角度的圆周循环。每个特征点除了必须分配一个主方向外,还可能有一个或更多个辅方冋同,增加辅方向的目的是为了增强图像匹配的鲁棒性。辅方向的定义是,当存在另个柱体高度大于主方向柱体高度的80%时,则该柱体所代表的方向角度就是该特征点的辅方向。在第2步中,我们实现∫用两个信息量来表小一个特征点,即位置和尺度。那么经过上面的计算,我们对特征点的表示形式又增加了个信息量一一方向,即(x,y,o,6)。如果某个特征点还有一个辅方向,则这个特征点就要用两个值来表示——(x,y,,B1)和(x,y,,02),其中O1表示主方向,O2表示辅方向,而其他的变量x,y,不变。4、特征点描述符生成
- 2020-06-25下载
- 积分：1
