《《Kalman滤波原理及程序(手册)》》由会员分享，可在线阅读，更多相关《《Kalman滤波原理及程序(手册)》（13页珍藏版）》请在金锄头文库上搜索。
1、星空实地工作室内部资料星空实地工作室内部资料 -Kalman 滤波技术交流滤波技术交流 QQ：3451941121Kalman 滤波原理及程序滤波原理及程序 KF/EKF/UKF 原理原理+应用实例应用实例+MATLAB 程序程序本手册的研究内容主要有 Kalman 滤波，扩展 Kalman 滤波，无迹 Kalman滤波等，包括理论介绍和 MATLAB 源程序两部分。本手册所介绍的线性滤波器，主要是 Kalman 滤波和 - 滤波，交互多模型 Kalman 滤波，这些算法的应用领域主要有温度测量、自由落体，GPS 导航、石油地震勘探、视频图像中的目标检测和跟踪。EKF 和 UKF 主要在非线性
2、领域有着重要的应用，目标跟踪是最主要的非线性领域应用之一，除了讲解目标跟踪外，还介绍了通用非线性系统的 EKF和 UKF 滤波处理问题，相信读者可以通过学习本文通用的非线性系统，能快速掌握 EKF 和 UKF 滤波算法。本文所涉及到的每一个应用实例，都包含原理介绍和程序代码（含详细的中文注释） 。一、四维目标跟踪一、四维目标跟踪 Kalman 线性滤波例子线性滤波例子在不考虑机动目标自身的动力因素，将匀速直线运动的船舶系统推广到四维，即状态包含水平方向的位置和速度和纵向TkykykxkxkX)()()()()(clear; T=1;%雷达扫描周期, N=80/T; %总的采样次数 X=zero
3、s(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; % 观测矩阵星空实地工作室内部资料星空实地工作室内部资料 -K
4、alman 滤波技术交流滤波技术交流 QQ：3451941123二、视频图像目标跟踪二、视频图像目标跟踪 Kalman 滤波算法实例滤波算法实例如下图所示，对于自由下落的皮球，要在视频中检测目标，这里主要检测目标中心，即红心皮球的重心，在模型建立时可以将该重心抽象成为一个质点，坐标为。),(yx图 2-6-1 下落的球 图 2-6-2 检测下落的球 图 2-6-3 跟踪下落的球那么对该质点跟踪，它的状态为，状态方程如下yxyxkX %清除所有内存变量、图形窗口 % 计算背景图片数目 Imzero = zeros(240,320,3); for i = 1:5% 将图像文件 i.jpg 的图像像
5、素数据读入矩阵 ImImi = double(imread(DATA/,int2str(i),.jpg);Imzero = Imi+Imzero; end星空实地工作室内部资料星空实地工作室内部资料 -Kalman 滤波技术交流滤波技术交流 QQ：3451941124Imback = Imzero/5; MR,MC,Dim = size(Imback); % 遍历所有图片 for i = 1 : 60 % 读取所有帧运行程序得到的运行程序得到的 x,y 方向的位置跟踪偏差分析方向的位置跟踪偏差分析0102030405060050100150200Y 方向的位置偏差010203040506005
6、0100150X 方向的位置偏差三、通用非线性系统的三、通用非线性系统的 EKF 实现例子：实现例子：所谓的非线性方程，就是因变量与自变量的关系不是线性的，这类方程很多，例如平方关系，对数关系，指数关系，三角函数关系等等。这些方程可分为两类，一类是多项式方程，一种是非多项式方程。为了便于说明非线性卡尔曼滤波扩展 Kalman 滤波的原理，我们选用以下系统，星空实地工作室内部资料星空实地工作室内部资料 -Kalman 滤波技术交流滤波技术交流 QQ：3451941125系统状态为，它仅包含一维变量，即，系统状态方程为)(kX)()(kxkX(3-2-1)()2 . 1cos(8) 1(1) 1(
7、5 . 2) 1(5 . 0)(2kwkkXkXkXkX观测方程为(3-2-2)(20)()(2 kvkXkY其中，式(3-1-1)是包含分式，平方，三角函数在内的严重非线性的方程，为过程噪声，其均值为 0，方差为 Q，观测方程中，观测信号与状态)(kw)(kY的关系也是非线性的，也是均值为 0，方差为 R 的高斯白噪声。因此)(kX)(kv关于(3-1-1)和(3-2-2)是一个状态和观测都为非线性的一维系统。以此为通用的非线性方程的代表，接下来讲述如何用扩展 Kalman 滤波来处理噪声问题。第一步：初始化初始状态，协防差矩阵。)0(X)0(Y0P第二步：状态预测(3-2-3)2 . 1c
8、os(8) 1(1) 1(5 . 2) 1(5 . 0) 1|(2kkXkXkXkkX第三步：观测预测(3-2-4)20) 1|() 1|(2kkXkkY第九步：协方差更新(3-2-10) 1|()()()(kkPkHkKIkPn以上九步为扩展卡尔曼滤波的一个计算周期，如此循环下去就是各个时刻EKF 对非线性系统的处理过程。其他参数设置请查看源程序，仿真以上系统得到状态滤波结果，如图 3-2-1 所示，滤波后的状态与真值之间的偏差如图图 3-2-2 所示。星空实地工作室内部资料星空实地工作室内部资料 -Kalman 滤波技术交流滤波技术交流 QQ：34519411260510152025303
9、5404550-15-10-5051015估 估 估 EKF估 估 估05101520253035404550012345678图 3-2-1 EKF 滤波处理后的状态与真值对比 图 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) % % function EKF_for_One_Div_UnLine_System % 初始化 T=50;
10、% 总时间 Q=10; R=1; % 产生过程噪声 w=sqrt(Q)*randn(1,T); % 产生观测噪声 v=sqrt(R)*randn(1,T);四、四、EKF 在纯方位寻的导弹制导中的应用例子：在纯方位寻的导弹制导中的应用例子：考虑一个在三维平面 x-y-z 内运动的质点 M，其在某一时刻 k 的位置、速度和加速度可用矢量可以表示为： T zyxzyxzyxkakakakvkvkvkrkrkrkx)()()()()()()()()()(质点 M 可以在三维空间内做任何运动，同时假设三个 x-y-z 方向上运动具有加性系统噪声( )kw，则在笛卡尔坐标系下该质点的运动状态方程为：)(
11、),() 1(kwkxfkxk通常情况下，上述方程为线性的，即能表示为以下方式，)()()() 1(kwkukxkx星空实地工作室内部资料星空实地工作室内部资料 -Kalman 滤波技术交流滤波技术交流 QQ：3451941127其中，333333323300)1 (10) 1(1IeIeIItetIIttt 33320)2/( tIIt为测量周期，也叫扫描周期，采样时间间隔等。动态噪声为t)(kwT zyxkkkkw)()()(000000)(而且，1910)( qkwE 32 63366 1000)()(IQkwkwET 是高斯型白色随机向量序列。)(kw现在考虑一个带有观测器的飞行中的导
12、弹，可以假设为质点 M，对移动的 目标进行观测，如下图所示，导弹与目标的相对位置依然可用 x-y-z 表示，那么，导弹对目标纯方位角观测，主要是俯仰角和水平方向偏向角，实际测量中雷达具有加性测量噪声( )kv，则在笛卡尔坐标系下，观测方程为)()()(kvkxhkz式中，Tzxzxy krkrkrkrkrkxh )()(arctan )()()(arctan)( 22为测量噪声，他也是高斯型白色随机向量序列，而且)(kv，1210)( rkvE1)()(RkvkvET星空实地工作室内部资料星空实地工作室内部资料 -Kalman 滤波技术交流滤波技术交流 QQ：3451941128对于，其定义为
13、1R)()()(1 1kxDkDkRT其中，21 . 0 Ix )()()(00)()()()(222222krkrkrkrkrkrkDzyxzyx显然在笛卡尔坐标系下，该模型运动观测方程为非线性的。仿真结果为：-100001000200030004000500100015006007008009001000error of positionrealekf050100150200250300350400150200250300350400450500550600650error of positionekf轨迹跟踪图 位置误差0501001502002503003504009010011012
14、0130140150160170180error of velocityekf050100150200250300350400024681012141618error of accelerationekf速度误差 加速度误差寻的制导寻的制导 matlab 仿真程序仿真程序% % % 程序说明：目标跟踪程序，实现运动弹头对运动物体的三维跟踪，主函数 % 状态方程： x(t)=Ax(t-1)+Bu(t-1)+w(t） % 参考资料：寻的导弹新型导引第 5.5 和 5.6 节中仿真参数设置 % % function main % % delta_t=0.01; % 测量周期,采样周期星空实地工作室内部资料星空实地工作室内部资料 -Kalman 滤波技术交流滤波技术交流 QQ：3451941129longa=1; % 机动时间常数的倒数，即机动频率 T=3.7/delta_t;% 时间长度 3.7 秒钟，一共采样 T=370 次 F=eye(3),delta_t*eye(3),(e
