卡尔曼滤波 matlab仿真

随机信号处理上机作业
题目：假设有一个二坐标雷达对一平面上运动目标的进行观察，目标在t ＝0～400秒沿y 轴作恒速直线运功，运动速度为-15m/s ，目标的起点为（2000m ，10000m ），雷达扫描周期为2秒，x 和y 独立地进行观察，观察噪声的标准差均为100m 。

试建立雷达对目标的跟踪算法，并进行仿真分析，给出仿真结果，画出目标真实轨迹、对目标的观察和滤波曲线。

一、跟踪算法
考虑利用卡尔曼滤波算法对目标的运动状态进行估计。

由于目标在二维平面内做匀速运动，因此这里只考虑匀速运动情况。

1. 建立模型
由于目标沿y 轴作匀速直线运动，取状态变量
⎥⎥
⎥⎦
⎤⎢⎢⎢⎣⎡=y v y x S 状态方程：
()()k AS k S =+1 (1)
观测方程：
()()()k V k CS k Z += (2)
其中，
⎥⎥⎥⎦
⎤
⎢⎢⎢⎣⎡=10010001T A
⎥⎦⎤⎢⎣⎡=010001C
⎥⎦
⎤
⎢⎣⎡=y x z z Z ⎥⎦⎤⎢⎣⎡=y x v v V
对目标位置和速度的同时滤波与一步预测的方程组如下:
预测估计方程:
()()
11/^
^-=-k A k k S S
预测误差协方差：
()()T A
k AP k k P 11/-=-
滤波估计增益：
()()()R C k k C k k C k B T T +--=1/1/，其中
⎥⎥⎦
⎤⎢⎢⎣⎡=2200y x R σσ 滤波估计方程：
()()()()()⎥⎦⎤⎢
⎣⎡--+-=1/1//^
^
^
k k k Z k B k k k k S S S
滤波误差协方差：
()()[]()1/1/--=k k P C k B k k P
2. 初始化
利用目标的前几个测量值建立状态的其实估计,采用两点起始法。

()⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎣⎡-=∧T Z Z Z Z S y y y x )1()2()2()2(2/2
()⎥⎥
⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎢⎣
⎡=T T T P y y y y
x 2222
220000
2/2σσσσσ
滤波误差均值：
()k k S k S N
k e I
N
i i
x /)(1
)(1
∧
=-=
∑
滤波误差标准差：
()22
1)(/)(1k e k k S k S N
x N
i I i y
-⎥⎦⎤
⎢⎣
⎡-=
∑=∧
∧
σ
二. 仿真分析
利用MATLAB 对前面建立的模型进行仿真,结果如下。

图 2.1
图 2.1 是目标运动的真实轨迹和观测轨迹曲线。

其中，真实轨迹显示目标在x=2000米处沿y 轴方向做匀速直线运动，而观测轨迹是目标运动的真实轨迹加上方差和随机测量噪声得到的。

从图中可以看出，观测轨迹围绕真实轨迹作上下浮动。

图 2.2
图2.2是单次滤波和100次滤波后的数据曲线。

从图中可以看出，滤波刚开始时误差较大，之后滤波误差逐渐降低，估计值逐步逼近真实轨迹。

而随着滤波次数增加，滤波后的结果更为接近真实轨迹。

图 2.3
图 2.4
图2.3,图2.4分别是x和y方向滤波估计误差均值及误差标准差曲线。

从图上可以看出，滤波开始时误差较大，随着采样次数的增加，误差逐渐减小，误差的标准差也具有相同特性。

另外,可以看到由于在y方向上有速度分量,因此y方向的估计误差均值比x方向的估计误差均值波动要大一些。

%仿真场景
sigma=10000;
T=2;
t=200;
Vy=-15;
C=[1 0 0;0 1 0];
A=[1 0 0;0 1 T;0 0 1];
eSk(:,t)=[0 0 0]';eSz(:,t)=[0 0 0]';eeSz(:,t)=[0 0]';
N=100;%蒙特卡洛次数
for i=1:N
for j=1:t
Zk(:,j)=[2000+wgn(1,1,40);10000+Vy*T*(j-1)+wgn(1,1,40)];
end
for j=1:200
if j==1
Sk(:,1)=[Zk(1,1),Zk(2,1),0]';Sk1(:,1)=Sk(:,1);
Sk(:,2)=[Zk(1,2),Zk(2,2),(Zk(2,2)-Zk(2,1))/T]';Sk1(:,2)=Sk(:,2);
Pk=[sigma,0,0;0,sigma,sigma/T;0,sigma/T,2*sigma/T];
else
if j>2
Sk1(:,j)=A*Sk(:,j-1);%预测
Pk1=A*Pk*A';%预测误差协方差
Bk=Pk1*C'*inv(C*Pk1*C'+sigma*eye(2));%kalman增益
Sk(:,j)=Sk1(:,j)+Bk*(Zk(:,j)-C*Sk1(:,j));%滤波
Pk=(eye(3)-Bk*C)*Pk1;%滤波协方差
end
end
%%
%1000次求平均
eSk(:,j)=eSk(:,j)+Sk(:,j)/N;%滤波
eSz(:,j)=eSz(:,j)+([2000;10000+Vy*(j-1)*T;0]-Sk(:,j))/N;%滤波误差均值
eeSz(:,j)=eeSz(:,j)+[(2000-Sk(1,j))^2;(10000+Vy*(j-1)*T-Sk(2,j))^2]/N;%滤波误差标准差end
end
%绘图
%真实轨迹和测量轨迹
subplot(2,1,1);j=0:0.1:t;plot(2000,10000+Vy*(j-1)*T);
title('目标真实轨迹');xlabel('X(米)');ylabel('Y(米)');
subplot(2,1,2);plot(Zk(1,:),Zk(2,:));
title('测量轨迹');xlabel('X(米)');ylabel('Y(米)');
%滤波单次仿真和蒙特卡洛仿真
figure;
subplot(2,1,1);
plot(Sk(1,:),Sk(2,:));
title('单次滤波数据曲线');xlabel('X(米)');ylabel('Y(米)'); subplot(2,1,2);
plot(eSk(1,:),eSk(2,:));title('100次滤波数据曲线');
xlabel('X(米)');ylabel('Y(米)');
j=1:t;
figure;subplot(211);plot(j,eSz(1,:));title('X滤波误差均值曲线'); xlabel('采样次数');ylabel('X(米)');
subplot(212);
for j=1:t
eeSz(1,j)=sqrt(eeSz(1,j)-eSz(1,j)^2);
eeSz(2,j)=sqrt(eeSz(2,j)-eSz(2,j)^2);
end
j=1:t;
plot(j,eeSz(1,:));title('x滤波误差标准差曲线');
xlabel('采样次数');ylabel('X(米)');
figure;subplot(211);plot(j,eSz(2,:));
title('y滤波误差均值曲线');xlabel('采样次数');ylabel('Y(米)'); subplot(212);plot(j,eeSz(2,:));title('y滤波误差标准差曲线'); xlabel('采样次数');ylabel('Y(米/秒)');
figure;subplot(211);plot(j,Sk(3,:));title('单次滤波速度估计'); xlabel('采样次数');ylabel('Y(米/秒)');
subplot(212);plot(j,eSk(3,:));title('100次滤波速度估计'); xlabel('采样次数');ylabel('Y(米/秒)');。