getz.asv

高速车辆组合导航研究

function [Z,truervector,truevvector] = GetZ(X0,T,tf,Q)
% get the true measurment value
g=3.9860305e14;%coefficient of G-force 
J2=1.0823e-3;%coefficient of the core of earth
Re=6378137;%radius of earth
R=148.1/3600/180*pi;
aa=1.69;
X00 = X0;
tff = tf;
TT = T;

i = 1;
truervector = [];
truevvector  = [];
randn('state',2);

truex = RungeKutta(tff,X00,TT);%use R-K to solve the equation to get the true state vector
Z = sqrt(truex(1,:).^2+truex(2,:).^2+truex(3,:).^2-Re^2)*tan(R)-aa;%measurment values+sqrt(Rnoise)*randn

truervector = sqrt(truex(1,:).^2+truex(2,:).^2+truex(3,:).^2);
truevvector = sqrt(truex(4,:).^2+truex(5,:).^2+truex(6,:).^2);