|
1
|
%扩展Kalman滤波在目标跟踪中的应用
|
|
2
|
%functionEKF_For_TargetTracking
|
|
3
|
clc;clear;
|
|
4
|
T=1;%雷达扫描周期
|
|
5
|
N=60/T;%总的采样次数
|
|
6
|
X=zeros(4,N);%目标真实位置、速度
|
|
7
|
X(:,1)=[-100,2,200,20];%目标初始位置、速度
|
|
8
|
Z=zeros(1,N);%传感器对位置的观测
|
|
9
|
delta_w=1e-3;%如果增大这个参数，目标的真实轨迹就是曲线了
|
|
10
|
Q=delta_w*diag([0.5,1]);%过程噪声方差
|
|
11
|
G=[T^2/2,0;T,0;0,T^2/2;0,T];%过程噪声驱动矩阵
|
|
12
|
R=5;%观测噪声方差
|
|
13
|
F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];%状态转移矩阵
|
|
14
|
x0=200;%观测站的位置
|
|
15
|
y0=300;
|
|
16
|
Xstation=[x0,y0];
|
|
17
|
for
t=2:N
|
|
18
|
X(:,t)=F*X(:,t-1)+G*sqrtm(Q)*randn(2,1);%目标真实轨迹
|
|
19
|
end
|
|
20
|
|
|
21
|
for t=1:N
|
|
22
|
Z(t)=Dist(X(:,t),Xstation)+sqrtm(R)*randn;%观测值
|
|
23
|
end
|
|
24
|
%EKF
|
|
25
|
Xekf=zeros(4,N);
|
|
26
|
Xekf(:,1)=X(:,1);%Kalman滤波的状态初始化
|
|
27
|
P0=eye(4);%误差协方差矩阵的初始化
|
|
28
|
for
i=2:N
|
|
29
|
Xn=F*Xekf(:,i-1);%一步预测
|
|
30
|
P1=F*P0*F'+G*Q*G';%预测误差协方差
|
|
31
|
dd=Dist(Xn,Xstation);%观测预测
|
|
32
|
%求解雅克比矩阵H
|
|
33
|
H=[(Xn(1,1)-x0)/dd,0,(Xn(3,1)-y0)/dd,0];%泰勒展开的一阶近似
|
|
34
|
K=P1*H'*inv(H*P1*H'+R);%卡尔曼最优增益
|
|
35
|
Xekf(:,i)=Xn+K*(Z(:,i)-dd);%状态更新
|
|
36
|
P0=(eye(4)-K*H)*P1;%滤波误差协方差更新
|
|
37
|
end
|
|
38
|
%误差分析
|
|
39
|
for
i=1:N
|
|
40
|
Err_KalmanFilter(i)=Dist(X(:,i),Xekf(:,i));%
|
|
41
|
end
|
|
42
|
%画图
|
|
43
|
figure
|
|
44
|
hold on;boxon;
|
|
45
|
plot(X(1,:),X(3,:),'-k');%真实轨迹
|
|
46
|
plot(Xekf(1,:),Xekf(3,:),'-r');%扩展Kalman滤波轨迹
|
|
47
|
legend('real
trajectory','EKFtrajectory');
|
|
48
|
xlabel('X-axis X/m');
|
|
49
|
ylabel('Y-axisY/m');
|
|
50
|
|
|
51
|
figure
|
|
52
|
hold on
;boxon;
|
|
53
|
plot(Err_KalmanFilter,'-ks','MarkerFace','r')
|
|
54
|
xlabel('Time/s');
|
|
55
|
ylabel('Positionestimation
bias /m');
|
|
56
|
|
|
57
|
%子函数求欧氏距离
|
|
58
|
function d=Dist(X1,X2);
|
|
59
|
if
length(X2)<=2
|
|
60
|
d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(2))^2);
|
|
61
|
else
|
|
62
|
d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(3))^2);
|
|
63
|
end
|
|
64
|
|
|
65
|
%子函数求欧氏距离
|
|
66
|
function d=Dist(X1,X2);
|
|
67
|
if
length(X2)<=2
|
|
68
|
d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(2))^2);
|
|
69
|
else
|
|
70
|
d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(3))^2);
|
|
71
|
end
|
|
72
|
|
|
73
|
|
|
74
|
|
|
75
|
|
|
76
|
|
|
77
|
|
|
78
|
|
|
