《Kalman滤波原理及程序手册》Word格式.docx

1、% Kalman滤波在目标跟踪中的应用实例function Kalmanclc;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,

2、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 跟踪下落的球那么对该质点跟踪，它的状态为，状态方程如下观测方程为在这个过程中，前提是目标检测，一定要找到重心，与雷达目标跟踪中观测目标位置是一回事。图像目标检测跟踪程序% 目标检测函数，这个函数主要完成将目标从背景中提取出来function detect

3、clear,clc; %清除所有内存变量、图形窗口% 计算背景图片数目Imzero = zeros（240,320,3）;for i = 1:5 % 将图像文件 i.jpg 的图像像素数据读入矩阵Im Imi = double（imread（DATA/,int2str（i）,.jpg）; Imzero = Imi+Imzero;endImback = Imzero/5;MR,MC,Dim = size（Imback）;% 遍历所有图片for i = 1 : 60% 读取所有帧运行程序得到的x,y方向的位置跟踪偏差分析Y方向的位置偏差X方向的位置偏差三、通用非线性系统的EKF实现例子：所谓的非线

4、性方程，就是因变量与自变量的关系不是线性的，这类方程很多，例如平方关系，对数关系，指数关系，三角函数关系等等。这些方程可分为两类，一类是多项式方程，一种是非多项式方程。为了便于说明非线性卡尔曼滤波扩展Kalman滤波的原理，我们选用以下系统，系统状态为，它仅包含一维变量，即，系统状态方程为 （3-2-1） （3-2-2）其中，式（3-1-1）是包含分式，平方，三角函数在内的严重非线性的方程，为过程噪声，其均值为0，方差为Q，观测方程中，观测信号与状态的关系也是非线性的，也是均值为0，方差为R的高斯白噪声。因此关于（3-1-1）和（3-2-2）是一个状态和观测都为非线性的一维系统。以此为通用的非

5、线性方程的代表，接下来讲述如何用扩展Kalman滤波来处理噪声问题。第一步：初始化初始状态，协防差矩阵。第二步：状态预测 （3-2-3）第三步：观测预测 （3-2-4）第九步：协方差更新 （3-2-10）以上九步为扩展卡尔曼滤波的一个计算周期，如此循环下去就是各个时刻EKF对非线性系统的处理过程。其他参数设置请查看源程序，仿真以上系统得到状态滤波结果，如图3-2-1所示，滤波后的状态与真值之间的偏差如图图3-2-2所示。 图3-2-1 EKF滤波处理后的状态与真值对比 图3-2-2 偏差分析EKF一维非线性系统仿真程序% 函数功能：一维非线性系统扩展Kalman滤波问题% 状态函数：X（k+1

6、）=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; % 总时间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方向上运动具有加性系

7、统噪声，则在笛卡尔坐标系下该质点的运动状态方程为：通常情况下，上述方程为线性的，即能表示为以下方式，其中， 为测量周期，也叫扫描周期，采样时间间隔等。动态噪声为而且是高斯型白色随机向量序列。现在考虑一个带有观测器的飞行中的导弹，可以假设为质点M，对移动的目标进行观测，如下图所示，导弹与目标的相对位置依然可用x-y-z表示，那么，导弹对目标纯方位角观测，主要是俯仰角和水平方向偏向角，实际测量中雷达具有加性测量噪声，则在笛卡尔坐标系下，观测方程为式中，为测量噪声，他也是高斯型白色随机向量序列，而且对于，其定义为其中， 显然在笛卡尔坐标系下，该模型运动观测方程为非线性的。仿真结果为： 轨迹跟踪图 位

8、置误差 速度误差 加速度误差寻的制导matlab仿真程序% 程序说明：目标跟踪程序，实现运动弹头对运动物体的三维跟踪，主函数% 状态方程： x（t）=Ax（t-1）+Bu（t-1）+w（t）% 参考资料：寻的导弹新型导引第5.5和5.6节中仿真参数设置function maindelta_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）/longa2*eye

9、（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_t2*eye（3）;-1*delta_t*eye（3）;zeros（3）; % 控制量驱动矩阵gama五、UKF在六维CA目标跟踪模型中的应用例子：一、仿真问题描述考虑一个在二维平面x-y内运动的质点M，其在某一时刻k的位置、速度和加速度可用矢量表示。假设M在水平方向（x）作近似匀加速直线运动，垂直方向（y）上亦作近似匀加速直线运

10、动。两方向上运动具有加性系统噪声，则在笛卡尔坐标系下该质点的运动状态方程为假设一坐标位置为（0,0）的雷达对M进行测距和测角，实际测量中雷达具有加性测量噪声，则在传感器极坐标系下，观测方程为我们根据雷达测量值使用UKF算法对目标进行跟踪，并与EKF算法结果进行比较。三、实验仿真与结果分析假设设系统噪声具有协方差阵，具有协方差阵，二者不相关。观测次数N=50，采样时间为t=0.5。初始状态。则生成的运动轨迹如图1所示。轨迹跟踪图4.3.2 仿真程序% 功能说明： UKF在目标跟踪中的应用% 参数说明： 状态6维，x方向的位置、速度、加速度；y方向的位置、速度、加速度；% 观测信息为距离和角度；%function ukf_for_track_6_div_systemn=6; % 状态位数t=0.5; % 采样时间Q=1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 0.01 0 0 0; 0 0 0 0.01 0 0; 0 0 0 0 0.0001 0; 0 0 0 0 0 0.0001;%过程噪声协方差阵R = 100 0; 0 0.0012;%量测噪声协方差阵% 状态方程