sen.m

给出了在卫星轨道确定中的各种测量量及测量噪声

function Z = sen(t)     % sen: 敏感器相关????

% ===================================================
k = pi/180;
R_moon = moon_ECI(t);   % t时刻惯性坐标系下地月矢量
R_sun = sun_ECI(t);     % t时刻惯性坐标系下地日矢量
R = orbitinECI(t);	    % t时刻卫星的位置和速度

% ========== 惯性坐标系到体坐标系的转换矩阵 ==========
r = R(1:3);		% 取R的分量
v = R(4:6);
e3 = -r/norm(r);
% p = -cross(v, -r);	% cross: 向量的叉乘
% e2 = p/(norm(p));   % 李鑫的写法
p = cross(v, r);
e2 = p/norm(p);
e1 = cross(e2, e3);
AA = [e1; e2; e3];

SM = R_moon - r;
SS = R_sun - r;
r_m = AA * SM';		% ': 实矩阵的转置
r_m = r_m'/norm(r_m);	% 体坐标系下月球的矢量
r_s = AA * SS';
r_s = r_s'/norm(r_s);	% 体坐标系下太阳的矢量
% r_e = AA * r';
% rr = r_e'/norm(r_e);	% 体坐标系下地球矢量

%================ 加噪声 ================

% moon
fi_m = atan2(r_m(2), r_m(1)) + randn*0.01/3*k;	% atan2(Y,X) & k=pi/180;
% 0.01是不是敏感器精度????
cita_m = asin(r_m(3)) + randn*0.01/3*k;
M1 = cos(cita_m) * cos(fi_m);
M2 = cos(cita_m) * sin(fi_m);
M3 = sin(cita_m);
sm_e = [M1; M2; M3];	% 敏感器测得体坐标系下的星月单位矢量

% sun
fi_s = atan2(r_s(2), r_s(1)) + randn*0.01/3*k;
cita_s = asin(r_s(3)) + randn*0.01/3*k;
S1 = cos(cita_s) * cos(fi_s);
S2 = cos(cita_s) * sin(fi_s);
S3 = sin(cita_s);
ssu_e = [S1; S2; S3];	% 敏感器测得体坐标系下的星日单位矢量

% earth  // 假定卫星严格对地定向！
fi_e = randn*0.03/3*k;  % 还是有一点疑问 ????
cita_e = pi/2 + randn*0.03/3*k;
E1 = cos(cita_e) * cos(fi_e);
E2 = cos(cita_e) * sin(fi_e);
E3 = sin(cita_e);
se_e = [E1; E2; E3];	% 敏感器测得体坐标系下的星地单位矢量

%================ h(alpha,beta) ================% ????有疑问
% Z = [sm_e ssu_e se_e];	% moon sun earth & each has three data
D = dot(sm_e, se_e);		% (sm_e,se_e)
DD = dot(ssu_e, se_e);		% (ssu_e,se_e)
Z = [D; DD];