误差理论与测量平差实习(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
    评论
IMU和GNSS是常见的导航传感器,它们可以用于无人机、自动驾驶汽车和航空航天应用等领域。IMU可以测量加速度和角速度,而GNSS可以测量位置、速度和时间信息。由于IMU和GNSS具有互补性,将它们结合起来可以提高导航精度和鲁棒性。 为了将IMU和GNSS数据融合,需要编写代码来实现融合算法。Matlab是一种流行的科学计算软件,它具有编程和可视化功能,非常适合开发导航代码。下面是一些可能包含在IMU / GNSS组合代码中的常见步骤: 1. 整合IMU数据:首先,需要通过积分加速度和角速度信号来计算出运动状态估计量。这通常涉及使用姿态解算器(例如Mahony滤波器或互补滤波器)来计算方向估计量。 2. 处理GNSS数据:其次,需要解算卫星信号以获取位置和速度信息。这可以通过使用GNSS解算器(例如GPS或GLONASS)实现。 3. 数据融合:最后,可以利用卡尔曼滤波器或扩展卡尔曼滤波器等算法来将IMU和GNSS数据进行融合。通过选择合适的状态向量,可以将IMU和GNSS测量量结合起来,以实现更精确和鲁棒的导航解决方案。 总之,IMU / GNSS组合代码需要考虑多个因素,包括传感器的特性、算法选择、数据处理和滤波步骤等。通过使用Matlab编程来构建IMU / GNSS组合代码,可以简化实现流程并提供可视化的结果,以便更好地评估算法性能和调试代码
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值