误差理论与测量平差实习(GNSS定位代码)matlab

代码

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('单位权中误差图')
    

运行结果

自己去跑

  • 3
    点赞
  • 4
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值