Kalman滤波原理及程序(手册)

Kalman滤波原理及仿真手册KF/EKF/UKF原理+应用实例+MATLAB程序KalmanKalman滤波，无迹KalmanKalman滤波和α-β滤波，交互多模型Kalman滤波，这些算法的应用领测和跟踪。EKF和UKFEKFUKF滤波算法。〔含具体的中文注释。Kalman线性滤波例子在不考虑机动目标自身的动力因素，将匀速直线运动的船舶系统推广到四维，即状态X(k)x(k) (k)

y(k)

(k)T包含水平方向的位置和速度和纵向的位置和速度。则目标跟踪的系统方程可以用式〔3.1〕和〔3.2〕表示，X(k1)X(k)u(k) 〔2-4-9〕Z(k)HX(k)v(k) 〔2-4-10〕1 T 0 0 0.5T2

0

0T

xT0 1 0 0

T

0

，

，H

，X ，0 0 1

0 0.5T2 0 1

y0 0 0

0 T

0

Zx，u,v为零均值的过程噪声和观测噪声。Ty将状态方程和观测方程具体化：x(k) 1 T 0 0x(k1) 0.5T2 0 (k)

1 0 0(k)

0w (k)0y(k) 0 0 1 Ty(k1) 0 0.5T2 21(k) 0 0 0 1(k) 0 T

x(k)x(k) 1 0 0 Z

(k)y(k) 0 0 1 0y(k) (k

21假定船舶在二维水平面上运动，初始位置为-100m,200m，水平运动速度2m/s20m/s，GPST=1s，观测0，方差为100。过程噪声越小，目标越接近匀速直线运动，反之，则为曲线运动。仿真得到以下结果：1600 251400201200151000800106005400200-120 -100 -80 -60 -40 -20 0 20 40

00 10 20 30 40 50 60图3-1 跟踪轨迹图 图3-2 跟踪误差图仿真程序%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Kalman滤波在目标跟踪中的应用实例%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%functionKalmanclc;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滤波算法实例标为(xy)。图2-6-1下落的球 图2-6-2检测下落的球 图2-6-3跟踪下落的球那么对该质点跟踪，它的状态为X(k)x y ，状态方程如下1 dt 0 0 00 1 dt 0 0X(k) w(k)0 0 1 0 00 0 0 1 g观测方程为

1 0 0 0 Z(k)0 1 0 0X(k)v(k 在这个过程中，前提是目标检测，确定要找到重心(x,y)，与雷达目标跟踪中观测目标位置是一回事。图像目标检测跟踪程序%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%目标检测函数，这个函数主要完成将目标从背景中提取出来%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%functiondetectclear,clc; %去除全部内存变量、图形窗口%计算背景图片数目Imzero=zeros(240,320,3);fori=1:5%将图像文件i.jpg的图像像素数据读入矩阵ImIm{i}=double(imread([”DATA/”,int2str(i),”.jpg”]));Imzero=Im{i}+Imzero;endImback=Imzero/5;[MR,MC,Dim]=size(Imback);%遍历全部图片fori=1:60% 读取全部帧…………运行程序得到的x,y方向的位置跟踪偏差分析2001501005000 10 20 30 40 50 60Y方向的位置偏差1501005000 10 20 30 40 50 60X方向的位置偏差EKF实现例子：Kalman滤波的原理，我们选用以下系统，X(k)X(k)x(k)，系统状态方程为X(k)0.5X(k1)2.5X(k1)8cos(1.2k)w(k) (3-2-1)1X2(k1)观测方程为

X2(k)20

v(k) (3-2-2)其中，式(3-1-1)w(k为过程噪声，其均值为0，方差为Q，观测方程中，观测信号Y(k)X(k)的Kalman滤波来处理噪声问题。X(0)Y(0)P。0其次步：状态推想X(k|k1)0.5X(k1)2.5X(k1)8cos(1.2k) (3-2-3)1X2(k1)

X2(k|k1)20

(3-2-4)…………第九步：协方差更P(k)(InK(k)H(k))P(k|k1) (3-2-10)以上九步为扩展卡尔曼滤波的一个计算周期，如此循环下去就是各个时刻EKF对非线性系统的处理过程。其他参数设置请查看源程序，仿真以上系统得3-2-2所示。真实值EKF估量值15 真实值EKF估量值7106550 43-52-101-15

0 5 10 15 20 25 30 35 40 45 50

00 5 10 15 20 25 30 35 40 45 50图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)%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%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的位置、速度和加速度可用矢量可以表示为： x(k)r(k) r(k) r(k) v(k) v(k) v(k) a(k) a(k) a(k)Tx y z x y z x y z质点Mx-y-z方向上运动具有加性w(k)，则在笛卡尔坐标系下该质点的运动状态方程为：x(k1)fk(x(k),w(k))通常状况下，上述方程为线性的，即能表示为以下方式，x(k1)x(k)u(k)w(k)其中I tII3 3

1(et21

33

(t2/2)I 30

(1et)I

， tI 3 3

3 0 3 0 03

etI 3

3 t为测量周期，也叫扫描周期，采样时间间隔等。动态噪声w(k)为 w(k)0 0 0 0 0 0 x而且

(k) y

(k) z

(k)TEw(k)q1

0

，Ew(k)wT(k)Q

00636

0 632I3w(k是高斯型白色随机向量序列。现在考虑一个带有观测器的飞行中的，可以假设为质点M，对移动的x-y-z表示，v(k)，则在笛卡尔坐标系下，观测方程为z(k)hx(k)v(k)式中，

(k)

r(k)Thx(k)arctan

arctan x r2(k)r2(k)x z

r(k)zv(k)为测量噪声，他也是高斯型白色随机向量序列，而且R1

，其定义为

1

0

，Ev(k)vT(k)R x0.1I2D(k)0

R(k)D1(k)xDT(k)1r2r2(k)r2(k)r2(k)x y zrr(k)r(k)r(k)222x y z明显在笛卡尔坐标系下，该模型运动观测方程为非线性的。仿真结果为：errorofposition650ekf600errorofposition650ekf600550500450400350300250200ekf10009008007006001500

1000

500

0-1000

1000

30002023

4000

1500 50 100 150 200 250 300 350 400轨迹跟踪图 位置误差erroroferrorofvelocity180ekf170160150140130120110100errorofaccelerationekf161412108642900 50 100 150 200 250 300 350 400

00 50 100 150 200 250 300 350 400速度误差 加速度误差寻的制导matlab仿真程序%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%程序说明：目标跟踪程序，实现运动弹头对运动物体的三维跟踪，主函数%状态方程：x(t)=Ax(t-1)+Bu(t-1)+w(t〕%5.55.6节中仿真参数设置%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%functionmain%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%delta_t=0.01;%测量周期,采样周期longa=1; %机动时间常数的倒数，即机动频率T=3.7/delta_t;%3.7T=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)]; %状态转移矩阵faiG=[-1*0.5*delta_t^2*eye(3);-1*delta_t*eye(3);zeros(3)];%把握量驱动矩阵gama…………五、UKFCA目标跟踪模型中的应用例子：一、仿真问题描述x(k)[xk

,y,xk

,y,x,y]Tk k k

表示。假设M在水平方向〔x〕作近似匀加速直线运动，垂直方向〔y〕上亦作近似匀加速直线运动。两方向上运动具w(k)，则在笛卡尔坐标系下该质点的运动状态方程为x(k1)fk其中

(x(k))w(k)Fx(k)w(k)k1 0 1

t2 002002000F0k 00000

1 0 t 0 1 0 t0 0 1 0 0 0 0 0 0

t2220tt011假设一坐标位置为〔0,0〕的雷达对M进展测距rk

和测角k

，实际测量中雷达具有加性测量噪声v(k)，则在传感器极坐标系下，观测方程为rv

(k)

v(k)z(k)h

(x(k))v(k)k

r k v(k)

tan1

(k)x2y2x2y2k kykUKFEKF算法结果进展比较。，三、试验仿真与结果分析，w(k)Qk

100000

00000001000000.12000000.12000000.012000000.0

v(k具有协方差阵Rk

520

0 N=50t=0.5。初始0.012

1所示。实际值测量值ukf实际值测量值ukf估量值53005250520051505100505050001000 1100 1200 1300 1400 1500 1600 1700 1800 1900轨迹跟踪图跟踪位置偏差10987654321005101520 25 30354045504.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];%量测噪声协方差阵%状态方程…………目录第一章Kalman滤波概述1kalman1kalman3kalman滤波应用领域 5其次章线性卡尔曼滤波 6Kalman滤波原理6射影定理 6Kalman8Kalman10Kalman滤波在温度测量中的应用12原理介绍 122.2.1matlab仿真程序13Kalman滤波在自由落体中的应用14原理介绍 14Matlab仿真程序19C语言程序20Kalman滤波在船舶GPS导航定位系统中的应用 22原理介绍 22matlab仿真程序25Kalman滤波在石油地震勘探中的应用26石油地震勘探白噪声反卷积滤波原理 26石油地震勘探白噪声反卷积滤波仿真程序 28Kalman滤波在视频图像目标跟踪中的应用31视频图像处理的根本方法及程序 31Kalman33目标检测程序 33Kal