?? transfer_alignment_modify.m
字號:
wiey=wie*cos(L);
wiez=wie*sin(L);
%計算地理系t相對地球的角速度，單位：弧度/秒
wetx=-vy(i)/Rtyh;
wety=vx(i)/Rtxh;
wetz=vx(i)*tan(L)/Rtxh;
%計算MINS比力，單位：米/(秒*秒)
fmx=dvx(i)-vy(i)*(2*wiez+wetz)+vz(i)*(2*wiey+wety);%利用比力方程反推加速度計輸出
fmy=dvy(i)+vx(i)*(2*wiez+wetz)-vz(i)*wetx;
fmz=dvz(i)-vx(i)*(2*wiey+wety)+vy(i)*wetx+g;
%計算地理系t相對慣性系的角速度，單位：弧度/秒
witx=wiex+wetx;
wity=wiey+wety;
witz=wiez+wetz;
%計算飛機機體坐標系b相對慣性系的角速度，單位：弧度/秒
wibx=witx+wtbx(i);
wiby=wity+wtby(i);
wibz=witz+wtbz(i);
%計算姿態陣cbt
cbt=zeros(3,3);
cbt(1,1)=cos(theta(i))*sin(psi(i));
cbt(1,2)=sin(gama(i))*cos(psi(i))-sin(theta(i))*cos(gama(i))*sin(psi(i));
cbt(1,3)=cos(gama(i))*cos(psi(i))+sin(psi(i))*sin(gama(i))*sin(theta(i));
cbt(2,1)=cos(theta(i))*cos(psi(i));
cbt(2,2)=-sin(gama(i))*sin(psi(i))-sin(theta(i))*cos(gama(i))*cos(psi(i));
cbt(2,3)=-cos(gama(i))*sin(psi(i))+sin(theta(i))*sin(gama(i))*cos(psi(i));
cbt(3,1)=sin(theta(i));
cbt(3,2)=cos(theta(i))*cos(gama(i));
cbt(3,3)=-cos(theta(i))*sin(gama(i));
%計算矩陣WE
WE=zeros(3,3);
WE(1,1)=2*wibz+2*wiby*xix;
WE(1,2)=-2*wibz*xiz-2*wibx*xix;
WE(1,3)=2*wiby*xiz-2*wibx;
WE(2,1)=2*wibz*xix-2*wiby;
WE(2,2)=2*wibz*xiy+2*wibx;
WE(2,3)=-2*wiby*xiy-2*wibx*xix;
WE(3,1)=-2*wibz*xiz-2*wiby*xiy;
WE(3,2)=-2*wibz+2*wibx*xiy;
WE(3,3)=2*wiby+2*wibx*xiz;
%計算函數RB(X,Y,Z)
RBXX=(cbt(1,2)*rz-cbt(1,3)*ry)*betax;
RBXY=(cbt(1,3)*rx-cbt(1,1)*rz)*betay;
RBXZ=(cbt(1,1)*ry-cbt(1,2)*rx)*betaz;
RBYX=(cbt(2,2)*rz-cbt(2,3)*ry)*betax;
RBYY=(cbt(2,3)*rx-cbt(2,1)*rz)*betay;
RBYZ=(cbt(2,1)*ry-cbt(2,2)*rx)*betaz;
RBZX=(cbt(3,2)*rz-cbt(3,3)*ry)*betax;
RBZY=(cbt(3,3)*rx-cbt(3,1)*rz)*betay;
RBZZ=(cbt(3,1)*ry-cbt(3,2)*rx)*betaz;
%計算函數WB(X,Y,Z)
WBXX=WE(1,1)*RBXX+WE(1,2)*RBYX+WE(1,3)*RBZX;
WBXY=WE(1,1)*RBXY+WE(1,2)*RBYY+WE(1,3)*RBZY;
WBXZ=WE(1,1)*RBXZ+WE(1,2)*RBYZ+WE(1,3)*RBZZ;
WBYX=WE(2,1)*RBXX+WE(2,2)*RBYX+WE(2,3)*RBZX;
WBYY=WE(2,1)*RBXY+WE(2,2)*RBYY+WE(2,3)*RBZY;
WBYZ=WE(2,1)*RBXZ+WE(2,2)*RBYZ+WE(2,3)*RBZZ;
WBZX=WE(3,1)*RBXX+WE(3,2)*RBYX+WE(3,3)*RBZX;
WBZY=WE(3,1)*RBXY+WE(3,2)*RBYY+WE(3,3)*RBZY;
WBZZ=WE(3,1)*RBXZ+WE(3,2)*RBYZ+WE(3,3)*RBZZ;
%計算函數RP(X,Y,Z)
RPXX=cbt(1,3)*ry-cbt(1,2)*rz;
RPXY=cbt(1,1)*rz-cbt(1,3)*rx;
RPXZ=cbt(1,2)*rx-cbt(1,1)*ry;
RPYX=cbt(2,3)*ry-cbt(2,2)*rz;
RPYY=cbt(2,1)*rz-cbt(2,3)*rx;
RPYZ=cbt(2,2)*rx-cbt(2,1)*ry;
RPZX=cbt(3,3)*ry-cbt(3,2)*rz;
RPZY=cbt(3,1)*rz-cbt(3,3)*rx;
RPZZ=cbt(3,2)*rx-cbt(3,1)*ry;
%計算函數EP(X,Y,Z)
EPXX=WE(1,1)*RPXX+WE(1,2)*RPYX+WE(1,3)*RPZX;
EPXY=WE(1,1)*RPXY+WE(1,2)*RPYY+WE(1,3)*RPZY;
EPXZ=WE(1,1)*RPXZ+WE(1,2)*RPYZ+WE(1,3)*RPZZ;
EPYX=WE(2,1)*RPXX+WE(2,2)*RPYX+WE(2,3)*RPZX;
EPYY=WE(2,1)*RPXY+WE(2,2)*RPYY+WE(2,3)*RPZY;
EPYZ=WE(2,1)*RPXZ+WE(2,2)*RPYZ+WE(2,3)*RPZZ;
EPZX=WE(3,1)*RPXX+WE(3,2)*RPYX+WE(3,3)*RPZX;
EPZY=WE(3,1)*RPXY+WE(3,2)*RPYY+WE(3,3)*RPZY;
EPZZ=WE(3,1)*RPXZ+WE(3,2)*RPYZ+WE(3,3)*RPZZ;
%計算函數CW1,CW2,CW3
cw1=cbt(1,1)*witx+cbt(2,1)*wity+cbt(3,1)*witz+wtbx(i);
cw2=cbt(1,2)*witx+cbt(2,2)*wity+cbt(3,2)*witz+wtby(i);
cw3=cbt(1,3)*witx+cbt(2,3)*wity+cbt(3,3)*witz+wtbz(i);
%計算函數HF(X,Y,Z)
HFXX=cbt(1,3)*cw2-cbt(1,2)*cw3;
HFXY=cbt(2,3)*cw2-cbt(2,2)*cw3;
HFXZ=cbt(3,3)*cw2-cbt(3,2)*cw3;
HFYX=cbt(1,1)*cw3-cbt(1,3)*cw1;
HFYY=cbt(2,1)*cw3-cbt(2,3)*cw1;
HFYZ=cbt(3,1)*cw3-cbt(3,3)*cw1;
HFZX=cbt(1,2)*cw1-cbt(1,1)*cw2;
HFZY=cbt(2,2)*cw1-cbt(2,1)*cw2;
HFZZ=cbt(3,2)*cw1-cbt(3,1)*cw2;
%計算狀態轉移陣a
a=zeros(15,15);
a(1,4)=fmz;
a(1,6)=-fmx;
a(1,7)=fmz*cbt(1,1)-fmx*cbt(3,1)+WBXX;
a(1,8)=fmz*cbt(1,2)-fmx*cbt(3,2)+WBXY;
a(1,9)=fmz*cbt(1,3)-fmx*cbt(3,3)+WBXZ;
a(1,14)=1;
a(2,4)=-fmy;
a(2,5)=fmx;
a(2,7)=-fmy*cbt(1,1)+fmx*cbt(2,1)+WBYX;
a(2,8)=-fmy*cbt(1,2)+fmx*cbt(2,2)+WBYY;
a(2,9)=-fmy*cbt(1,3)+fmx*cbt(2,3)+WBYZ;
a(2,15)=1;
a(3,5)=-fmz;
a(3,6)=fmy;
a(3,7)=-fmz*cbt(2,1)+fmy*cbt(3,1)+WBZX;
a(3,8)=-fmz*cbt(2,2)+fmy*cbt(3,2)+WBZY;
a(3,9)=-fmz*cbt(2,3)+fmy*cbt(3,3)+WBZZ;
a(3,13)=1;
a(4,1)=-1/Rtyh;
a(4,5)=witz;
a(4,6)=-wity;
a(4,7)=witz*cbt(2,1)-wity*cbt(3,1);
a(4,8)=witz*cbt(2,2)-wity*cbt(3,2);
a(4,9)=witz*cbt(2,3)-wity*cbt(3,3);
a(4,10)=1;
a(5,3)=1/Rtxh;
a(5,4)=-witz;
a(5,6)=witx;
a(5,7)=-witz*cbt(1,1)+witx*cbt(3,1);
a(5,8)=-witz*cbt(1,2)+witx*cbt(3,2);
a(5,9)=-witz*cbt(1,3)+witx*cbt(3,3);
a(5,11)=1;
a(6 ,3)=tan(L)/Rtxh;
a(6,4)=wity;
a(6,5)=-witx;
a(6,7)=wity*cbt(1,1)-witx*cbt(2,1);
a(6,8)=wity*cbt(1,2)-witx*cbt(2,2);
a(6,9)=wity*cbt(1,3)-witx*cbt(2,3);
a(6,12)=1;
a(7,7)=-betax;
a(8,8)=-betay;
a(9,9)=-betaz;
%量測系數陣H
H=zeros(6,15);
H(1,1)=1;
H(2,2)=1;
H(3,3)=1;
H(4,4)=HFXX;
H(4,5)=HFXY;
H(4,6)=HFXZ;
H(4,7)=-betax;
H(4,8)=-cw3;
H(4,9)=cw2;
H(4,10)=cbt(1,1);
H(4,11)=cbt(2,1);
H(4,12)=cbt(3,1);
H(5,4)=HFYX;
H(5,5)=HFYY;
H(5,6)=HFYZ;
H(5,7)=cw3;
H(5,8)=-betay;
H(5,9)=cw1;
H(5,10)=cbt(1,2);
H(5,11)=cbt(2,2);
H(5,12)=cbt(3,2);
H(6,4)=HFZX;
H(6,5)=HFZY;
H(6,6)=HFZZ;
H(6,7)=-cw2;
H(6,8)=cw1;
H(6,9)=-betaz;
H(6,10)=cbt(1,3);
H(6,11)=cbt(2,3);
H(6,12)=cbt(3,3);
%計算系統白噪聲方差陣Q
Q=zeros(9,9);
Q(1,1)=aay+EPXX^2*acx+EPXY^2*acy+EPXZ^2*acz;
Q(2,2)=aaz+EPYX^2*acx+EPYY^2*acy+EPYZ^2*acz;
Q(3,3)=aax+EPZX^2*acx+EPZY^2*acy+EPZZ^2*acz;
Q(4,4)=abx;
Q(5,5)=aby;
Q(6,6)=abz;
Q(7,7)=acx;
Q(8,8)=acy;
Q(9,9)=acz;
%量測噪聲方差陣R
R=zeros(6,6);
R(1,1)=1^2;
R(2,2)=1^2;
R(3,3)=1^2;
R(4,4)=cbt(1,1)^2*SWEX^2+cbt(2,1)^2*SWEY^2+cbt(3,1)^2*SWEZ^2+2*betax*Tm*(0.05*pi/180)^2;
R(5,5)=cbt(1,2)^2*SWEX^2+cbt(2,2)^2*SWEY^2+cbt(3,2)^2*SWEZ^2+2*betay*Tm*(0.05*pi/180)^2;
R(6,6)=cbt(1,3)^2*SWEX^2+cbt(2,3)^2*SWEY^2+cbt(3,3)^2*SWEZ^2+2*betaz*Tm*(0.05*pi/180)^2;
%對系統離散化
A=I+a*Tm+a*a*(Tm^2)/2;
B=Tm*(I+a*Tm/2+a*a*(Tm^2)/6)*G;
%動態系統噪聲
sysnoise=zeros(9,1);
sysnoise(1,1)=randn(1,1)*SWDX+EPXX*randn(1,1)*SWPX+EPXY*randn(1,1)*SWPY+EPXZ*randn(1,1)*SWPZ;
sysnoise(2,1)=randn(1,1)*SWDY+EPYX*randn(1,1)*SWPX+EPYY*randn(1,1)*SWPY+EPYZ*randn(1,1)*SWPZ;
sysnoise(3,1)=randn(1,1)*SWDZ+EPZX*randn(1,1)*SWPX+EPZY*randn(1,1)*SWPY+EPZZ*randn(1,1)*SWPZ;
sysnoise(4,1)=randn(1,1)*SWEX;
sysnoise(5,1)=randn(1,1)*SWEY;
sysnoise(6,1)=randn(1,1)*SWEZ;
sysnoise(7,1)=randn(1,1)*SWPX;
sysnoise(8,1)=randn(1,1)*SWPY;
sysnoise(9,1)=randn(1,1)*SWPZ;
%觀測系統噪聲
obsnoise=zeros(6,1);
obsnoise(1,1)=randn(1,1)*1;
obsnoise(2,1)=randn(1,1)*1;
obsnoise(3,1)=randn(1,1)*1;
obsnoise(4,1)=cbt(1,1)*randn(1,1)*SWEX+cbt(2,1)*randn(1,1)*SWEY+cbt(3,1)*randn(1,1)*SWEZ+randn(1,1)*SWPX;
obsnoise(5,1)=cbt(1,2)*randn(1,1)*SWEX+cbt(2,2)*randn(1,1)*SWEY+cbt(3,2)*randn(1,1)*SWEZ+randn(1,1)*SWPY;
obsnoise(6,1)=cbt(1,3)*randn(1,1)*SWEX+cbt(2,3)*randn(1,1)*SWEY+cbt(3,3)*randn(1,1)*SWEZ+randn(1,1)*SWPZ;
%計算觀測值z
z=H*[A*xfeed+B*sysnoise]+obsnoise;
%計算一步預測均方誤差
P1=A*P0*A'+B*Q*B';
%計算濾波增益
K=P1*H'*inv(H*P1*H'+R);
%計算估計均方誤差
P=(I-K*H)*P1*(I-K*H)'+K*R*K';
%計算狀態
x0=A*x0+K*(z-H*x0);
%下面計算各狀態量估計均方誤差
%速度誤差 單位:米/秒
y1(i)=sqrt(P(1,1)); %X軸
y2(i)=sqrt(P(2,2)); %Y軸
y3(i)=sqrt(P(3,3)); %Z軸
%平臺誤差角 單位:角分
y4(i)=sqrt(P(4,4))*60*180/pi; %X軸
y5(i)=sqrt(P(5,5))*60*180/pi; %Y軸
y6(i)=sqrt(P(6,6))*60*180/pi; %Z軸
%撓曲變形角 單位:角分
y7(i)=sqrt(P(7,7))*60*180/pi; %X軸
y8(i)=sqrt(P(8,8))*60*180/pi; %Y軸
y9(i)=sqrt(P(9,9))*60*180/pi; %Z軸
%陀螺漂移 單位:度/小時
y10(i)=sqrt(P(10,10))*180*3600/pi; %X軸
y11(i)=sqrt(P(11,11))*180*3600/pi; %Y軸
y12(i)=sqrt(P(12,12))*180*3600/pi; %Z軸
%加速度計誤差 單位:米/(秒*秒)
y13(i)=sqrt(P(13,13))*10^3/g0; %X軸
y14(i)=sqrt(P(14,14))*10^3/g0; %Y軸
y15(i)=sqrt(P(15,15))*10^3/g0; %Z軸
%保存各狀態量估計
%速度誤差 單位:米/秒
x1(i)=x0(1,1); %X軸
x2(i)=x0(2,1); %Y軸
x3(i)=x0(3,1); %Z軸
%平臺誤差角 單位:角分
x4(i)=x0(4,1)*60*180/pi; %X軸
x5(i)=x0(5,1)*60*180/pi; %Y軸
x6(i)=x0(6,1)*60*180/pi; %Z軸
%撓曲變形角 單位:角分
x7(i)=x0(7,1)*60*180/pi; %X軸
x8(i)=x0(8,1)*60*180/pi; %Y軸
x9(i)=x0(9,1)*60*180/pi; %Z軸
%陀螺漂移 單位:度/小時
x10(i)=x0(10,1)*180*3600/pi; %X軸
x11(i)=x0(11,1)*180*3600/pi; %Y軸
x12(i)=x0(12,1)*180*3600/pi; %Z軸
%加速度計誤差 單位:米/(秒*秒)
x13(i)=x0(13,1)*10^3/g0; %X軸
x14(i)=x0(14,1)*10^3/g0; %Y軸
x15(i)=x0(15,1)*10^3/g0; %Z軸
%計算飛機所在點緯度 單位：弧度
L=L+vy(i)*Tm/Rtyh;
%計算飛機所在點重力加速度, 單位:米/(秒*秒)
g=9.7803267714*(1+0.00193185138639*sin(L)^2)/sqrt(1-0.00669437999013*sin(L)^2);
%計算地理系t偏離初始時刻地理系t0的偏角xi 單位:弧度
xix=xix+wetx*Tm;
xiy=xiy+(wiey+wety)*Tm;
xiz=xiz+(wiez+wetz)*Tm;
%計算初始對準結束時的SINS姿態陣CMS：CMS=cit*cto*cts*cbt*cbs
%計算cit陣
cit=zeros(3,3);
cit(1,2)=1;
cit(2,3)=1;
cit(3,1)=1;
%計算cto陣
cto=zeros(3,3);
cto(1,1)=1;
cto(1,2)=-xiz;
cto(1,3)=xiy;
cto(2,1)=xiz;
cto(2,2)=1;
cto(2,3)=-xix;
cto(3,1)=-xiy;
cto(3,2)=xix;
cto(3,3)=1;
%計算cts陣
tmpnew=cbt*[x0(7,1);x0(8,1);x0(9,1)];
cts=zeros(3,3);
cts(1,1)=1;
cts(1,2)=-(x0(6,1)+tmpnew(3,1));
cts(1,3)=x0(5,1)+tmpnew(3,1);
cts(2,1)=-cts(1,2);
cts(2,2)=1;
cts(2,3)=-(x0(4,1)+tmpnew(1,1));
cts(3,1)=-cts(1,3);
cts(3,2)=-cts(2,3);
cts(3,3)=1;
%計算cbs陣
cbs=zeros(3,3);
cbs(1,1)=1;
cbs(2,2)=0.707;
cbs(2,3)=0.707;
cbs(3,2)=-0.707;
cbs(3,3)=0.707;
%計算SINS姿態陣cms
cms=cit*cto*cts*cbt*cbs;%45度安裝
CMS=[CMS,cms];
cms2=cit*cto*cts*cbt;%0度安裝
CMS2=[CMS2,cms2];
%當前值作為下一步計算的初始值
P0=P;
xfeed=x0;
J=[0 0 1;1 0 0;0 1 0];
cgbs2=(cto'*J*cms2*J')';%cto為采樣時刻t'相對于im的絕對偏差角所構成的偏離矩陣
fuyang2=asin(cgbs2(2,3))*180/pi;%導彈傾斜0度安裝,導彈的姿態角(橫滾、俯仰、航向)
hengun2=atan(-cgbs2(1,3)/cgbs2(3,3))*180/pi;
hangxiang2=asec(cos(fuyang2*pi/180)/cgbs2(2,2))*180/pi;
%vb=[vx(i)]
if (psi(i)<0) %不考慮導彈相對機體的坐標延遲，具體見相應文檔
psi(i)=psi(i)+2*pi;
hangxiang2=360-hangxiang2;
elseif (psi(i)>pi)&(psi(i)<=1.5*pi)
hangxiang2=360-hangxiang2;
elseif (psi(i)>1.5*pi)&(psi(i)<=2*pi)
hangxiang2=360-hangxiang2;
elseif psi(i)>2*pi
psi(i)=psi(i)-2*pi;
end
%數據格式為采樣時間；飛機的姿態角（橫滾、俯仰、航向）、飛機的北、東、地速度；經、緯、高度；導彈的姿態角；速度誤差均方根（北、東、地）；
%平臺誤差角均方根；撓曲變形角均方根(機頭、機背、右翼)；陀螺漂移均方根(北、東、地)；加速度計零偏均方根(北、東、地)
%if( hangxiang2<0)
fprintf(fid,'%f %f %f %f %f %f %f %12.8f %12.8f %12.8f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f \n',t,gama(i)*180/pi,theta(i)*180/pi,psi(i)*180/pi,vy(i),vx(i),-vz(i),long1(i),lat1(i),h(i),hengun2,fuyang2,hangxiang2,y2(i),y1(i),-y3(i),y4(i),y5(i),y6(i),y7(i),y8(i),y9(i),y11(i),y10(i),-y12(i),y14(i),y13(i),-y15(i),x2(i),x1(i),-x3(i),x4(i),x5(i),x6(i),x7(i),x8(i),x9(i),x11(i),x10(i),-x12(i),x14(i),x13(i),-x15(i));
%1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43
i=i+1;
end %end of for
fclose(fid);?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -