geth.m

高速车辆组合导航研究

function H = GetH(X)
% get the vectro of the measurment equatin

R=148.1/3600/180*pi;
aa=1.69;
Re=6378137;%radius of earth
r = sqrt(X(1)^2+X(2)^2+X(3)^2);
H = [X(1)/(r^2-Re^0.5)^2*tan(R)-aa X(2)/(r^2-Re^2)^0.5*tan(R)-aa X(3)/(r^2-Re^2)^0.5*tan(R)-aa 0 0 0];