代码
clear
clc
format long g
x_0=-2171498.04; %x_0初始值
y_0=4385365.659; %y_0初始值
z_0=4077062.179; %z_0初始值
t_0=0; %t_0初始值
pos=[-2171498.04,4385365.659,4077062.179,1]; %初始值x_0,y_0,z_0矩阵
load('Satpos.mat') %引入卫星位置文件
load('Dis.mat') %引入伪距观测值文件
cout=0;
D_x=[];
D_y=[];
D_z=[];
D_t=[];
D_dis=[];
P=[];
danweiquanzhongwucha=[];
for i=1:350
B = zeros(8,4);
f_x0=zeros(8,1);
l=zeros(8,1);
x=zeros(4,1);
V=zeros(8,1);
N=zeros(4,4);
for j=1:8
x_s=Satpos(cout+j,1);
y_s=Satpos(cout+j,2);
z_s=Satpos(cout+j,3);
f_x0(j,1)=sqrt((x_s-x_0)^2+(y_s-y_0)^2+(z_s-z_0)^2); %泰勒展开常数项
B(j,1)=(-(x_s-x_0)/sqrt((x_s-x_0).^2+(y_s-y_0).^2+(z_s-z_0).^2));
B(j,2)=(-(y_s-y_0)/sqrt((x_s-x_0).^2+(y_s-y_0).^2+(z_s-z_0).^2));
B(j,3)=(-(z_s-z_0)/sqrt((x_s-x_0).^2+(y_s-y_0).^2+(z_s-z_0).^2));
B(j,4)=1;
end
l=f_x0'-Dis(i,1:8);
N=inv(B'*B);
x=N*B'*l';
P(:,i)=x;
V=B*x-l';
Delta_0=sqrt((V'*V)/4);
danweiquanzhongwucha(i)=Delta_0;
Q_xx=inv(N);
p_x=1/Q_xx(1,1);
p_y=1/Q_xx(2,2);
p_z=1/Q_xx(3,3);
p_t=1/Q_xx(4,4);
Delta_x=sqrt(Delta_0^2/p_x);
Delta_y=sqrt(Delta_0^2/p_y);
Delta_z=sqrt(Delta_0^2/p_z);
Delta_t=sqrt(Delta_0^2/p_t);
Delta_dis=Delta_0;
D_x(i)=Delta_x;
D_y(i)=Delta_y;
D_z(i)=Delta_z;
D_t(i)=Delta_t;
D_dis(i)=Delta_dis;
B = B*0;
f_x0=f_x0*0;
l=l*0;
x=x*0;
V=V*0;
N=N*0;
cout=cout+8;
end
x=1:1:350;
figure(1)
plot(x,D_x,'-*b')
legend('x中误差')
xlabel('历元')
ylabel('中误差')
title('x中误差图')
figure(2)
plot(x,D_y,'-or')
legend('y中误差')
xlabel('历元')
ylabel('中误差')
title('y中误差图')
figure(3)
plot(x,D_z,'-or')
legend('z中误差')
xlabel('历元')
ylabel('中误差')
title('z中误差图')
figure(4)
plot(x,D_t,'-or')
legend('t中误差')
xlabel('历元')
ylabel('中误差')
title('t中误差图')
figure(5)
plot(x,D_dis,'-or')
legend('dis中误差')
xlabel('历元')
ylabel('中误差')
title('dis中误差图')
heng=P(1,:);
zong=P(2,:);
gao=P(3,:);
figure(6)
plot3(heng,zong,gao)
xlabel('x')
ylabel('y')
zlabel('z')
title('卫星位置立体图')
figure(7)
plot(x,danweiquanzhongwucha,'-or')
legend('单位权中误差')
xlabel('历元')
ylabel('中误差')
title('单位权中误差图')
运行结果
自己去跑