close all;
clear all;
T=1;
N=60;%跟踪步数
X=zeros(4,N);
X(:,1)=[-100,2,200,20];%初始状态值
Z=zeros(1,N);
delta_w=1e-3;
Q=delta_w*diag([0.5,1]);
G=[0.5,0;1,0;0,0.5;0,1];
R=5;
F=[1,1,0,0;0,1,0,0;0,0,1,1;0,0,0,1];
x0=200;
y0=300;
Xstation=[x0,y0];%雷达基站的位置
v=sqrt(R)*randn(1,N);
for t=2:N
X(:,t)=F*X(:,t-1)+G*sqrt(Q)*randn(2,1);%目标真实轨迹
end
for t=1:N
Z(t)=Dist(X(:,t),Xstation)+v(t);%目标观测
end
alp=0.01;
beta=2;
n=4;
k=n*(alp^2-1);
lamda=alp^2*(n+k)-n;
for j=1:2*n+1;
Wm(j)=0.5/(lamda+n);
Wc(j)=0.5/(lamda+n);
end
Wm(1)=lamda/(n+lamda);
Wc(1)=lamda/(n+lamda)+1-alp^2+beta;
Xukf=zeros(4,N);%经过UKF滤波后的状态值
Xukf(:,1)=X(:,1);
P0=eye(4);%协方差初始值
for t=2:N
cho=(chol((P0*(n+lamda))))'
xestimate=Xukf(:,t-1);
for k=1:n
sigma1(:,k)=xestimate+cho(:,k);
sigma2(:,k)=xestimate-cho(:,k);
end
sigma=[xestimate,sigma1,sigma2];%对量测值采样2*n+1次
sigmapred=F*sigma;%对采样点的一步预测
Xpred=zeros(4,1);%定义X的一步预测值
for k=1:2*n+1
Xpred=Xpred+Wm(k)*sigmapred(:,k);
end
Ppred=zeros(4,4);
for k=1:2*n+1
Ppred=Ppred+Wc(k)*(sigmapred(:,k)-Xpred)*(sigmapred(:,k)-Xpred)';
end
Ppred=Ppred+G*Q*G';
for k=1:2*n+1
gamma(1,k)=h(sigmapred(:,k),Xstation);
end
Zpred=0;
for k=1:2*n+1
Zpred=Zpred+Wm(k)*gamma(1,k);
end
Pzz=0;
for k=1:2*n+1
Pzz=Wc(k)*(gamma(1,k)-Zpred)*(gamma(1,k)-Zpred)'+Pzz;
end
Pzz=Pzz+R;
Pxz=zeros(4,1);
for k=1:2*n+1
Pxz=Wc(k)*(sigmapred(:,k)-Xpred)*(gamma(1,k)-Zpred)'+Pxz;
end
P=zeros(4,4);
K=Pxz*inv(Pzz);
xestimate=Xpred+K*(Z(t)-Zpred);
P=Ppred-K*Pzz*K';
Xukf(:,t)=xestimate;
end
for i=1:N
error(i)=Dist(X(:,i),Xukf(:,i));
end
figure(1);
plot(X(1,:),X(3,:),'k',Xukf(1,:),Xukf(3,:),'--k');
legend('真实轨迹','UKF轨迹');
xlabel('x坐标/m');
ylabel('y坐标/m');
figure(2);
t=1:N;
plot(t,error(t),'k');
xlabel('时间/s');
ylabel('位置估计偏差RMS');
function d=Dist(X1,X2)
if length(X2)<=2
d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(2))^2);
else
d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(3))^2);
end
end
function y=h(x,xx)
y=sqrt((x(1)-xx(1))^2+(x(3)-xx(2))^2);
end
评论0