matlab中imzero,kalman滤波原理及程序(手册).doc

kalman滤波原理及程序(手册).doc

KALMAN滤波原理及仿真手册KF/EKF/UKF原理应用实例MATLAB程序本手册的研究内容主要有KALMAN滤波，扩展KALMAN滤波，无迹KALMAN滤波等，包括理论介绍和MATLAB源程序两部分。本手册所介绍的线性滤波器，主要是KALMAN滤波和ΑΒ滤波，交互多模型KALMAN滤波，这些算法的应用领域主要有温度测量、自由落体，GPS导航、石油地震勘探、视频图像中的目标检测和跟踪。EKF和UKF主要在非线性领域有着重要的应用，目标跟踪是最主要的非线性领域应用之一，除了讲解目标跟踪外，还介绍了通用非线性系统的EKF和UKF滤波处理问题，相信读者可以通过学习本文通用的非线性系统，能快速掌握EKF和UKF滤波算法。本文所涉及到的每一个应用实例，都包含原理介绍和程序代码(含详细的中文注释)。一、四维目标跟踪KALMAN线性滤波例子在不考虑机动目标自身的动力因素，将匀速直线运动的船舶系统推广到四维，即状态包含水平方向的位置和速度和纵向TKYKXKX的位置和速度。则目标跟踪的系统方程可以用式(31)和(32)表示，(249)1UX(2410)KVHKZ其中，，，，，10TT0522T01TYXXYXZ，U,V为零均值的过程噪声和观测噪声。T为采样周期。为了便于理解，将状态方程和观测方程具体化05101122KWTKYXTKYX12KVYXKYXZ假定船舶在二维水平面上运动，初始位置为(100M,200M)，水平运动速度为2M/S，垂直方向的运动速度为20M/S，GPS接收机的扫描周期为T1S，观测噪声的均值为0，方差为100。过程噪声越小，目标越接近匀速直线运动，反之，则为曲线运动。仿真得到以下结果12010080604020020402004006008001000120014001600估估估估估估估估估估估估01020304050600510152025估估估估估估估估估估图31跟踪轨迹图图32跟踪误差图仿真程序KALMAN滤波在目标跟踪中的应用实例FUNCTIONKALMANCLCCLEART1雷达扫描周期,N80/T总的采样次数XZEROS4,N目标真实位置、速度X,1100,2,200,20目标初始位置、速度ZZEROS2,N传感器对位置的观测Z,1X1,1,X3,1观测初始化DELTA_W1E2如果增大这个参数，目标真实轨迹就是曲线了QDELTA_WDIAG05,1,05,1过程噪声均值R100EYE2观测噪声均值F1,T,0,00,1,0,00,0,1,T0,0,0,1状态转移矩阵H1,0,0,00,0,1,0观测矩阵二、视频图像目标跟踪KALMAN滤波算法实例如下图所示，对于自由下落的皮球，要在视频中检测目标，这里主要检测目标中心，即红心皮球的重心，在模型建立时可以将该重心抽象成为一个质点，坐标为。,YX图261下落的球图262检测下落的球图263跟踪下落的球那么对该质点跟踪，它的状态为，状态方程如下YXKX0101KWGDTK观测方程为01KVXKZ在这个过程中，前提是目标检测，一定要找到重心，与雷达目标跟踪,YX中观测目标位置是一回事。图像目标检测跟踪程序目标检测函数，这个函数主要完成将目标从背景中提取出来FUNCTIONDETECTCLEAR,CLC清除所有内存变量、图形窗口计算背景图片数目IMZEROZEROS240,320,3FORI15将图像文件IJPG的图像像素数据读入矩阵IMIM{I}DOUBLEIMREAD DATA/ ,INT2STRI, JPG IMZEROIM{I}IMZEROENDIMBACKIMZERO/5MR,MC,DIMSIZEIMBACK遍历所有图片FORI160读取所有帧运行程序得到的X,Y方向的位置跟踪偏差分析0102030405060050100150200Y方向的位置偏差0102030405060050100150X方向的位置偏差三、通用非线性系统的EKF实现例子所谓的非线性方程，就是因变量与自变量的关系不是线性的，这类方程很多，例如平方关系，对数关系，指数关系，三角函数关系等等。这些方程可分为两类，一类是多项式方程，一种是非多项式方程。为了便于说明非线性卡尔曼滤波扩展KALMAN滤波的原理，我们选用以下系统，系统状态为，它仅包含一维变量，即，系统状态方程为KXKXX32121COS815250WK观测方程为32220KVXKY其中，式311是包含分式，平方，三角函数在内的严重非线性的方程，为过程噪声，其均值为0，方差为Q，观测方程中，观测信号与状态KWKY的关系也是非线性的，也是均值为0，方差为R的高斯白噪声。因此XKV关于311和322是一个状态和观测都为非线性的一维系统。以此为通用的非线性方程的代表，接下来讲述如何用扩展KALMAN滤波来处理噪声问题。第一步初始化初始状态，，协防差矩阵。0XY0P第二步状态预测32321COS815251|KKKKX第三步观测预测324201|1|KXKY第九步协方差更新32101|KPHKKIKPN以上九步为扩展卡尔曼滤波的一个计算周期，如此循环下去就是各个时刻EKF对非线性系统的处理过程。其他参数设置请查看源程序，仿真以上系统得到状态滤波结果，如图321所示，滤波后的状态与真值之间的偏差如图图322所示。0510152025303540455015105051015估估估EKF估估估05101520253035404550012345678图321EKF滤波处理后的状态与真值对比图322偏差分析EKF一维非线性系统仿真程序函数功能一维非线性系统扩展KALMAN滤波问题状态函数XK105XK25XK/1XK28COS12KWK观测方程Z(K)XK2/20VKFUNCTIONEKF_FOR_ONE_DIV_UNLINE_SYSTEM初始化T50总时间Q10R1产生过程噪声WSQRTQRANDN1,T产生观测噪声VSQRTRRANDN1,T四、EKF在纯方位寻的导弹制导中的应用例子考虑一个在三维平面XYZ内运动的质点M，其在某一时刻K的位置、速度和加速度可用矢量可以表示为TZYXZYXZYXAAKVKVRKRK质点M可以在三维空间内做任何运动，同时假设三个XYZ方向上运动具有加性系统噪声KW，则在笛卡尔坐标系下该质点的运动状态方程为,1KWXFKXK通常情况下，上述方程为线性的，即能表示为以下方式，U其中，3332301IEIITTTTT320/TI为测量周期，也叫扫描周期，采样时间间隔等。动态噪声为TKWTZYXKKW00而且，19QE326310IQWET是高斯型白色随机向量序列。KW现在考虑一个带有观测器的飞行中的导弹，可以假设为质点M，对移动的目标进行观测，如下图所示，导弹与目标的相对位置依然可用XYZ表示，那么，导弹对目标纯方位