weixianxing.m

这是我师兄做的空间谱估计的内容。这只是一小部分程序

%*********伪线性滤波在单站无源定位中的应用****************************；
clc;
clear;
T=1;               %雷达扫描周期；
c=3*10^8;          %光速；
op=0.002;          %观测角度噪声方差；
ot1=5*10^(-9)*c;    %观测时间差噪声方差；
num=100;           %蒙特卡洛滤波次数；
%***************产生真实轨迹****************；
n=150;             %跟踪时间；
x=zeros(n,1);y=zeros(n,1);
x(1)=40000;
y(1)=10000;         %目标初始位置；
vx=-400;
vy=100;             %目标速度；
a1=randn(n,1);
a2=randn(n,1);  %加速度扰动；
for i=1:n-1
    x(i+1)=x(i)+vx*T;
    y(i+1)=y(i)+vy*T;
end                 %产生真实轨迹；
%***********产生真实角度*****************;
p=zeros(n,1);
for i=1:n
    p(i)=atan(x(i)/y(i));
end                 
%***********产生真实时间差***************；
r=zeros(n,1);
for i=1:n
    r(i)=(x(i)^2+y(i)^2)^0.5;
end         %产生真实径向距离；
t1=zeros(n-1,1);
for i=1:n-1
    t1(i)=(r(i+1)-r(i))+c*1;
end          %产生真实时间差；
for j=1:num
%************产生观测角度************；
np=zeros(n,1);
np=op*randn(n,1);             %产生观测角度噪声；
pn=zeros(n,1);
pn=p+np;                      %产生观测角度；
%************产生观测时间差************；
nt1=zeros(n-1,1);
nt1=ot1*randn(n-1,1);
t1n=t1+nt1;        %产生观测时间差；
%****************产生等效观测矢量**************
z=zeros(2,n-1);
for i=1:n-1
    z(:,[i])=[0 t1n(i)]';
end
%****************蒙特卡洛法滤波开始************
    X=[60000 20000 -410 120 1.1]';     %初始估计值；
    peer=10^10*eye(5,5);               %初始估计协方差；
%*********观测矩阵和误差协方差迭代************
for i=1:n-1
    h=[cos(pn(i+1)) -sin(pn(i+1)) (i)*X(5,1)*cos(pn(i+1))              -(i)*X(5,1)*sin(pn(i+1))             0
        0            0            X(5,1)*(sin(pn(i))+sin(pn(i+1)))/2   X(5,1)*(cos(pn(i))+cos(pn(i+1)))/2   c];
 A=X(1,1)*sin(pn(i+1))+(i+1)*X(5,1)*X(3,1)*sin(pn(i+1))+X(2,1)*cos(pn(i+1))+(i+1)*X(5,1)*X(4,1)*cos(pn(i+1));
 B=X(5,1)*(X(3,1)*cos(pn(i+1))-X(4,1)*sin(pn(i+1)))/2;
 C=X(5,1)*(X(3,1)*cos(pn(i))-X(4,1)*sin(pn(i)))/2;
 vr=[A^2*op^2*10  -A*B*op^2
     -A*B*op^2  (B^2+C^2)*op^2+ot1^2];
 %**********rls滤波开始**********
 k=peer*h'*inv(h*peer*h'+vr+eye(2,2)*0);
 X=X+k*(z(:,[i])-h*X);
 peer=(eye(5,5)-k*h)*peer;
 %*********一次滤波结束，存储误差**********
 r1(j,i)=((X(1,1)+X(3,1)*i-x(i))^2+(X(2,1)+i*X(4,1)-y(i))^2)^0.5;
 rex(j,i)=X(1,1);
 rey(j,i)=X(2,1);
 revx(j,i)=X(3,1);
 revy(j,i)=X(4,1);
 end
end
%***************计算迭代滤波误差*****************
ex=0;ey=0;
evx=0;evy=0;
er1=0;
ex1=zeros(n-1,1);
ey1=zeros(n-1,1);           %位置滤波误差存储矩阵；
evx1=zeros(n-1,1);            
evy1=zeros(n-1,1);          %速度滤波误差存储矩阵；
er11=zeros(n-1,1);           
for i=1:n-1
    for j=1:num
    ex=ex+x(1)-rex(j,i);
    ey=ey+y(1)-rey(j,i);
    evx=evx+vx-revx(j,i);
    evy=evy+vy-revy(j,i);
    er1=er1+r1(j,i);
    end
  ex1(i)=ex/num;
  ey1(i)=ey/num;
  evx1(i)=evx/num;
  evy1(i)=evy/num;
  er11(i)=er1/num;
  ex=0;ey=0;
  evx=0;evy=0;
  er1=0;
end
%****************画图*******************
figure(1);
plot(ex1);
legend('初始位置x的误差');
figure(2);
plot(ey1);
legend('初始位置y的误差');
figure(3);
plot(evx1);
legend('速度vx误差');
figure(4);
plot(evy1);
legend('速度vy误差');
figure(5);
plot(er11);
legend('径向距离误差pseudo');