ekfplsueo.m

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

%*********伪线性算法和EKF直接递推相结合后的改进算法****************************；
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;             %目标速度；
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);
dr=zeros(n-1,1);
for i=1:n-1
    t1(i)=(r(i+1)-r(i))+c*1;
    dr(i)=r(i+1)-r(i);
end          %产生真实时间差；
for j=1:num
%************产生观测角度************；
dt=ot1*randn(n-1,1);
ndr=zeros(n-1,1);
ndr=dr+dt;
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 310 100 1.1]';     %初始估计值；
    peer=10^10*eye(5,5);               %初始估计协方差；
%*********观测矩阵和误差协方差迭代************
for i=1:n-1
 if(i<30)
    hk=[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*hk'*inv(hk*peer*hk'+vr);
 X=X+k*(z(:,[i])-hk*X);
 peer=(eye(5,5)-k*hk)*peer;
 %*********一次滤波结束，存储误差**********
 rm(j,i)=((X(1,1)+X(3,1)*i-x(i))^2+(X(2,1)+i*X(4,1)-y(i))^2)^0.5;
else
    if(i==30)
    po=[peer(1,1) peer(1,2) peer(1,3) peer(1,4)
       peer(2,1) peer(2,2) peer(2,3) peer(2,4)
       peer(3,1) peer(3,2) peer(3,3) peer(3,4)
       peer(4,1) peer(4,2) peer(4,3) peer(4,4)];
    XM=[(X(1,1)+X(3,1)*i) (X(2,1)+i*X(4,1)) X(3,1) X(4,1)]';  
    o=[1 0 1 0
       0 1 0 1
       0 0 1 0
       0 0 0 1];
  R=[4*10^(-6) 0
    0     2.25];
end
X1=o*XM;
p2=o*po*o';
h=zeros(2,4);
r1=(X1(1,1)^2+X1(2,1)^2)^0.5;
r2=((X1(1,1)-X1(3,1))^2+(X1(2,1)-X1(4,1))^2)^0.5;
h(1,1)=X1(2,1)/r1^2;
h(1,2)=-X1(1,1)/r1^2;
h(2,1)=X1(1,1)/r1-(X1(1,1)-X1(3,1))/r2;
h(2,2)=X1(2,1)/r1-(X1(2,1)-X1(4,1))/r2;
h(2,3)=(X1(1,1)-X1(3,1))/r2;
h(2,4)=(X1(2,1)-X1(4,1))/r2;
po=inv(inv(p2)+h'*inv(R)*h);
km=po*h'*inv(R);
zm=[pn(i+1) ndr(i)]';
hx=[atan(X1(1,1)/X1(2,1)) (r1-r2)]';
XM=X1+km*[zm-hx];

%*********一次滤波结束，存储误差**********
 rm(j,i)=((XM(1,1)-x(i+1))^2+(XM(2,1)-y(i+1))^2)^0.5;
 
end
 end
end
%***************计算迭代滤波误差*****************
er1=0;
         %速度滤波误差存储矩阵；
er11=zeros(n-1,1);           
for i=1:n-1
    for j=1:num
        er1=er1+rm(j,i);
    end
    er11(i)=er1/num;
    er1=0;
end
%****************画图*******************
figure(5);
plot(er11);
legend('改进后算法的径向距离误差');