卡尔曼滤波的仿真实现

x(k 1) x(k ) Tx(k ) (T 2 / 2)ax (k ) x(k 1) x(k ) Tax (k )
用矩阵的形式表述为，
X (k 1) X (k ) W (k )
2
1 2 T 1 T x(k ) 在上式中， X (k ) ， ， 2 ， W ( k ) ax 。 0 1 x(k ) T
%%%%%%%子程序
function [X,Y]=track(Ts,offtime) % 产生真实航迹[X,Y]，并在直角坐标系下显示出 % Ts 为雷达扫描周期，每隔 Ts 秒取一个观测数据 % 最初做匀速运动，接下来进行两个 90 度的机动转弯
if nargin>2 error('输入的变量过多，请检查'); end
ˆk Ak x ˆk 1 H k ( yk Ck Ak x ˆk 1 ) x
T T T 上式中：H k PkCk (Ck PkCk Rk )1 ，Pk Ak Pk 1 Ak Qk 1 ，Pk ( I H k Ck ) Pk
这里，再给出初始条件： E[x 0 ]=0 ， var[ x0 ] E[( x0 0 )( x0 0 )T ] p0
6
end
%滤波误差的均值 XERB=mean(Xerr,2); YERB=mean(Yerr,2); %滤波误差的标准差 XSTD=std(Xerr,1,2); % 计算有偏的估计值，flag='1' YSTD=std(Yerr,1,2);
%作图 figure plot(x,y,'r');hold on;
%返回值包括滤波预测后的估计航迹,以及均值和误差协方差
clear all;close all;clc; Ts=2; num_MC=50; ObserNoise=100;
offtime=800;
% 产生理论的航迹并绘图 [x,y]=track(Ts,offtime); figure plot(x,y,'LineWidth',2) title('目标真实轨迹'); %axis([1800 4500 2000 10000]), grid on;
vx=vx+ax*200; % 第一次机动转弯结束时的速度 vy=vy+ay*200;
% t=600:610s 匀速运动 for t=0:10 x(t+601)=x(601)+vx*t; y(t+601)=y(601)+vy*t; end
% t=610:660s，第二次转弯
9
ax=-0.3; ay=0.3;
3
11000 10000 9000 8000 7000 6000 5000 4000 3000 2000 1000 真实轨迹 滤波估计
2000
3000
4000
5000
6000
7000
8000
9000
10000
图2
Kalman 滤波跟踪结果
图 2 表示的是滤波跟踪的图，由图可以看出，kalman 滤波算法对目标的跟 踪效果是比较好的。
西安电子科技大学
DSP 技术及应用实验报告
题 学 专 学
目 院 业 号
Kalman 滤波算法的 Matlab 仿真 电子工程学院 信号与信息处理 1402121068 张俊飞 郭万有
学生姓名 授课教师
撰写日期： 2014 年 11 月 21 日
Kalman 滤波算法的 Matlab 仿真
一、实验目的
由此，得到误差协方差阵为：
T
PD P PD T
2 PD 1 2 T Q 4 T PD T
上式中， PD 表示观测距离误差， Q 表示过程误差。 以上推到，得到该场景的滤波初值及初始误差协方差阵。下面可以根据 Kalman 滤波的方法予以仿真。
四、实验结果
50
X方 向 滤 波 误 差 均 值
0
-50
0
50
100
150
200 观测次数
250
300
350
400
X方 向 滤 波 误 差 标 准 差
150
100
50
0
0
50
100
150
200 观测次数
250
300
350
400
图3
x 方向Biblioteka Baidu误差统计
4
50
Y方 向 滤 波 误 差 均 值
0
-50
0
50
100
150
Do D d D(2)
T
， Dx 表示 x 方向的距离值。
在观测过程中，我们有如下关系式：
Do (2) Do (1) 0.5aT T
所以，估计误差为：
v (1) vx (2) T X (2 / 2) vx (2) ax (1) x 2 T
观测方程为：
Z (k ) C (k ) X (k ) V (k )
C (k ) 1 0 ， V (k ) 为零均值的噪声序列，方差已知。
我们首先设定一个滤波初值，不妨选择滤波初值为：
ˆ (2 / 2) D (2) X x
Dx (2) Dx (1) / T
%plot(xObser,yObser,'g');hold on; plot(Xhat,Yhat,'b');hold off; grid on; legend('真实轨迹','滤波估计');
figure subplot(211) plot(XERB) axis([0 400 -50 50]) xlabel('观测次数') ylabel('X 方向滤波误差均值'),grid on; subplot(212) plot(XSTD) axis([0 400 0 150]) xlabel('观测次数') ylabel('X 方向滤波误差标准差'),grid on; figure
1. 掌握 Kalman 滤波算法的基本原理； 2．利用 Matlab 平台对 Kalman 算法进行仿真。
二、实验原理
卡尔曼滤波是一种高效率的递归滤波器（自回归滤波器） ，它能够从一系列 的不完全及包含噪声的测量中，估计动态系统的状态。 卡尔曼滤波的一个典型实例是从一组有限的，包含噪声的，通过对物体位置 的观察序列 （可能有偏差） 预测出物体的位置的坐标及速度。 在很多工程应用 （如 雷达、计算机视觉）中都可以找到它的身影。同时，卡尔曼滤波也是控制理论以 及控制系统工程中的一个重要课题。例如，对于雷达来说，人们感兴趣的是其能 够跟踪目标。但目标的位置、速度、加速度的测量值往往在任何时候都有噪声。 卡尔曼滤波利用目标的动态信息，设法去掉噪声的影响，得到一个关于目标位置 的好的估计。这个估计可以是对当前目标位置的估计（滤波） ，也可以是对于将 来位置的估计（预测） ，也可以是对过去位置的估计（插值或平滑） 。 N 维状态方程与 m 维量测方程分别是：
参考文献
随机信号处理 陆光华 彭学愚等 西安电子科技大学出版社
附代码
%main 主程序： % filter_result 均值和标准差 % Ts % num_MC % ObserNoise 采样时间，即雷达的工作周期 进行 Monte-Carlo 仿真的次数 测量的误差,单位 m
5
对观测数据进行卡尔曼滤波，得到预测的航迹以及估计误差的
N=ceil(offtime/Ts);
%rng('shuffle');%junfei_code %randn('state',sum(100*clock)); % 设置随机数发生器 %rng('v5normal')
Xerr=zeros(N,num_MC); Yerr=zeros(N,num_MC); for n=1:num_MC % 用卡尔曼滤波得到估计的航迹 Xhat=Kalman(Ts,offtime,ObserNoise,0); Yhat=Kalman(Ts,offtime,ObserNoise,1); %误差矩阵 Xerr(1:N,n)=x(1:N)-(Xhat(1:N))'; Yerr(1:N,n)=y(1:N)-(Yhat(1:N))';
目标真实轨迹 10000 9000 8000 7000 6000 5000 4000 3000 2000 2000
3000
4000
5000
6000
7000
8000
9000
10000
图 1 目标的真实轨迹 因为 x 和 y 独立地进行观测，所以两者并没有关联性。为了方便说明，这里 仅以 x 轴为例， y 轴的具体情况类似。目标运动沿 x 轴方向的运动可以用下面的 状态方程描述：
200 观测次数
250
300
350
400
Y方 向 滤 波 误 差 标 准 差
150
100
50
0
0
50
100
150
200 观测次数
250
300
350
400
图4
y 方向的误差统计
图 3 和图 4 展示了 kalman 算法在 x，y 两个方向的跟踪误差，由图我们可以 看出： 1. 整体的跟踪误差在 50m 以内。整体的跟踪的效果是比较好的。 2. 由标准差差图，可以看出，标准差基本上趋于平稳，说明 Kalman 算法的 抗噪性比较强。我们发现在第一个观测点，方差较大，说明滤波初值的选择不太 合理，但在后续的观测点上，方差又变的很小，说明 Kalman 滤波算法具有很强 的鲁棒性。
for t=0:50 x(t+611)=x(611)+vx*t+ax*t*t/2; y(t+611)=y(611)+vy*t+ay*t*t/2; end
vx=vx+ax*(660-610);% 第二次机动转弯结束时的速度 vy=vy+ay*(660-610); %%%%%% ax=0.2; ay=-0.2; %%%%%%%%%%%%%% % 660s 以后匀速运动，一直到截止时间 for t=0:(offtime-660) x(t+661)=x(661)+vx*t+ax*t*t/2; y(t+661)=y(661)+vy*t+ay*t*t/2; end
if offtime<600 error('仿真时间必须大于 600s，请重新输入'); end
x=zeros(offtime,1); y=zeros(offtime,1); X=zeros(ceil(offtime/Ts),1); Y=zeros(ceil(offtime/Ts),1);
8
% t=0:400s，速度 vx,vy 为沿 x 和 y 轴的速度分量(m/s) x0=2000;%起始点坐标 y0=10000; vx=5; vy=-15; % 沿-y 方向
xk Ak xk 1 wk 1 yk ck xk vk
假设动态噪声 wk 与观测噪声 vk 都是零均值的正态噪声，且两者互不相关。
T 这里定义 Qk var[wk ] E[wk wT k ] 为动态噪声协方差阵， Rk var[vk ] E[vk vk ] 为
量测噪声协方差阵。 卡尔曼滤波是基于最小均方误差的，经过推导，我们得到：
7
subplot(211) plot(YERB) axis([0 400 -50 50]) xlabel('观测次数') ylabel('Y 方向滤波误差均值'),grid on; subplot(212) plot(YSTD) axis([0 400 0 150]) xlabel('观测次数') ylabel('Y 方向滤波误差标准差'),grid on;
for t=1:400 x(t)=x0+vx*t; y(t)=y0+vy*t; end
% t=400:600s，ax,ay 为沿 x 和 y 轴的加速度分量(m/s/s) ax=0.075; ay=0.075;
for t=0:200 x(t+401)=x(400)+vx*t+ax*t*t/2; y(t+401)=y(400)+vy*t+ay*t*t/2; end
三、实验过程
1
在这里， 我们以某一场景予以说明。假定有一个二坐标雷达对一平面上运动 的目标进行观测， 目标在 t 0 400s 沿 y 轴作恒速直线运动， 运动速度为-15 m / s ， 目标的起始点为(2000 米,10000 米)，在 400 600 s 向 x 轴方向做 90 的慢转弯，加 速度均为 0.075 m / s 2 ，完成慢转弯后加速度将降为零，从 610s 开始做 90 的快转 弯，加速度为 0.3 m / s 2 ，在 660s 结束转弯，加速度均变为 0.2 m / s 2 ，到 800s 结 束。雷达扫描周期 T 2 秒， x 和 y 独立地进行观测，观测噪声的标准差均为 100 米，过程噪声为 5 m / s 2 目标轨迹为：