untitled2.asv

序惯最小二乘法

clear
clc
%*******************************初始化变量*************
K_num=30;
TM=60;
P0=100*eye(4,4);
beta0=zeros(4,1);%初始系统偏差为零
beta1=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=1;
detar_b=1;
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.05;
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_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_range_a=Exact_range_a;
% measure_azimuth_a=Exact_azimuth_a;
% measure_range_b=Exact_range_b;
% measure_azimuth_b=Exact_azimuth_b;
% measure_range_a=Exact_range_a+beta(1,1);
% measure_azimuth_a=Exact_azimuth_a+beta(2,1);
% measure_range_b=Exact_range_b+beta(3,1);
% measure_azimuth_b=Exact_azimuth_b+beta(4,1);
% measure_maxtri=zeros(4,K_num);
measure_maxtri=[measure_range_a;measure_azimuth_a;measure_range_b;measure_azimuth_b];
Zk=-ZZk(measure_maxtri);%%%%%%%%%%?

k=1;
Gk=GGk(k,measure_maxtri);
Sigamk=Gk*C*Gk';
Kk=P0*Gk'/(Sigamk+Gk*P0*Gk');
S1=Zk(:,k)-Gk*beta0;
beta=beta0+Kk*S1;
P1=(1-Kk*Gk)*P0;
P1=P0;
beta0=beta;