clc
clear all
%仿真周期T
N=25000;
m0=20;
I=1;
%焦距
f=100*0.001;%100毫米
load('trajectory3.mat') %位置
lambdaa=lon;
phii=lat;
heightt=height;
Vet_tt=Vn;
thetaa=pitch;
psii=yaw;
gammaa=roll;
Fbb=Fb;
Wib_bb=Wb;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%下面加测量误差%认为焦距无误差
%加入X,Y方向星图匹配噪声偏差
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
psi3=[];
gamma3=[];
theta3=[];
psi4=[];
gamma4=[];
theta4=[];
k1=[0,0,0];
%CCD随机偏差%0.01,每个像元直径10/1000/100
t1=[0.01*randn(3,2),k1']*0.001;%CCD固定偏差
while (I<N/m0+1)
%下面定姿
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%轨迹真实姿态
psi=psii(m0*I);
gamma=gammaa(m0*I);
theta=thetaa(m0*I);
psi3=[psi3;psi];
gamma3=[gamma3;gamma];
theta3=[theta3;theta];
%a=[thetaa(m0*I);psii(m0*I);gammaa(m0*I)]*180/pi;
%angle1=[angle1,a];
ctb=[cos(gamma)*cos(psi)-sin(gamma)*sin(theta)*sin(psi) cos(gamma)*sin(psi)+sin(gamma)*sin(theta)*cos(psi) -sin(gamma)*cos(theta);
-cos(theta)*sin(psi) cos(theta)*cos(psi) sin(theta);
sin(gamma)*cos(psi)+cos(gamma)*sin(theta)*sin(psi) sin(gamma)*sin(psi)-cos(gamma)*sin(theta)*cos(psi) cos(gamma)*cos(theta)];
A=ctb';%理想姿态矩阵
%设定星敏感器观察到恒星
%在天球赤道坐标系下对应恒星的赤经lambda与赤纬phi(九颗星)
abr=[80.941350,52.647360;51.284300,20.393330;20.495850,50.935650]*pi/180;
%需要将abr坐标转换为天球系下方向矢量
[m,n]=size(abr);
G=[];
for i=1:m
l(:,:,i)=[cos(abr(i,2))*cos(abr(i,1));sin(abr(i,2))*cos(abr(i,1));sin(abr(i,1))];%天球系下方向矢量
G=[G;l(:,:,i)'];%在天球系下坐标阵
end
%求取星敏感器下理想方向矢量
Si=G*A;
k=f./Si(:,3);
tt=[k,k,k];
S=Si.*tt;%在星敏感器坐标系下理想位置
t0=[normrnd(0,0.01,3,2),k1']*0.001;
S1=S+t0+t1;
y1=sqrt(S1(1,1)^2+S1(1,2)^2+S1(1,3)^2);
y2=sqrt(S1(2,1)^2+S1(2,2)^2+S1(2,3)^2);
y3=sqrt(S1(3,1)^2+S1(3,2)^2+S1(3,3)^2);
%星敏感器,加入噪声后的位置
Si=S1./[y1,y1,y1;y2,y2,y2;y3,y3,y3];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
A2=inv(G)*Si;
T=A2;
theta2=asin(T(3,2));
psi2=-atan(T(1,2)/T(2,2));
gamma2=-atan(T(3,1)/T(3,3));
psi4=[psi4;psi2];
gamma4=[gamma4;gamma2];
theta4=[theta4;theta2];
I=I+1;
end
figure(1);subplot(3,1,1);plot((theta4(:)-theta3(:))*3600),grid on;title('俯仰角误差(角秒)');subplot(3,1,2);plot(gamma4(:)-gamma3(:))*3600,grid on;title('滚转误差(角秒)');subplot(3,1,3);plot((psi4(:)-psi3(:))*3600),grid on;title('偏航角误差(角秒)');