卡尔曼滤波预测轨迹程序

直线轨迹预测卡尔曼滤波程序。

程序中有小问题，可能运行时会出现error。

多运行几次就会出现很好的结果。

clc;clear all;
%% 用户轨迹
error=zeros(1,100);
Tmax=0;
for cishu=1:100
T=0;
x=[0];
y=[0];
range_CDMA=[-100, 100 % 用户移动坐标范围
-100, 100];
range_WLAN=[-50, 50
-50, 50];
for i=1:10
xx(i)=randi(range_CDMA(1,:),1,1);
yy(i)=randi(range_CDMA(2,:),1,1);
end
start=randi([1,10],1,1);
terminal=randi([1,10],1,1);
while (terminal==start)
terminal=randi([1,10],1,1);
end
T=fix(sqrt((xx(terminal)-xx(start))^2+(yy(terminal)-yy(start))^2)/3);%用户移动速度3m/s
v_x=(xx(terminal)-xx(start))/T;
v_y=(yy(terminal)-yy(start))/T;
if T>Tmax
Tmax=T;
end
x(1)=xx(start);
y(1)=yy(start);
for t=2:T
x(t)=x(t-1)+v_x;
y(t)=y(t-1)+v_y;
end
x=x';
y=y';
for i=1:10
slop(i)=(xx(i)-xx(start))/(yy(i)-yy(start));
end
%% 卡尔曼滤波
xk_s=0; %赋初值
yk_s=0;
for m=1:100
I=[1,0,0,0;0,1,0,0;0,0,1,0;0,0,0,1];
n_x=randn(T,1);
n_y=randn(T,1);
z_x=x+n_x;
z_y=y+n_y; %(z_x,z_y)为观测样本值=真值+噪声
xk_s(1)=z_x(1); %赋初值
yk_s(1)=z_y(1);
xk_s(2)=z_x(2);
yk_s(2)=z_y(2);
Ak=[1,T,0,0;
0,1,0,0;
0,0,1,T;
0,0,0,1];%状态变量之间的增益矩阵Ak
Ck=[1 0 0 0;0 0 1 0];
Rk=[1 0;0 1];
var=1;
Pk=[var^2 var^2/T 0 0
var^2/T 2*var^2/(T^2) 0 0
0 0 var^2 var^2/T
0 0 var^2/T 2*var^2/(T^2)];%噪声的均方误差阵
vx=(z_x(2)-z_x(1))/T;
vy=(z_y(2)-z_y(1))/T;
xk=[z_x(1);vx;z_y(1);vy]; %将距离和速度做为估计量%%%%%%%%%%%%%%%%%%%%%%%%Kalman滤波开始,估计循环for r=3:T
yk=[z_x(r);z_y(r)];
Pk1=Ak*Pk*Ak';%(未考虑噪声)k时刻滤波的均方误差矩阵
Hk=Pk1*Ck'*inv(Ck*Pk1*Ck'+Rk); %增益方程
xk=Ak*xk+Hk*(yk-Ck*Ak*xk); %递推公式
Pk=((I-Hk*Ck)*Pk1);%滤波后的均方误差矩阵
xk_s(r)=xk(1,1);
yk_s(r)=xk(3,1); %(xk_s,yk_s)为估计值
temp_x(m,r)=xk_s(r);
temp_y(m,r)=yk_s(r);
end
end
for k=1:T
asd(k)=k;
slop_kalman=0;
for i=1:k
slop_kalman=(slop_kalman*(i-1)+(xk_s(i)-xx(start))/(yk_s(i)-yy(start)))/i; end
for i=1:10
slop_diff(i)=abs(slop_kalman-slop(i));
end
minslop_diff=min(slop_diff);
minslop=find(slop_diff==minslop_diff);
if minslop~=terminal
error(k)=error(k)+0.98;
end
end
end
error1=error(1:10:Tmax);
xerror=xk_s-x';
yerror=yk_s-y';
for i=1:length(error1);
asd1(i)=i;
end
figure;
plot(x,y,'r-',z_x,z_y,'g:',xk_s,yk_s,'b-.');
legend('真实轨迹','观测样本','估计轨迹');
figure;
plot (asd1,1-error1/100);
xlabel('选取点数');
ylabel('定位精度');
legend('预测精度')
figure;
plot(1:T,xerror);
figure;
plot(1:T,yerror);
结果图
2530354045
50556065
-60
-50-40-30-20-10
01020x 轴坐标
y 轴坐标
5
10
15
20
25
-1-0.500.51
1.5
2时间(s)x 方向误差(m
)
10
20
30
-1-0.500.511.5
2时间(s)
y 方向误差(m
)
图4- 1 x 方向预测误差 图4- 2 y 方向预测误差。