tls.m

目标跟踪中的全局最小二乘算法

close all;
clear all;
clc;



cycle_total = 1;%蒙德卡诺次数
p_norm_sque = 10e3:20e3:110e3;

for p_norm = p_norm_sque
    p = p_norm.*[0,1].';%目标起始位置
    x_ob_s = [5e1,0].';%观测飞机的速度
    T = 0.02;%采样时间间隔
    N = 100/T;
%     point = 2/T:2/T:N;
    point = N/2;
    t_1 = 1:1:N;
    x_ob_start = [-1*x_ob_s(1)*T*((N-2)/2+0.5),0].';%观测飞机的起始位置
    x_ob_p = x_ob_start*ones(1,length(t_1)) + x_ob_s*t_1*T;%观测飞机的轨迹
%     figure;
%     plot(x_ob_p(1,:),x_ob_p(2,:),'p');
    x_ta_p = p * ones(1,length(t_1));%目标的位置
    angle_ture = acot((x_ta_p(1,:) - x_ob_p(1,:))./((x_ta_p(2,:) - x_ob_p(2,:))));%真实目标和观测飞机之间的方向
    sigma = 1.*pi./180;%测向误差方差
    for cycle=1:1:cycle_total
        kk=1;
        
        for point_temp=point%采样的点数   
            
            angle_1 = angle_ture(N/2 - 0.5*point_temp+1:N/2 + 0.5*point_temp) + randn(1,point_temp).*sigma;%加入高斯白噪声误差，rms为1度
            %tls算法
            A = [sin(angle_1.'),-cos(angle_1.')];
            b = sin(angle_1.').*x_ob_p(1,N/2 - 0.5*point_temp+1:N/2 + 0.5*point_temp).'-cos(angle_1.').*x_ob_p(2,N/2 - 0.5*point_temp+1:N/2 + 0.5*point_temp).';
            t=cputime;
            %tls
            c = [A b];
            [U,S,V] = svd(c);
            cputime-t
            p_tls = -1.*[V(1,3);V(2,3)]./V(3,3);
            x_tls_mean(kk,cycle) = norm(p_tls) ;
            
            t=cputime;
            %ple
            p_ple = inv(A.'*A)*A.'*b;
            x_ple_mean(kk,cycle) = norm(p_ple) ;
            kk= kk + 1;
            cputime-t
        end
        
    end
    p_tls_norm=100.*abs(mean(x_tls_mean,2)-norm(p).*ones(length(point),1))./norm(p);
    p_ple_norm=100.*abs(mean(x_ple_mean,2)-norm(p).*ones(length(point),1))./norm(p);

    figure;
    plot(point.*x_ob_s(1).*T,p_tls_norm,'r*-');
    hold on;
    plot(point.*x_ob_s(1).*T,p_ple_norm,'b-.');
    title('估计偏差的范数与真实距离的范数的百分比');
    xlabel('观测者的距离（m）');
    ylabel('估计偏差的范数与真实距离的范数的百分比（%）');
    legend('tls估计偏差的范数','ple估计偏差的范数');
    axis([x_ob_s(1)*point(1)*T point(length(point)).*x_ob_s(1).*T 0 100]);
end