untitled2.m

序惯最小二乘法

clear
clc
%*******************************初始化变量*************
K_num=30;
TM=1;
P0=100*eye(4,4);
beta0=zeros(4,1);%初始系统偏差为零
beta12=zeros(4,1);
x=zeros(1,K_num);
y=zeros(1,K_num);
Exact_range_a=zeros(1,K_num);
Exact_azimuth_a=zeros(1,K_num);
Exact_range_b=zeros(1,K_num);
Exact_azimuth_b=zeros(1,K_num);
C=zeros(4,4);%噪声方差矩阵
detar_a=0.8;
detar_b=0.7;
detast_a=0.02;
detast_b=-0.02;
beta=[detar_a detast_a detar_b detast_b]';%统偏差
% beta=zeros(6,1);
for k=1:K_num
    x(k)=20+k;
    y(k)=30+5*sin(0.05*k);
    z(k)=15;
end
sigma_range=0.02;
sigma_azimuth=0.02;

C=diag([sigma_range^2 sigma_azimuth^2  sigma_range^2 sigma_azimuth^2 ]);
%****************************结束***********************************


polar_coordinate=getpolar(x,y,K_num);  %这个肯定没错


%*************得到初始准确值***********
Exact_range_a=polar_coordinate(1,:);
Exact_azimuth_a=polar_coordinate(2,:);
Exact_range_b=polar_coordinate(3,:);
Exact_azimuth_b=polar_coordinate(4,:);
%*********************结束*******************
%
%*********************初始化带有噪声的量测值
measure_range_a=zeros(1,K_num);
measure_azimuth_a=zeros(1,K_num);
measure_range_b=zeros(1,K_num);
measure_azimuth_b=zeros(1,K_num);

%**********************结束*******************\
%******************************伪测量******
measure_range_a1=Exact_range_a+beta(1,1);
measure_azimuth_a1=Exact_azimuth_a+beta(2,1);
measure_range_b1=Exact_range_b+beta(3,1);
measure_azimuth_b1=Exact_azimuth_b+beta(4,1);
measure_maxtri1=[measure_range_a1;measure_azimuth_a1;measure_range_b1;measure_azimuth_b1];
%******************************************结束******************************

  %for j=1:TM
   %****************************真实测量****************************
        measure_range_a=Exact_range_a+beta(1,1)+sigma_range*randn(1,K_num);
        measure_azimuth_a=Exact_azimuth_a+beta(2,1)+sigma_azimuth*randn(1,K_num);
        measure_range_b=Exact_range_b+beta(3,1)+sigma_range*randn(1,K_num);
        measure_azimuth_b=Exact_azimuth_b+beta(4,1)+sigma_azimuth*randn(1,K_num);
   %*****************************结束****************************************
        measure_maxtri=[measure_range_a;measure_azimuth_a;measure_range_b;measure_azimuth_b];
        Zk1=ZZk(measure_maxtri);%%%%%%%%%%
        measure=measure_maxtri-measure_maxtri1;
        for k=1:10
        Gk=GGk(k,measure_maxtri);
        %Zk=Gk*(beta+measure(:,k));
        Sigamk=Gk*C*Gk';
        Kk=P0*Gk'/(Sigamk+Gk*P0*Gk');
        S1=Zk1(:,k)-Gk*beta0;
        beta1=beta0+Kk*S1;
        P1=(1-Kk*Gk)*P0;
        beta1=beta12+beta1/TM;
        beta12=beta1;
       % end
        P0=P1;
       beta0=beta1;
end