ls_c.m

这是一个利用MATLAB6.5进行航迹跟踪的程序序

function [X_Obsv_M,Y_Obsv_M,X_Filt_M,Y_Filt_M,Error_X,Error_Y,Std_X,Std_Y] = LS_C(M,d,X_Real,Y_Real)
%输入参数:观测噪声方差d,采样间隔T,蒙特卡洛次数M,真实航迹Real_X,Real_Y  
%输出结果:观测轨迹Obsv_X_M,Obsv_Y_M 滤波轨迹Filt_X_M,Filt_Y_M 
%滤波误差Error_X,Error_Y 误差的标准差:Std_X,Std_Y

%随机初始化
randn('state',sum(100*clock));
N=length(X_Real);
%每次蒙特卡罗仿真的结果
X_Obsv_temp=zeros(N,M);
Y_Obsv_temp=zeros(N,M);
X_Filt_temp=zeros(N,M);
Y_Filt_temp=zeros(N,M);

T=1;

%蒙特卡罗仿真
for mont=1:M
    
    %观测数据产生
    X_Obsv=zeros(N,1);
    Y_Obsv=zeros(N,1);
    X_Filt=zeros(N,1);
    Y_Filt=zeros(N,1);
    error=d*randn(N,2);
    for i=1:N
        X_Obsv(i)=X_Real(i)+error(i,1);
        Y_Obsv(i)=Y_Real(i)+error(i,2);
        
        X_Obsv_temp(i,mont)=X_Obsv(i);
        Y_Obsv_temp(i,mont)=Y_Obsv(i);
    end
    
    I=zeros(4,4);%单位矩阵
    I(1,1) = 1;
    I(2,2) = 1;
    I(3,3) = 1;
    I(4,4) = 1;
    
    X0=zeros(4,1);%状态矩阵
    X0(1,1) = X_Obsv(1);
    X0(2,1) = (X_Obsv(2)-X_Obsv(1))/T;
    X0(3,1) = Y_Obsv(1);
    X0(4,1) = (Y_Obsv(2)-Y_Obsv(1))/T;
    
    H=zeros(2,4);   %状态－>观测
    
    H2=zeros(2,4);  %状态->观测初始值
    H2(1,1) = 1;
    H2(1,2) = T;
    H2(2,3) = 1;
    H2(2,4) = T;
    
    Z=zeros(2,1);   %观测值
    
    R=zeros(2,2);   %观测噪声协方差阵
    R(1,1) = d*d;
    R(2,2) = d*d;
    R_temp=zeros(2,2);
    R_temp(1,1) = 1;
    R_temp(2,2) = 1;
    
    K=zeros(4,2);    %增益
    
    P=zeros(4,4);   %预测误差，滤波协方差阵
    P=pinv(H2.'*pinv(R)*H2);
    
    X=zeros(2,1);
    
    %头两个滤波值就取观测值
    X_Filt(1) = X_Obsv(1);
    X_Filt(2) = X_Obsv(2);
    Y_Filt(1) = Y_Obsv(1);
    Y_Filt(2) = Y_Obsv(2);
    
    X_Filt_temp(1,mont)=X_Filt(1);
    X_Filt_temp(2,mont)=X_Filt(2);
    Y_Filt_temp(1,mont)=Y_Filt(1);
    Y_Filt_temp(2,mont)=Y_Filt(2);
    
    %最小二乘算法运行
    for i=3:N
        
        Z(1,1) = X_Obsv(i);
        Z(2,1) = Y_Obsv(i);
        
        H(1,1) = 1;
        H(1,2) = i*T;
        H(2,3) = 1;
        H(2,4) = i*T;
        
        K = P*(H.')*pinv(H*P*(H.')+R);
        X0 = X0+K*(Z-H*X0);
        P = (I-K*H)*P;
        X = H*X0;
                
        X_Filt(i) = X(1,1);
        Y_Filt(i) = X(2,1);
        
        X_Filt_temp(i,mont)=X_Filt(i);
        Y_Filt_temp(i,mont)=Y_Filt(i);
    end	
    
end

%蒙特卡罗仿真的最终结果

X_Obsv_M=zeros(N,1);
Y_Obsv_M=zeros(N,1);
X_Filt_M=zeros(N,1);
Y_Filt_M=zeros(N,1);

%注意：观测轨迹和滤波轨迹都取最后一次的
X_Obsv_M=X_Obsv_temp(:,mont);
Y_Obsv_M=Y_Obsv_temp(:,mont);
X_Filt_M=X_Filt_temp(:,mont);
Y_Filt_M=Y_Filt_temp(:,mont);

%注意：误差是取平均值
Error_X=zeros(N,1);
Error_Y=zeros(N,1);
Error_X=X_Real-(mean(X_Filt_temp'))';
Error_Y=Y_Real-(mean(Y_Filt_temp'))';

Std_X=zeros(N,1);
Std_Y=zeros(N,1);
Std_X_temp=zeros(N,M);
Std_Y_temp=zeros(N,M);

for t=1:mont
    for i=1:N
       Std_X_temp(i,t)=(X_Real(i)-X_Filt_temp(i,t))^2;
       Std_Y_temp(i,t)=(Y_Real(i)-Y_Filt_temp(i,t))^2;
   end
end

Std_X=sqrt(mean(Std_X_temp')'-Error_X.^2);
Std_Y=sqrt(mean(Std_Y_temp')'-Error_Y.^2);