《Kalman滤波原理及程序手册》.docx
- 文档编号:1108338
- 上传时间:2022-10-17
- 格式:DOCX
- 页数:16
- 大小:345.40KB
《Kalman滤波原理及程序手册》.docx
《《Kalman滤波原理及程序手册》.docx》由会员分享,可在线阅读,更多相关《《Kalman滤波原理及程序手册》.docx(16页珍藏版)》请在冰豆网上搜索。
《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滤波在目标跟踪中的应用实例
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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];%量测噪声协方差阵
%状态方程
……
……
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- Kalman滤波原理及程序手册 Kalman 滤波 原理 程序 手册