Kalman滤波原理及程序（手册）解析

Kalman滤波原理及仿真手册

KF/EKF/UKF原理+应用实例+MATLAB程序

本手册的研究内容主要有Kalman滤波，扩展Kalman滤波，无迹Kalman滤波等，包括理论介绍和MATLAB源程序两部分。本手册所介绍的线性滤波器，主要是Kalman滤波和α-β滤波，交互多模型Kalman滤波，这些算法的应用领域主要有温度测量、自由落体，GPS导航、石油地震勘探、视频图像中的目标检测和跟踪。

EKF和UKF主要在非线性领域有着重要的应用，目标跟踪是最主要的非线性领域应用之一，除了讲解目标跟踪外，还介绍了通用非线性系统的EKF和UKF滤波处理问题，相信读者可以通过学习本文通用的非线性系统，能快速掌握EKF和UKF滤波算法。

本文所涉及到的每一个应用实例，都包含原理介绍和程序代码（含详细的中文注释）。

一、四维目标跟踪Kalman线性滤波例子

在不考虑机动目标自身的动力因素，将匀速直线运动的船舶系统推广到四

?(k)维，即状态X(k)??x(k)xy(k)?(k)?T包含水平方向的位置和速度和纵向y的位置和速度。则目标跟踪的系统方程可以用式（3.1）和（3.2）表示，

X(k?1)??X(k)??u(k) （2-4-9） Z(k)?HX(k)?v(k) （2-4-10）

?1?0其中，????0??0T1000010?0.5T20??0??，???T?0T???1??0?0??1??00?，H??2??00.5T??T???00??x??x??0??，X???，?y?1??????0??yTT?x?Z???，u,v为零均值的过程噪声和观测噪声。T为采样周期。为了便于理解，

?y?将状态方程和观测方程具体化：

?x(k)??1?x?(k)??0?????y(k)??0????(k)??0?yT00??x(k?1)??0.5T2?x?(k?1)??100??????T01T??y(k?1)??0?????(k?1)??001??y?00??0?w(k) 2?2?10.5T?T???x(k)???(k)??x(k)??1000??x??v2?1(k) Z???????y(k)??0010??y(k)????y(k)??假定船舶在二维水平面上运动，初始位置为（-100m,200m），水平运动速度为2m/s，垂直方向的运动速度为20 m/s，GPS接收机的扫描周期为T=1s，观测噪声的均值为0，方差为100。过程噪声越小，目标越接近匀速直线运动，反之，则为曲线运动。仿真得到以下结果：

1600真实轨迹观测轨迹滤波轨迹 25滤波前误差滤波后误差 1400201200151000800106005400200 -120-100-80-60-40-20020400 0102030405060

图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滤波算法实例

如下图所示，对于自由下落的皮球，要在视频中检测目标，这里主要检测目标中心，即红心皮球的重心，在模型建立时可以将该重心抽象成为一个质点，坐标为(x,y)。

图2-6-1 下落的球 图2-6-2 检测下落的球 图2-6-3 跟踪下落的球 那么对该质点跟踪，它的状态为X(k)??x?yx??，状态方程如下 y?1dt00??0??01dt0??0??X(k)???w(k) X(k?1)???0010??0?????0001???g?观测方程为

?1000?Z(k)???X(k)?v(k) 0100??在这个过程中，前提是目标检测，一定要找到重心(x,y)，与雷达目标跟踪中观测目标位置是一回事。 图像目标检测跟踪程序

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 目标检测函数，这个函数主要完成将目标从背景中提取出来

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function detect

clear,clc; %清除所有内存变量、图形窗口 % 计算背景图片数目 Imzero = zeros(240,320,3); for i = 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); % 遍历所有图片 for i = 1 : 60

% 读取所有帧

…… ……

运行程序得到的x,y方向的位置跟踪偏差分析

2001501005000102030405060

Y方向的位置偏差

1501005000102030405060

X方向的位置偏差

三、通用非线性系统的EKF实现例子：

所谓的非线性方程，就是因变量与自变量的关系不是线性的，这类方程很多，例如平方关系，对数关系，指数关系，三角函数关系等等。这些方程可分为两类，一类是多项式方程，一种是非多项式方程。为了便于说明非线性卡尔曼滤波——扩展Kalman滤波的原理，我们选用以下系统，

系统状态为X(k)，它仅包含一维变量，即X(k)??x(k)?，系统状态方程为

X(k)?0.5X(k?1)?2.5X(k?1)?8cos(1.2k)?w(k) (3-2-1)

1?X2(k?1)观测方程为

X2(k)Y(k)??v(k) (3-2-2)

20其中，式(3-1-1)是包含分式，平方，三角函数在内的严重非线性的方程，w(k)为过程噪声，其均值为0，方差为Q，观测方程中，观测信号Y(k)与状态X(k)的

v(k)也是均值为0，关系也是非线性的，方差为R的高斯白噪声。因此关于(3-1-1)

和(3-2-2)是一个状态和观测都为非线性的一维系统。以此为通用的非线性方程的代表，接下来讲述如何用扩展Kalman滤波来处理噪声问题。

第一步：初始化初始状态X(0)，Y(0)，协防差矩阵P0。 第二步：状态预测

X(k|k?1)?0.5X(k?1)?2.5X(k?1)?8cos(1.2k) (3-2-3)

1?X2(k?1)

第三步：观测预测

X2(k|k?1)Y(k|k?1)? (3-2-4)

20…… ……

第九步：协方差更新

P(k)?(In?K(k)H(k))P(k|k?1) (3-2-10)

以上九步为扩展卡尔曼滤波的一个计算周期，如此循环下去就是各个时刻EKF对非线性系统的处理过程。其他参数设置请查看源程序，仿真以上系统得到状态滤波结果，如图3-2-1所示，滤波后的状态与真值之间的偏差如图图3-2-2

所示。

15真实值EKF估计值 8761055043-52-101-15 051015202530354045500

05101520253035404550

图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)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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的位置、速度和加速度可用矢量可以表示为：

x(k)?rx(k)ry(k)rz(k)vx(k)vy(k)vz(k)ax(k)ay(k)az(k)

??T质点M可以在三维空间内做任何运动，同时假设三个x-y-z方向上运动具有加性系统噪声w(k)，则在笛卡尔坐标系下该质点的运动状态方程为：

x(k?1)?fk(x(k),w(k))

通常情况下，上述方程为线性的，即能表示为以下方式，

x(k?1)??x(k)??u(k)?w(k)

其中

??I3????03??03???tI3I303????t(e???t?1)I3??2??(?t2/2)I3??1?????t?，?????tI3? (1?e)I3???????t03eI3?????1?t为测量周期，也叫扫描周期，采样时间间隔等。动态噪声w(k)为

w(k)?000000?x(k)?y(k)?z(k)

??T而且

06?3??0 E?w(k)??q1?09?1，Ew(k)wT(k)?Q1??6?2?03?6?I3???w(k)是高斯型白色随机向量序列。

现在考虑一个带有观测器的飞行中的导弹，可以假设为质点M，对移动的目标进行观测，如下图所示，导弹与目标的相对位置依然可用x-y-z表示，

那么，导弹对目标纯方位角观测，主要是俯仰角和水平方向偏向角，实际测量中雷达具有加性测量噪声v(k)，则在笛卡尔坐标系下，观测方程为

z(k)?h?x(k)??v(k)

式中，

?ry(k)h?x(k)???arctan22?r(k)?rxz(k)??rx(k)?? arctanrz(k)??Tv(k)为测量噪声，他也是高斯型白色随机向量序列，而且

E?v(k)??r1?02?1，Ev(k)vT(k)?R1

对于R1，其定义为

??R1(k)?D?1(k)xD?T(k)

其中，x?0.1I2

?r2(k)?r2(k)?r2(k)xyzD(k)???0???

222rx(k)?ry(k)?rz(k)??

显然在笛卡尔坐标系下，该模型运动观测方程为非线性的。仿真结果为：

0error of positionreal ekf

error of position650ekf6001000 550500450900800400350300700600150040001000 5000-1000100020003000250200150 050100150200250300350400 轨迹跟踪图 位置误差

error of velocity180ekf17016015014013012011010090 0

ekferror of acceleration181614121086420 05010015020025030035040050100150200250300350400 速度误差 加速度误差

寻的制导matlab仿真程序

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 程序说明：目标跟踪程序，实现运动弹头对运动物体的三维跟踪，主函数 % 状态方程： x(t)=Ax(t-1)+Bu(t-1)+w(t） % 参考资料：《寻的导弹新型导引》第5.5和5.6节中仿真参数设置

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function main

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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的位置、速度和加速度可用矢量x(k)?[xk,yk,xk,yk,xk,yk]表示。假设M在水平方向（x）作近似匀加速直线运动，垂直方向（y）上亦作近似匀加速直线运动。两方向上运动具有加性系统噪声w(k)，则在笛卡尔坐标系下该质点的运动状态方程为

x(k?1)?fk(x(k))?w(k)?Fkx(k)?w(k)

T其中

??1???0Fk??

?0?0??0??0

0t

t20

2

0t010

?0??2t?2??0? t??0??1?

10t010001000000

假设一坐标位置为（0,0）的雷达对M进行测距rk和测角?k，实际测量中雷达具有加性测量噪声v(k)，则在传感器极坐标系下，观测方程为

?x2?y2?v(k)?kr?rk?vr(k)??k?z(k)?hk(x(k))?v(k)???????1yk??v(k)tan?v?(k)? k????xk??显然在笛卡尔坐标系下，该模型运动观测方程为非线性的。我们根据雷达测

量值使用UKF算法对目标进行跟踪，并与EKF算法结果进行比较。

三、实验仿真与结果分析

?1?0??0Qk??假设设系统噪声w(k)具有协方差阵?0?0???001000000000.01200?0??0??v(k)具有0?，0??0.012??00.120000.12000000?52协方差阵Rk???00?，二者不相关。观测次数N=50，采样时间为t=0.5。初始2?0.01?Tx(0)?[1000,5000,10,50,2,?4]状态。则生成的运动轨迹如图1所示。

5350实际值测量值ukf估计值 5300525052005150510050505000 1000110012001300140015001600170018001900

轨迹跟踪图

跟踪位置偏差10987654321005101520253035404550

4.3.2 仿真程序

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 功能说明： UKF在目标跟踪中的应用

% 参数说明： 状态6维，x方向的位置、速度、加速度；y方向的位置、速度、加速度； % 观测信息为距离和角度；

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%function ukf_for_track_6_div_system

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

n=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.001^2];%量测噪声协方差阵 % 状态方程

…… ……

目 录

第一章 Kalman滤波概述 1 1.1 kalman滤波的背景 1 1.2 kalman滤波的发展过程 3 1.3 kalman滤波应用领域 5 第二章 线性卡尔曼滤波 6 2.1 Kalman滤波原理 6 2.1.1 射影定理 6

2.1.2 Kalman滤波原理推导 8 2.1.3 Kalman滤波过程描述 10

2.2 Kalman滤波在温度测量中的应用 12 2.2.1 原理介绍 12 2.2.1 matlab仿真程序 13

2.3 Kalman滤波在自由落体中的应用 14 2.3.1 原理介绍 14 2.3.2 Matlab仿真程序 19 2.3.3 C语言程序 20

2.4 Kalman滤波在船舶GPS导航定位系统中的应用 22 2.4.1 原理介绍 22 2.4.2 matlab仿真程序 25

2.5 Kalman滤波在石油地震勘探中的应用 26 2.5.1 石油地震勘探白噪声反卷积滤波原理 26 2.5.2 石油地震勘探白噪声反卷积滤波仿真程序 28 2.6 Kalman滤波在视频图像目标跟踪中的应用 31

2.6.1 视频图像处理的基本方法及程序 31 2.6.2 Kalman滤波对自由下落的皮球跟踪应用 33 2.6.3 目标检测程序 33

2.6.4 Kalman滤波视频跟踪程序 35 第三章 扩展卡尔曼滤波EKF 37 3.1 扩展Kalman滤波原理 37

3.2 扩展卡尔曼在一维非线性系统中的应用 38 3.2.1 状态方程和观测方程都为非线性的通用系统 38 3.2.2 EKF一维非线性系统仿真程序 40

3.3 扩展卡尔曼在二维非线性系统中的应用 41

3.3.1 状态方程和观测方程都为非线性的通用二维系统 41 3.3.2 EKF二维非线性系统仿真程序 43 3.4 扩展卡尔曼在二维目标跟踪中的应用 44 3.4.1 基于观测距离的EKF目标跟踪算法 44 3.4.2跟踪算法matlab程序 44

3.4.3 基于纯方位的EKF目标跟踪算法 46 3.4.4纯方位目标跟踪算法matlab程序 47 3.5 EKF在纯方位寻的导弹制导中的应用 48 3.5.1 三维寻的制导系统 48 3.5.2 寻的制导matlab仿真程序 52 第四章 无迹卡尔曼滤波UKF 55 4.1 无迹Kalman滤波原理 55 4.1.1 UT变换 55

4.1.2 无迹卡尔曼原理 56

4.2 UKF在单站二维目标跟踪中的应用 57 4.2.1 原理介绍 57 4.2.2 仿真程序 57

4.3 UKF在六维CA目标跟踪模型中的应用 60 4.3.1 原理介绍 60 4.3.2 仿真程序 62

第五章 交互多模型卡尔曼滤波 65

5.1交互多模型Kalman滤波（IMM） 65 5.1.1 交互多模原理 65 5.1.2 仿真方法 66

5.2交互多模型Kalman滤波在目标跟踪中应用 70 5.2.1 仿真分析 70 5.2.2 IMM仿真程序 71

第六章 自校正α-β跟踪滤波器 76 6.1 自校正α-β跟踪滤波器 76

6.2 自校正α-β跟踪滤波器在雷达跟踪系统中的应用 78 6.2.1 跟踪系统介绍 78

6.2.2 自校正α-β跟踪滤波器仿真程序 80 6.2.3 Gevers-Wouters算法仿真程序 81 6.2.4 递推增广最小二乘自校正算法程序 82

6.3 含未知模型参数的自校正α-β跟踪滤波器 6.3.1 原理介绍 83 6.3.2 仿真程序 85

83

本书的网址：http://item.taobao.com/item.htm?id=16410732773

本文来源：https://www.bwwdw.com/article/y89o.html