wls+kalman.m

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

%*********************WLS+kalmanfilter联合跟踪**********************
clc;
clear;
n=150;                         %跟踪时间；
c=3*10^8;                      %光速；   
T=1;                           %测量周期；
vart1=5*10^(-9);               %测量时差噪声的方差；
varp=0.002;                    %测量角度噪声的方差； 
vx=-400;vy=100;                %目标运动速度； 
num=100;                        %蒙特卡洛迭代次数；
%产生真实轨迹；                
xr=zeros(n,1);
yr=zeros(n,1);
xr(1)=40000;
yr(1)=20000;
for i=2:n
    xr(i)=xr(i-1)+vx;          
    yr(i)=yr(i-1)+vy;          %目标的真实轨迹； 
end
%产生真实角度；
p=zeros(n,1);
for i=1:n
    p(i)=atan(xr(i)/yr(i));    %没加入噪声的目标真实角度；
end                              
%产生真实径向距离r;
r=zeros(n,1);
for i=1:n
    r(i)=(xr(i)^2+yr(i)^2)^0.5;
end
%产生真实时差t1;
t1=zeros(n-1,1);
for i=1:n-1
    t1(i)=(r(i+1)-r(i))/c;
end
%蒙特卡洛法滤波100次；
for j=1:num
    np=varp*randn(n,1);                               %产生角度观测噪声；
    nt1=vart1*randn(n-1,1);                           %产生时差观测噪声； 
    pm=zeros(n,1);                                    %观测角度存储矩阵； 
    t1m=zeros(n-1,1);                                 %观测时差存储矩阵；
    pm=p+np;                                          %产生观测角度；
    t1m=t1+nt1;                                       %产生观测时差；
    %产生观测矩阵h;
    h=zeros(n,2);
    for i=1:n
        h([i],:)=[sin(p(i)) cos(p(i))];
    end
    %产生等效观测矢量z;
    z=zeros(n-1,1);
    for i=1:n-1
        z(i)=c*t1m(i);
    end
    x=[-440 10]';                             %粗略估计目标速度；      
    peer=eye(2,2)*1000;                      %速度估计的起始协方差；
    xk=[50100 20100]';                       %初始位置估计；
    peerk=eye(2,2)*10^8;                    %初始位置估计协方差；
    %伪线性rls滤波开始；
    for i=1:n-1        
         w=(x(1)*cos(p(i))-x(2)*sin(p(i)))^2*varp^2+c^2*vart1^2;
         k=peer*h([i],:)'*inv(h([i],:)*peer*h([i],:)'+w);
        peer=(eye(2)-k*h([i],:))*peer;
        x=x+k*(z(i)-h([i],:)*x);
        %一次最小2乘滤波结束；
         xest(j,i)=(x(2)-x(1)/tan(pm(i)))/(1/tan(pm(i+1))-1/tan(pm(i)));
        yest(j,i)=(x(1)-x(2)*tan(pm(i)))/(tan(pm(i+1))-tan(pm(i)));
         xmm(j,i)=x(1);
        ymm(j,i)=x(2);
        rest(j,i)=((xest(j,i)-xr(i+1))^2+(yest(j,i)-yr(i+1))^2)^0.5;
   %卡尔曼滤波
    peers=peer*1;
     hk=[1 0
         0 1];
     o=[1 0
        0 1];
        zk=[xest(j,i) yest(j,i)]';
    xk1=o*xk+x;
    peerk1=o*peerk*o';
    kk=peerk1*hk'*inv(hk*peerk1*hk'+peers);
    xk=xk1+kk*(zk-hk*xk1);
    peerk=(eye(2)-kk*hk)*peerk1;
    xks(j,i)=xk(1,1);
    yks(j,i)=xk(2,1);
    restk(j,i)=((xks(j,i)-xr(i+1))^2+(yks(j,i)-yr(i+1))^2)^0.5;        
    end
end
%给出误差存储矩阵；
        xz=0;xz1=zeros(n,1);yz=0;yz1=zeros(n,1);
        xxz=0;xxz1=zeros(n,1);yyz=0;yyz1=zeros(n,1);
        rz=0;rz1=zeros(n,1);          %没有经过卡尔曼滤波平滑前；
        xxzk=0;xxzk1=zeros(n,1);yyzk=0;yyzk1=zeros(n,1);
        rzk=0;rzk1=zeros(n,1);          %经过卡尔曼滤波平滑后；
%计算蒙特卡洛迭代的误差均值；        
for i=1:n-1
 for j=1:num
        xz=xz+xmm(j,i)-vx;
        yz=yz+ymm(j,i)-vy;
        xxz=xxz+xest(j,i)-xr(i+1);
        yyz=yyz+yest(j,i)-yr(i+1);
        rz=rz+rest(j,i);   %卡尔曼滤波平滑前
        xxzk=xxzk+xks(j,i)-xr(i+1);
        yyzk=yyzk+yks(j,i)-yr(i+1);
        rzk=rzk+restk(j,i);   %卡尔曼滤波平滑后        
    end
    xz1(i)=xz/num;
    yz1(i)=yz/num;
    xxz1(i)=xxz/num;
    yyz1(i)=yyz/num;
    rz1(i)=rz/num;
    xxz=0;
    yyz=0;
     xz=0;
     yz=0;
     rz=0;                            %卡尔曼滤波平滑前
    xxzk1(i)=xxzk/num;
    yyzk1(i)=yyzk/num;
    rzk1(i)=rzk/num;
     xxzk=0;
     yyzk=0;
     rzk=0   
end
%绘图
figure(1);
plot(xz1);
legend('速度x误差');
figure(2);
plot(yz1);
legend('速度y误差');
figure(3);
plot(xxzk1);
legend('卡尔曼滤波平滑后x方向误差');
figure(4);
plot(yyzk1);
legend('卡尔曼滤波平滑后y方向误差');
figure(5);
plot(rzk1);
legend('卡尔曼滤波平滑后径向距离r的跟踪误差');