📄 untitled2.m
字号:
clear all;
clc;
g0=9.78049;
rad_deg=0.01745329;
wie=7.27220417e-5;
Re=6378393.0;
e=3.3670e-3;
Hn=0.1;
time=150;
estimate_time=300;
Kg=[0.0000 0 0
0 0.0000 0
0 0 0.0000];
Ka=[0.0000 0 0
0 0.0000 0
0 0 0.0000];
G_Drift=[0.01*rad_deg/3600
0.01*rad_deg/3600
0.01*rad_deg/3600];
A_Bias=[1e-4*g0
1e-4*g0
1e-4*g0];
Kg=eye(3)+Kg;
Ka=eye(3)+Ka;
%%%%%%
T_p=3;
T_r=5;
T_y=7;
%%%%%%
pitchm=6*rad_deg;
rollm=8*rad_deg;
yawm=10*rad_deg;
%%%%初始角%%
pitchk=0*rad_deg;
rollk=0*rad_deg;
yawk=0*rad_deg;
%%%%
% p_pitch=30*rad_deg;
% p_roll=30*rad_deg;
% p_yaw=30*rad_deg;
p_pitch=0*rad_deg;
p_roll=0*rad_deg;
p_yaw=0*rad_deg;
%%%初始误差角%%
pitch_error=1*rad_deg;
roll_error=1*rad_deg;
yaw_error=1*rad_deg;
%%
%%% n对应主惯导
lati=45.7796*rad_deg;
longi=126.6705*rad_deg;
wien=[0
wie*cos(lati)
wie*sin(lati)];
%%%p对子惯导
phi=45.7796*rad_deg;
lamda=126.6705*rad_deg;
wiep=[0
wie*cos(phi)
wie*sin(phi)];
g1=g0+0.051799*(sin(phi))^2;
%%%%%% 初始姿态角%%%%
pitch0=pitchm*sin(p_pitch)+pitchk;
roll0=rollm*sin(p_roll)+rollk;
yaw0=yawm*sin(p_yaw)+yawk;
%%%%%
% sea_mile=1.852e3/3600;
a0=0.1*g0;%(7-4)*sea_mile/time;
aold=[a0*sin(yaw0);a0*cos(yaw0);0];
v0=[0.0;0.0;0.0];
v=v0;
a=aold;
Rxp=Re*(1+e*(sin(phi))^2);
Ryp=Re*(1-2*e+3*e*(sin(phi))^2);
wepp=[-v(2)/Ryp;
v(1)/Rxp;
v(1)*tan(phi)/Rxp];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
q=[1 0 0 0]';
%%%%%%建立 初始 捷 联阵
%%%%%%%%%
pitch00=pitch0;
roll00=roll0;
yaw00=yaw0;
Tbn=[cos(roll00)*cos(yaw00) + sin(roll00)*sin(pitch00)*sin(yaw00) cos(pitch00)*sin(yaw00) sin(roll00)*cos(yaw00) - cos(roll00)*sin(pitch00)*sin(yaw00)
-cos(roll00)*sin(yaw00) + sin(roll00)*sin(pitch00)*cos(yaw00) cos(pitch00)*cos(yaw00) -sin(roll00)*sin(yaw00) - cos(roll00)*sin(pitch00)*cos(yaw00)
-sin(roll00)*cos(pitch00) sin(pitch00) cos(roll00)*cos(pitch00)];
T(1,1) = cos(roll0+roll_error)*cos(yaw0+yaw_error) + sin(roll0+roll_error)*sin(pitch0+pitch_error)*sin(yaw0+yaw_error);
T(1,2) = cos(pitch0+pitch_error)*sin(yaw0+yaw_error);
T(1,3) = sin(roll0+roll_error)*cos(yaw0+yaw_error) - cos(roll0+roll_error)*sin(pitch0+pitch_error)*sin(yaw0+yaw_error);
T(2,1) = -cos(roll0+roll_error)*sin(yaw0+yaw_error) + sin(roll0+roll_error)*sin(pitch0+pitch_error)*cos(yaw0+yaw_error);
T(2,2) = cos(pitch0+pitch_error)*cos(yaw0+yaw_error);
T(2,3) = -sin(roll0+roll_error)*sin(yaw0+yaw_error) - cos(roll0+roll_error)*sin(pitch0+pitch_error)*cos(yaw0+yaw_error);
T(3,1) = -sin(roll0+roll_error)*cos(pitch0+pitch_error);
T(3,2) = sin(pitch0+pitch_error);
T(3,3) = cos(roll0+roll_error)*cos(pitch0+pitch_error);
% TT=[1 yaw_error -roll_error;
% -yaw_error 1 pitch_error;
% roll_error -pitch_error 1];
% T=TT*Tbn;
TT=T*inv(Tbn);
dp=TT(2,3)/rad_deg*60
dr=TT(3,1)/rad_deg*60
dy=TT(1,2)/rad_deg*60
q(1) = sqrt(1+T(1,1)+T(2,2)+T(3,3))/2.0;
q(2) = (T(3,2)-T(2,3))/(4*q(1));
q(3) = (T(1,3)-T(3,1))/(4*q(1));
q(4) = (T(2,1)-T(1,2))/(4*q(1));
Tn=(time+estimate_time)/Hn;
X=[0;
0;
0;
0;
0;
0;
0;
0;
0;
0];
H=[1 0 0 0 0 0 0 0 0 0 ;
0 1 0 0 0 0 0 0 0 0];
P = zeros(10);
P(1,1) = 0.1^2;
P(2,2) = 0.1^2;
P(3,3) = power(1*rad_deg,2);
P(4,4) = power(1*rad_deg,2);
P(5,5) = power(1*rad_deg,2);
P(6,6) = power(A_Bias(1),2);
P(7,7) = power(A_Bias(2),2);
P(8,8) = power(G_Drift(1),2);
P(9,9) = power(G_Drift(2),2);
P(10,10) = power(G_Drift(3),2);
Q=zeros(10);
Q(1,1) = power(A_Bias(1),2);
Q(2,2) = power(A_Bias(2),2);
Q(3,3) = power(G_Drift(1),2);
Q(4,4) = power(G_Drift(2),2);
Q(5,5) = power(G_Drift(3),2);
% Q(1,1) = power(0.5*A_Bias(1),2);
% Q(2,2) = power(0.5*A_Bias(2),2);
% Q(3,3) = power(0.5*G_Drift(1),2);
% Q(4,4) = power(0.5*G_Drift(2),2);
% Q(5,5) = power(0.5*G_Drift(3),2);
R=zeros(2);
R(1,1) = power(0.001,2);
R(2,2) = power(0.001,2);
X1=zeros(5,1);
P1=zeros(5,5);
P1(1,1) = 0.1^2;
P1(2,2) = 0.1^2;
P1(3,3) = power(1*rad_deg,2);
P1(4,4) = power(1*rad_deg,2);
P1(5,5) = power(1*rad_deg,2);
Q1=zeros(5,5);
Q1(1,1) = power(A_Bias(1),2);
Q1(2,2) = power(A_Bias(2),2);
Q1(3,3) = power(G_Drift(1),2);
Q1(4,4) = power(G_Drift(2),2);
Q1(5,5) = power(G_Drift(3),2);
R1=zeros(2,2);
R1(1,1) = power(0.001,2);
R1(2,2) = power(0.001,2);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for k=1:Tn
for i=1:3
pitchT(i) = pitchm*sin(2*pi*(k-(3-i)/2)*Hn/T_p+p_pitch)+pitchk;
rollT(i) = rollm*sin(2*pi*(k-(3-i)/2)*Hn/T_r+p_roll)+rollk;
yawT(i) = yawm*sin(2*pi*(k-(3-i)/2)*Hn/T_y+p_yaw)+yawk;
wpT(i) = 2*pi/T_p*pitchm*cos(2*pi*(k-(3-i)/2)*Hn/T_p+p_pitch);
wrT(i) = 2*pi/T_r*rollm*cos(2*pi*(k-(3-i)/2)*Hn/T_r+p_roll);
wyT(i) = 2*pi/T_y*yawm*cos(2*pi*(k-(3-i)/2)*Hn/T_y+p_yaw);
end
Tbn=[cos(rollT(3))*cos(yawT(3)) + sin(rollT(3))*sin(pitchT(3))*sin(yawT(3)) cos(pitchT(3))*sin(yawT(3)) sin(rollT(3))*cos(yawT(3)) - cos(rollT(3))*sin(pitchT(3))*sin(yawT(3))
-cos(rollT(3))*sin(yawT(3)) + sin(rollT(3))*sin(pitchT(3))*cos(yawT(3)) cos(pitchT(3))*cos(yawT(3)) -sin(rollT(3))*sin(yawT(3)) - cos(rollT(3))*sin(pitchT(3))*cos(yawT(3))
-sin(rollT(3))*cos(pitchT(3)) sin(pitchT(3)) cos(rollT(3))*cos(pitchT(3))];
for i=1:3
wnbb(1,i) = sin(rollT(i))*cos(pitchT(i))*wyT(i) + cos(rollT(i))*wpT(i);
wnbb(2,i) = -sin(pitchT(i))*wyT(i) + wrT(i);
wnbb(3,i) = -cos(rollT(i))*cos(pitchT(i))*wyT(i) + sin(rollT(i))*wpT(i);
end
wnbb1=[wnbb(1,1)
wnbb(2,1)
wnbb(3,1)];
wnbb2=[wnbb(1,2)
wnbb(2,2)
wnbb(3,2)];
wnbb3=[wnbb(1,3)
wnbb(2,3)
wnbb(3,3)];
%%%%%%%%%%%%%%%%%%%%%
Rxt = Re*(1+e*sin(lati)*sin(lati));
Ryt = Re*(1-2*e+3*e*sin(lati)*sin(lati));
%%%%%%%%%%%%%%%%%%%
v00=v0;
v0(1) = v0(1)+a(1)*Hn;
v0(2) = v0(2)+a(2)*Hn;
wenn(1,1) = -v0(2)/Ryt;
wenn(2,1) = v0(1)/Rxt;
wenn(3,1) = v0(1)*tan(lati)/Rxt;
%%%%%%%%%%%%
longi= longi+(v00(1)+v0(1))/2*Hn/(Rxt*cos(lati));
lati = lati+(v00(2)+v0(2))/2*Hn/Ryt;
wien=[0;
wie*cos(lati);
wie*sin(lati)];
winn=wien+wenn;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%555
for i=1:3
Tnb11(i) = cos(rollT(i))*cos(yawT(i)) + sin(rollT(i))*sin(pitchT(i))*sin(yawT(i));
Tnb12(i) = -cos(rollT(i))*sin(yawT(i)) + sin(rollT(i))*sin(pitchT(i))*cos(yawT(i));
Tnb13(i) = -sin(rollT(i))*cos(pitchT(i));
Tnb21(i) = cos(pitchT(i))*sin(yawT(i));
Tnb22(i) = cos(pitchT(i))*cos(yawT(i));
Tnb23(i) = sin(pitchT(i));
Tnb31(i) = sin(rollT(i))*cos(yawT(i)) - cos(rollT(i))*sin(pitchT(i))*sin(yawT(i));
Tnb32(i) = -sin(rollT(i))*sin(yawT(i)) - cos(rollT(i))*sin(pitchT(i))*cos(yawT(i));
Tnb33(i) = cos(rollT(i))*cos(pitchT(i));
end
%%%%%导航 坐标系到 载体坐标
Tnb1=[Tnb11(1) Tnb12(1) Tnb13(1)
Tnb21(1) Tnb22(1) Tnb23(1)
Tnb31(1) Tnb32(1) Tnb33(1)];
Tnb2=[Tnb11(2) Tnb12(2) Tnb13(2)
Tnb21(2) Tnb22(2) Tnb23(2)
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -