getz.m

来自「这是个扩展kalman滤波的历程」· M 代码 · 共 25 行

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);
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);