卫星轨道计算一建立系统模型选取地心惯性坐标系假设地球为.DOC

卫星轨道计算 建立系统模型 选取地心惯性坐标系，假设地球为球形的情况下，卫星只受到重力作用。卫星导航系统状态模型为： 其中=3.986005×，为地球引力常数，为卫星在地心惯性坐标系中的位置，为卫星在地心惯性系中的速度。 根据星光角距概念，系统量测方程为 其中是卫星在地心惯性坐标系中的位置矢量，是导航星星光方向的单位矢量，是量测误差。 滤波公式 因为状态方程与量测方程是非线性方程，所以使用扩展卡尔曼滤波。设状态方程为： 其中为误差，状态方程中不存在测量值，因此为模型误差，设其为零均值白噪声。将状态方程线性化并按泰勒级数展开可以得到如下离散方程： 设观测方程为： 将其线性化得到： 若模型噪声的协方差阵，观测噪声的协方差阵，可得到滤波公式如下： 其中 Matlab仿真 仿真条件： 量测误差：0.05 0 噪声协方差阵： 状态初值，导航星星光方向的单位矢量，观测值由已知数据给出。 仿真得出的卫星轨道如下： 附录 仿真程序 clear; load F:\第二学期\天文导航原理\作业2\work; n=8000; T=3; %滤波周期 X=zeros(n+1,6); %以卫星位置和速度为状态量 Xj=zeros(3,6); X(1,:)=X0; Xk=zeros(n+1,6); %状态一步预测 P=zeros(6,6,n+1); %估计均方误差 P(:,:,1)=eye(6); Pk=zeros(6,6,n+1); %一步预测均方误差 Q=diag([0.2^2*ones(1,3),0.002^2*ones(1,3)]); %系统噪声序列方差阵 v=0.05*pi/180; %红外地平仪精度为0.050 R=v^2; %测量噪声序列方差 K=zeros(n+1,6); %滤波增益 Fai=zeros(6,6,n+1); %一步转移矩阵 mu=3.986005*10^14; %地球引力常数 tZ=measurement; %星光角距 Z=zeros(n,3); for i=1:n Z(i,:)=tZ(i,:)+normrnd(0,v,1,3); end for i=1:n r=sqrt(X(i,1:3)*X(i,1:3)); Fa=[mu*(3*X(i,1)^2-r^2)/r^5,3*mu*X(i,1)*X(i,2)/r^5,3*mu*X(i,1)*X(i,3)/r^5; 3*mu*X(i,1)*X(i,2)/r^5,mu*(3*X(i,2)^2-r^2)/r^5,3*mu*X(i,2)*X(i,3)/r^5; 3*mu*X(i,1)*X(i,3)/r^5,3*mu*X(i,2)*X(i,3)/r^5,mu*(3*X(i,3)^2-r^2)/r^5]; F=[zeros(3),eye(3);Fa,zeros(3)]; Fai(:,:,i)=eye(6)+T*F; f=[X(i,4:6),-mu*X(i,1)/r^3,-mu*X(i,2)/r^3,-mu*X(i,3)/r^3]; Xk(i+1,:)=X(i,:)+f*T+F*f*T^2/2; Pk(:,:,i+1)=Fai(:,:,i)*P(:,:,i)*Fai(:,:,i)+Q; for j=1:3 ap=X(i,1:3)*starc(j,:); bp=r^2; hx=-(starc(j,1)*X(i,1)^2+(starc(j,2)*X(i,2)+starc(j,3)*X(i,3))*X(i,1)-starc(j,1)*bp)/(bp*sqrt(bp-ap^2)); hy=-(starc(j,2)*X(i,2)^2+(starc(j,1)*X(i,1)+starc(j,3)*X(i,3))*X(i,2)-starc(j,2)*bp)/(bp