基于MATLAB的卡尔曼滤波仿真研究

3 利 用 MATLAB 进 行 卡 尔 曼 滤 波 仿 真的实现过程
在 MATLAB 中有专用的卡尔曼滤波函数，是 基于具有一般性的线性定常系统：
x觶 (k+1)=Ax(k)+Bu(k)+Gw(k) y(k)=Cx(k)+Du(k)+Hw(k)+v(k) 给出的。其中，x(k)为 k 时刻的状态向量，u(k)为 k 时刻的控制向量，w(k)为状态随机噪声，v(k)为观 测随机噪声。在 MATLAB 中，按下列步骤编写程 序： a） 给动态方程中各矩阵 A，B，C，D，G，H 赋初始值。 b） 给定 A，B，C，D，G，H 后，利用下面语
社， 1994. [4] 王照林.现代控制理论基础[M]. 北京：国防工业出版社，
1983.
63
x (1) !
$
" "
k
% %
"
%
d） 令 yk=Cxk=[0
1
0]
x"
" "
k
(2)
=x %
% %
k
(2)。利用计
"
%
x (3) ""
%%
#k
&
end %绘图说明状态理念值与滤波后最优估计值的 对比 （如图 1 所示）
算机生成标准正态随机噪声 v1，v2…，v20，E[v(i)e(i)]
t=1:20;
subplot(2,2,3),plot(t,x(3,:),'b',t,x_e(3,:),'r');
%理论初值
x(:,1)=[8.593;1计初值
x_e(:,1)=[15;16;17];
A=[0.5,0.3,0.4;0.5,- 0.4,0.4;- 0.1,0.4,0.3];
MATLAB 能适合多学科、多部门的要求，其 特点是：1） 以复数矩阵或数组为数据单元进行运 算，可直接处理矩阵或数组；2） 语言结构紧凑， 内涵丰富，编程效率高，用户使用方便；3） 强大 的绘图功能，用户只需一条或几条语句就可方便 地给出复杂的二维、三维图形；4） 含有丰富的内 部函数，可直接调用而不需另行编程 （如用来求 解微分方程或微分方程组的 Dsolve 函数、求解线 性方程组的 Solve 函数）；5） 带有 simulink 动态模 拟工具及 Toolbox 等其它功能，可方便地生成模拟 模型；6） 便于系统扩充，通过 M 文件形式，共享 C、Fortran 等语言的资源；7） 在涉及复杂算法的 仿真中(如电气传动控制系统)，弥补了 Pspice 传递 函数的不足。
图 1 状态理念值与滤波后最优估计值的对比
通过图 1 可以看出，10 个时间间隔后滤波值 与状态真值已经非常接近。
x1(:,i)=x(:,i)+w; QN=eye(2,2); RN=1; NN=0; %加观测噪声干扰 z0(:,i)=C*x1(:,i)+v;
4.2 求出模拟系统状态滤波值
5 结束语
本文通过MatLab 平台实现了卡尔曼滤波的过 程仿真，并给出了仿真结果。实践证明，利用 MatLab 进行卡尔曼滤波仿真，可以快速、准确地 得到火控计算机所需的目标运动参数，为整个火力 打击的精度和实效性提供了可靠的保证。
（防空兵学院， 河南 郑州 450052）
摘 要： 针对防空兵高炮火控系统中的滤波问题， 应用矩阵实验室 (MATLAB) 实现对目标运动参数的滤波仿
真。 介绍了 MATLAB 软件的相关知识， 阐述了利用 MATLAB 进行卡尔曼滤波仿真的实现过程。 通过实例， 利
用 MATLAB 对某型雷达卡尔曼滤波的过程和结果进行了进一步的仿真。 结果表明， 利用 MATLAB 进行卡尔曼
DIANZI CHANPIN KEKAOXING YU HUANJING SHIYAN
参考文献： [1] 刘慧颖. MatLab R2007 基础教程[M]. 北京：清华大学出
版社，2008. [2] 李相民.火力控制原理[M]. 北京：国防工业出版社，2007. [3] 朱元昌.火控系统原理与分析[M]. 北京：兵器工业出版
电子产品可靠性与环境试验 ELECTRONIC PRODUCT RELIABILITY AND ENVIRONMENTAL TESTING
计算机科学与技术
Vol．29 No．6 Dec., 2011 2011 年 12 月第 29 卷 第 6 期
基于 MATLAB 的卡尔曼滤波仿真研究
冯刚， 吕茂庭， 覃天
1
0
)xk，x1=
""1.25
"
% % %
0.39 ""
%%
#
&
b） 利 用 递 推 公 式 x (k+1)=Axk 和 状 态 初 值 x1=
!"0.85
$ %
"
%
""1.25
"
%%，得出确定值
%
x2，x3，…x20
0.39 ""
%%
#
&
DIANZI CHANPIN KEKAOXING YU HUANJING SHIYAN
调用卡尔曼滤波函数，利用得到的输出值 y1，y2 …，y20 进行滤波，求出模拟系统状态滤波值x赞 1，x赞 2， …x赞20。将 xk(i)和x赞k(i)，k=1，2，…，20；i=1，2，3 分 别打印出来进行比较。具体的程序如下：
%描述系统 SYS=SS(A,[B G],C,[D H],- 1); %卡尔曼滤波
FENG Gang， LV Mao-ting， QIN Tian （Air Defense Forces Academy, Zhengzhou 450052， China）
Abstract： For the air -defense anti -aircraft gun fire control system, MATLAB is used for the
62
句对系统进行描述： SYS=SS(A,[B G],C,[D H],T)
在上述语句中，SYS 代表系统；T 表示该离散 系统的采样间隔时间，若取 T=- 1，则表示采样间 隔时间为默许值。如果系统中缺少某项，则可将相 应的矩阵用零阵代替。
c） 调用卡尔曼滤波函数进行滤波 [KEST,L,P,M,Z]=kalman(SYS,QN,RN,NN)
x1
B=zeros(3,3);
x2
G=eye(3,3);
C=[0,1,0];
D=[0,0,0];
x赞 3
H=zeros(1,3);
for i=2:20
%产生服从正态分布 N(0，1)的模型噪声
x3
w=randn(3,1);
%产生服从正态分布 N(0，1)的观测噪声 v=randn(1,1); x(:,i)=A*x(:,i- 1); %加模型噪声干扰
2 MATLAB 简介
MATLAB 是一种将数据结构、编程特性以及 图形用户界面完美地结合到一起的软件，由美国 MathWorks 公司开发。它提供了常用的矩阵代数运 算功能，同时还提供了非常广泛、灵活的数组运 算功能，并可以与 Fortran 和 C 语言混合编程，进 一 步 扩 展 了 它 的 功 能 。 尤 为 重 要 的 是 ， MATLAB 中提供了可视化动态仿真环境— ——simulink，可实 现动态系统的直观建模、仿真与分析，并支持连 续、离散及两者混合的线性和非线性系统，因此 使一个复杂系统的输入和仿真变得相当简单。
=0，并分别与 x1，x2，…x20 叠加得出模拟系统的输
subplot(2,2,1),plot(t,x(1,:),'b',t,x_e(1,:),'r');
出yi=yi+vi，i=1，2，…，20
subplot(2,2,2),plot(t,x(2,:),'b',t,x_e(2,:),'r');
相应的 MATLAB 程序如下：
第6期
冯 刚等： 基于 MATLAB 的卡尔曼滤波仿真研究
c） 利用计算机生成标准正态随机噪声 e1，e2
[KEST,L,P]=kalman(SYS,QN,RN,NN);
…，e20 并分别与 x1，x2，…x20 叠加，得到 xi=xi+ei，
%计算滤波值
i=1，2，…，20
x_e(:,i)=A*x_e(:,i- 1)+L*(z0(:,i)- C*A*x_e(:,i- 1));
中，滤波对整个系统起着至关重要的作用。 卡尔曼滤波器是一种滤波稳定、滤波质量高、
适应各种不同条件的最理想的滤波器。因为它大大 减少了存贮量，便于实时处理，所以被广泛地应用 于火控系统中。尽管卡尔曼滤波递推公式在形式上 比较简单，但人工计算仍然在计算速度和精度上存 在很大的困难，利用矩阵实验室 （MATLAB） 来实 现卡尔曼滤波仿真，将对提高整个火控系统的精确
Key words： MATLAB; Kalman filtering; simulation
1 引言
在防空兵高炮火控系统中，火控计算机要根据 目标的运动参数来计算射击诸元，这些参数是由跟 踪系统提供，跟踪系统中的测量装置测得目标运动 参数后，因为这些测量参数存在着随机误差，必须 经过滤波处理后，才能够得到较准确的目标运动参 数供火控计算机使用，因此，在整个火力打击过程
计算出 x(k|k)。 e） 输出所需要的结果或曲线。
4 实例分析
已知某型雷达测得某飞机的运动参数，现要将
这些参数通过卡尔曼滤波处理，输出此飞机的较为
准确的运动参数(计算结果输出到第 20 位)。
根据雷达测量结果，得到：