Kalman滤波原理及程序综合手册

《Kalman滤波原理及程序》KF/EKF/UKF原理+应用实例+MATLAB程序本手册研究 内容 财务内部控制制度的内容财务内部控制制度的内容人员招聘与配置的内容项目成本控制的内容消防安全演练内容 关键有Kalman滤波,扩展Kalman滤波,无迹Kalman滤波等,包含理论介绍和MATLAB源程序两部分。本手册所介绍线性滤波器,关键是Kalman滤波和α-β滤波,交互多模型Kalman滤波,这些算法应用领域关键有温度测量、自由落体,GPS导航、石油地震勘探、视频图像中目标检测和跟踪。EKF和UKF关键在非线性领域有着关键应用,目标跟踪是最关键非线性领域应用之一,除了讲解目标跟踪外,还介绍了通用非线性系统EKF和UKF滤波处理问 题 快递公司问题件快递公司问题件货款处理关于圆的周长面积重点题型关于解方程组的题及答案关于南海问题 ,相信读者能够经过学习本文通用非线性系统,能快速掌握EKF和UKF滤波算法。本文所包含到每一个应用实例,都包含原理介绍和程序代码（含具体汉字注释）。一、四维目标跟踪Kalman线性滤波例子在不考虑机动目标本身动力原因,将匀速直线运动船舶系统推广到四维,即状态包含水平方向位置和速度和纵向位置和速度。则目标跟踪系统方程能够用式（3.1）和（3.2）表示,（2-4-9）（2-4-10）其中,,,,,,u,v为零均值过程噪声和观察噪声。T为采样周期。为了便于了解,将状态方程和观察方程具体化:假定船舶在二维水平面上运动,初始位置为（-100m,200m）,水平运动速度为2m/s,垂直方向运动速度为20m/s,GPS接收机扫描周期为T=1s,观察噪声均值为0,方差为100。过程噪声越小,目标越靠近匀速直线运动,反之,则为曲线运动。仿真得到以下结果:图3-1跟踪轨迹图图3-2跟踪误差图仿真程序%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%Kalman滤波在目标跟踪中应用实例%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%functionKalmanclc;clear;T=1;%雷达扫描周期,N=80/T;%总采样次数X=zeros(4,N);%目标真实位置、速度X(:,1)=[-100,2,200,20];%目标初始位置、速度Z=zeros(2,N);%传感器对位置观察Z(:,1)=[X(1,1),X(3,1)];%观察初始化delta_w=1e-2;%假如增大这个参数,目标真实轨迹就是曲线了Q=delta_w*diag([0.5,1,0.5,1]);%过程噪声均值R=100*eye(2);%观察噪声均值F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];%状态转移矩阵H=[1,0,0,0;0,0,1,0];%观察矩阵…………二、视频图像目标跟踪Kalman滤波算法实例以下图所表示,对于自由下落皮球,要在视频中检测目标,这里关键检测目标中心,即红心皮球重心,在模型建立时能够将该重心抽象成为一个质点,坐标为。图2-6-1下落球图2-6-2检测下落球图2-6-3跟踪下落球那么对该质点跟踪,它状态为,状态方程以下观察方程为在这个过程中,前提是目标检测,一定要找到重心,与雷达目标跟踪中观察目标位置是一回事。图像目标检测跟踪程序%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%目标检测函数,这个函数关键完成将目标从背景中提取出来%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%functiondetectclear,clc;%清除全部内存变量、图形窗口%计算背景图片数目Imzero=zeros(240,320,3);fori=1:5%将图像文件i.jpg图像像素数据读入矩阵ImIm{i}=double(imread(['DATA/',int2str(i),'.jpg']));Imzero=Im{i}+Imzero;endImback=Imzero/5;[MR,MC,Dim]=size(Imback);%遍历全部图片fori=1:60%读取全部帧…………运行程序得到x,y方向位置跟踪偏差 分析 定性数据统计分析pdf销售业绩分析模板建筑结构震害分析销售进度分析表京东商城竞争战略分析 Y方向位置偏差X方向位置偏差三、通用非线性系统EKF实现例子:所谓非线性方程,就是因变量与自变量关系不是线性,这类方程很多,比如平方关系,对数关系,指数关系,三角函数关系等等。这些方程可分为两类,一类是多项式方程,一个是非多项式方程。为了便于说明非线性卡尔曼滤波——扩展Kalman滤波原理,我们选择以下系统,系统状态为,它仅包含一维变量,即,系统状态方程为(3-2-1)观察方程为(3-2-2)其中,式(3-1-1)是包含分式,平方,三角函数在内严重非线性方程,为过程噪声,其均值为0,方差为Q,观察方程中,观察信号与状态关系也是非线性,也是均值为0,方差为R高斯白噪声。所以相关(3-1-1)和(3-2-2)是一个状态和观察都为非线性一维系统。以此为通用非线性方程代表,接下来讲述怎样用扩展Kalman滤波来处理噪声问题。第一步:初始化初始状态,,协防差矩阵。第二步:状态估计(3-2-3)第三步:观察估计(3-2-4)…………第九步:协方差更新(3-2-10)以上九步为扩展卡尔曼滤波一个计算周期,如此循环下去就是各个时刻EKF对非线性系统处理过程。其她参数设置请查看源程序,仿真以上系统得到状态滤波结果,如图3-2-1所表示,滤波后状态与真值之间偏差如图图3-2-2所表示。图3-2-1EKF滤波处理后状态与真值对比图3-2-2偏差分析EKF一维非线性系统仿真程序%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%函数功效:一维非线性系统扩展Kalman滤波问题%状态函数:X(k+1)=0.5X(k)+2.5X(k)/(1+X(k)^2)+8cos(1.2k)+w(k)%观察方程:Z（k）=X(k)^2/20+v(k)%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%functionEKF_for_One_Div_UnLine_System%初始化T=50;%总时间Q=10;R=1;%产生过程噪声w=sqrt(Q)*randn(1,T);%产生观察噪声v=sqrt(R)*randn(1,T);…………四、EKF在纯方位寻导弹制导中应用例子:考虑一个在三维平面x-y-z内运动质点M,其在某一时刻k位置、速度和加速度可用矢量能够表示为:质点M能够在三维空间内做任何运动,同时假设三个x-y-z方向上运动含有加性系统噪声,则在笛卡尔坐标系下该质点运动状态方程为:通常情况下,上述方程为线性,即能表示为以下方法,其中,为测量周期,也叫扫描周期,采样时间间隔等。动态噪声为而且,是高斯型白色随机向量序列。现在考虑一个带有观察器飞行中导弹,能够假设为质点M,对移动目标进行观察,以下图所表示,导弹与目标相对位置仍然可用x-y-z表示,那么,导弹对目标纯方位角观察,关键是俯仰角和水平方向偏向角,实际测量中雷达含有加性测量噪声,则在笛卡尔坐标系下,观察方程为式中,为测量噪声,她也是高斯型白色随机向量序列,而且,对于,其定义为其中,显然在笛卡尔坐标系下,该模型运动观察方程为非线性。仿真结果为:轨迹跟踪图位置误差速度误差加速度误差寻制导matlab仿真程序%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%程序说明:目标跟踪程序,实现运动弹头对运动物体三维跟踪,主函数%状态方程:x(t)=Ax(t-1)+Bu(t-1)+w(t）%参考资料:《寻导弹新型导引》第5.5和5.6节中仿真参数设置%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%functionmain%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%delta_t=0.01;%测量周期,采样周期longa=1;%机动时间常数倒数,即机动频率T=3.7/delta_t;%时间长度3.7秒钟,一共采样T=370次F=[eye(3),delta_t*eye(3),(exp(-1*longa*delta_t)+longa*delta_t-1)/longa^2*eye(3);zeros(3),eye(3),(1-exp(-1*longa*delta_t))/longa*eye(3);zeros(3),zeros(3),exp(-1*longa*delta_t)*eye(3)];%状态转移矩阵faiG=[-1*0.5*delta_t^2*eye(3);-1*delta_t*eye(3);zeros(3)];%控制量驱动矩阵gama…………五、UKF在六维CA目标跟踪模型中应用例子:一、仿真问题描述考虑一个在二维平面x-y内运动质点M,其在某一时刻k位置、速度和加速度可用矢量表示。假设M在水平方向（x）作近似匀加速直线运动,垂直方向（y）上亦作近似匀加速直线运动。两方向上运动含有加性系统噪声,则在笛卡尔坐标系下该质点运动状态方程为其中假设一坐标位置为（0,0）雷达对M进行测距和测角,实际测量中雷达含有加性测量噪声,则在传感器极坐标系下,观察方程为显然在笛卡尔坐标系下,该模型运动观察方程为非线性。我们依据雷达测量值使用UKF算法对目标进行跟踪,并与EKF算法结果进行比较。三、试验仿真与结果分析假设设系统噪声含有协方差阵,含有协方差阵,二者不相关。观察次数N=50,采样时间为t=0.5。初始状态。则生成运动轨迹如图1所表示。轨迹跟踪图4.3.2仿真程序%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%功效说明:UKF在目标跟踪中应用%参数说明:状态6维,x方向位置、速度、加速度;y方向位置、速度、加速度;%观察信息为距离和角度;%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%functionukf_for_track_6_div_system%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%n=6;%状态位数t=0.5;%采样时间Q=[100000;010000;000.01000;0000.0100;00000.00010;000000.0001];%过程噪声协方差阵R=[1000;00.001^2];%量测噪声协方差阵%状态方程…………目录第一章Kalman滤波概述11.1kalman滤波背景11.2kalman滤波发展过程31.3kalman滤波应用领域5第二章线性卡尔曼滤波62.1Kalman滤波原理62.1.1射影定理62.1.2Kalman滤波原理推导82.1.3Kalman滤波过程描述102.2Kalman滤波在温度测量中应用122.2.1原理介绍122.2.1matlab仿真程序132.3Kalman滤波在自由落体中应用142.3.1原理介绍142.3.2Matlab仿真程序192.3.3C语言程序202.4Kalman滤波在船舶GPS导航定位系统中应用222.4.1原理介绍222.4.2matlab仿真程序252.5Kalman滤波在石油地震勘探中应用262.5.1石油地震勘探白噪声反卷积滤波原理262.5.2石油地震勘探白噪声反卷积滤波仿真程序282.6Kalman滤波在视频图像目标跟踪中应用312.6.1视频图像处理基础方法及程序312.6.2Kalman滤波对自由下落皮球跟踪应用332.6.3目标检测程序332.6.4Kalman滤波视频跟踪程序35第三章扩展卡尔曼滤波EKF373.1扩展Kalman滤波原理373.2扩展卡尔曼在一维非线性系统中应用383.2.1状态方程和观察方程都为非线性通用系统383.2.2EKF一维非线性系统仿真程序403.3扩展卡尔曼在二维非线性系统中应用413.3.1状态方程和观察方程都为非线性通用二维系统413.3.2EKF二维非线性系统仿真程序433.4扩展卡尔曼在二维目标跟踪中应用443.4.1基于观察距离EKF目标跟踪算法443.4.2跟踪算法matlab程序443.4.3基于纯方位EKF目标跟踪算法463.4.4纯方位目标跟踪算法matlab程序473.5EKF在纯方位寻导弹制导中应用483.5.1三维寻制导系统483.5.2寻制导matlab仿真程序52第四章无迹卡尔曼滤波UKF554.1无迹Kalman滤波原理554.1.1UT变换554.1.2无迹卡尔曼原理564.2UKF在单站二维目标跟踪中应用574.2.1原理介绍574.2.2仿真程序574.3UKF在六维CA目标跟踪模型中应用604.3.1原理介绍604.3.2仿真程序62第五章交互多模型卡尔曼滤波655.1交互多模型Kalman滤波（IMM）655.1.1交互多模原理655.1.2仿真方法665.2交互多模型Kalman滤波在目标跟踪中应用705.2.1仿真分析705.2.2IMM仿真程序71第六章自校正α-β跟踪滤波器766.1自校正α-β跟踪滤波器766.2自校正α-β跟踪滤波器在雷达跟踪系统中应用786.2.1跟踪系统介绍786.2.2自校正α-β跟踪滤波器仿真程序806.2.3Gevers-Wouters算法仿真程序816.2.4递推增广最小二乘自校正算法程序826.3含未知模型参数自校正α-β跟踪滤波器836.3.1原理介绍836.3.2仿真程序85本书网址链接:HYPERLINK""