《Kalman滤波原理及程序手册》Word格式.docx
《《Kalman滤波原理及程序手册》Word格式.docx》由会员分享，可在线阅读，更多相关《《Kalman滤波原理及程序手册》Word格式.docx（16页珍藏版）》请在冰豆网上搜索。
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Kalman滤波在目标跟踪中的应用实例
functionKalman
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滤波算法实例
如下图所示，对于自由下落的皮球，要在视频中检测目标，这里主要检测目标中心，即红心皮球的重心，在模型建立时可以将该重心抽象成为一个质点，坐标为。
图2-6-1下落的球图2-6-2检测下落的球图2-6-3跟踪下落的球
那么对该质点跟踪，它的状态为，状态方程如下
观测方程为
在这个过程中，前提是目标检测，一定要找到重心，与雷达目标跟踪中观测目标位置是一回事。
图像目标检测跟踪程序
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%目标检测函数，这个函数主要完成将目标从背景中提取出来
functiondetect
clear,clc;
%清除所有内存变量、图形窗口
%计算背景图片数目
Imzero=zeros（240,320,3）;
fori=1:
5
%将图像文件i.jpg的图像像素数据读入矩阵Im
Im{i}=double（imread（['
DATA/'
int2str（i）,'
.jpg'
]））;
Imzero=Im{i}+Imzero;
end
Imback=Imzero/5;
[MR,MC,Dim]=size（Imback）;
%遍历所有图片
fori=1:
60
%读取所有帧
运行程序得到的x,y方向的位置跟踪偏差分析
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）];
%状态转移矩阵fai
G=[-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];
%量测噪声协方差阵
%状态方程
升级会员 