卡尔曼滤波预测轨迹程序

直线轨迹预测卡尔曼滤波程序。程序中有小问题，可能运行时会出现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

2025

-1-0.500.51

1.5

2时间(s)

x 方向误差(m )

x 方向距离误差

10

2030

-1-0.5

00.511.5

2时间(s)

y 方向误差(m )

y 方向距离误差

图4- 1 x 方向预测误差 图4- 2 y 方向预测误差