《Kalman滤波原理及程序(手册)》.docVIP

《Kalman滤波原理及程序》 KF/EKF/UKF原理+应用实例+MATLAB程序 本手册的研究内容主要有Kalman滤波，扩展Kalman滤波，无迹Kalman滤波等，包括理论介绍和MATLAB源程序两部分。本手册所介绍的线性滤波器，主要是Kalman滤波和α-β滤波，交互多模型Kalman滤波，这些算法的应用领域主要有温度测量、自由落体，GPS导航、石油地震勘探、视频图像中的目标检测和跟踪。 EKF和UKF主要在非线性领域有着重要的应用，目标跟踪是最主要的非线性领域应用之一，除了讲解目标跟踪外，还介绍了通用非线性系统的EKF和UKF滤波处理问题，相信读者可以通过学习本文通用的非线性系统，能快速掌握EKF和UKF滤波算法。 本文所涉及到的每一个应用实例，都包含原理介绍和程序代码（含详细的中文注释）。 一、四维目标跟踪Kalman线性滤波例子 在不考虑机动目标自身的动力因素，将匀速直线运动的船舶系统推广到四维，即状态X(k)=[x(k) (k)x y(k) (k)]y T 包含水平方向的位置和速度和纵向 的位置和速度。则目标跟踪的系统方程可以用式（3.1）和（3.2）表示， X(k+1)=ΦX(k)+Γu(k) Z(k)=HX(k)+v(k) （2-4-9） （2-4-10） ??1 ?? 00? ?H=，2 ?00.5T? ??T???00 0? ?0?1??0? T ?1?0 其中，Φ=? ?0??0 ?x?Z=?? ?y? T100 0010 ?0.5T0? ?? 0T?，Γ=? ?0T? ?? 1???0 2 ?x? ?? x?，X=??y??? ??y T ， ，u,v为零均值的过程噪声和观测噪声。T为采样周期。为了便于理解， 将状态方程和观测方程具体化： 星空实地工作室内部资料 -----Kalman滤波技术交流QQ：345194112 1 ?x(k)??1??? (k)x0??=??y(k)??0??? y(k)???0 T100 0010 0??x(k ?? (k0x??T??y(k?? (k1??y-1)??0.5T ??-1)T ?+?-1)??0 ??-1)???0 ? ?0? w2?1(k) 2?0.5T?T??0 ?x(k)??1 Z=??=? y(k)???0 00 01 ?x(k)? ?? (k)0?x??+v(k) 2?1? 0??y(k)??? y(k)?? 假定船舶在二维水平面上运动，初始位置为（-100m,200m），水平运动速度为2m/s，垂直方向的运动速度为20 m/s，GPS接收机的扫描周期为T=1s，观测噪声的均值为0，方差为100。过程噪声越小，目标越接近匀速直线运动，反之，则为曲线运动。仿真得到以下结果： 图3-1 跟踪轨迹图 图3-2 跟踪误差图 仿真程序 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Kalman滤波在目标跟踪中的应用实例 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function Kalman clc;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滤波技术交流QQ：345194112 …… 二、视频图像目标跟踪Kalman滤波算法实例 如下图所示，对于自由下落的皮球，要在视频中检测目标，这里主要检测目标中心，即红心皮球的重心，在模型建立时可以将该重心抽象成为一个质点，坐标为(x,y)。 那么对该质点跟踪，它的状态为X(k)=[x ?1?0?X(k+1)=?0??0 dt100 0dt10 ]，状态方程如下 y?0???0 ??w(k) ?0????g? 图2-6-1 下落的球 图2-6-2 检测下落的球 图2-6-3 跟踪下落的球 y x 0??0 ?X(k)+0??1? 观测方程为 ?1 Z(k)=? ?0 01 00 0? ?X(k)+v(k) 0? 在这个过程中，前提是