third_get_h.asv

来自「RAIM 完好性分析」· ASV 代码 · 共 24 行

function H = Third_get_H(k_gps,k_galileo,k_beidou,xs_gps,ys_gps,zs_gps,xs_galileo,ys_galileo,zs_galileo,xs_beidou,ys_beidou,zs_beidou,xu,yu,zu)
%求H矩阵
dx_gps=xs_gps-xu*ones(k_gps,1);
dy_gps=ys_gps-yu*ones(k_gps,1);
dz_gps=zs_gps-zu*ones(k_gps,1);
rou_gps=sqrt(dx_gps.^2+dy_gps.^2+dz_gps.^2);
e1_gps=dx_gps./rou_gps;e2_gps=dy_gps./rou_gps;e3_gps=dz_gps./rou_gps;

dx_galileo = xs_galileo-xu*ones(k_galileo,1);
dy_galileo = ys_galileo-yu*ones(k_galileo,1);
dz_galileo = zs_galileo-zu*ones(k_galileo,1);
rou_galileo = sqrt(dx_galileo.^2+dy_galileo.^2+dz_galileo.^2);
e1_galileo =dx_galileo./rou_galileo;e2_galileo=dy_galileo./rou_galileo;e3_galileo=dz_galileo./rou_galileo;

dx_beidou=xs_beidou-xu*ones(k_beidou,1);
dy_beidou=ys_beidou-yu*ones(k_beidou,1);
dz_beidou=zs_beidou-zu*ones(k_beidou,1);
rou_beidou=sqrt(dx_beidou.^2+dy_beidou.^2+dz_beidou.^2);
e1_beidou=dx_beidou./rou_beidou;e2_beidou=dy_beidou./rou_beidou;e3_beidou=dz_beidou./rou_beidou;

H=[e1_gps,e2_gps,e3_gps,ones(k_gps,1),zeros(k_gps,1),zeros(k_gps,1);
   e1_galileo,e2_galileo,e3_galileo,zeros(k_galileo,1),ones(k_galileo,1),zeros(k_galileo,1);
   e1_beidou,e2_beidou,e3_beidou,zeros(k_beidou,1),zeros(k_beidou,1),ones(k_beidou,1)];