挠曲补偿的完整传递对准程序.doc

clear all;
clc;

g0=9.78049; %重力加速度
rad_deg=0.01745329; %表示1°的弧度值，即π/180
wie=7.27220417e-5; %地球自转角速度
Re=6378393.0; %地球半径
e=3.3670e-3; %地球偏心率
Hn=0.1;
tem=1;
%%%%%%%假设r是在子惯导载体坐标系中的投影————————————————%%%%%%%
%%%%%%%%%%%%%%%仿真时杆臂效应误差是在子惯导坐标系内计算的，把计算得到的杆臂效应误差项加到加速度计输出上，再转换到载体坐标系，
%%%%%%%经过滤波后进行初始对准
r=[2 2 2]';
Kg=[0.0001 0 0
0 0.0001 0
0 0 0.0001];
Ka=[0.0001 0 0
0 0.0001 0
0 0 0.0001];
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=7;
T_r=9;
T_y=11;

%二阶马尔科夫常数
Beta_p=2.146/60;
Beta_r=2.146/60;
Beta_y=2.146/60;

%--------------------------------------------------------------------------
%挠曲变形角初值
Sita_p=0*rad_deg;
Sita_r=0*rad_deg
Sita_y=0*rad_deg;

%%%%%%
pitchm=3*rad_deg;
rollm=5*rad_deg;
yawm=7*rad_deg;
%%%%初始角%%
pitchk=0*rad_deg;
rollk=0*rad_deg;
yawk=30*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;
%%
time=60;
%%% 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;%0.1*g0;%(7-4)*sea_mile/time;
aold=[a0*sin(yaw0);a0*cos(yaw0);0];
aold=[a0;0;0];
v0=[4.0*sea_mile*sin(yaw0);4.0*sea_milecos(yaw0);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+rollerror)*cos(pitch0+pitch_error);

TT=T*inv(Tbn)
dp=TT(2,3)/rad_deg
dr=TT(3,1)/rad_deg
dy=TT(1,2)/rad_deg

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/Hn;

X=[0; %东向速度差值
0; %北向速度差值
0; %俯仰角失准角
0; %滚转角失准角
0; %偏航角失准角
0; %加速度计零偏的x向分量
0; %加速度计零偏的y向分量
0; %陀螺常值漂移的x向分量
0; %陀螺常值漂移的y向分量
0; %陀螺常值漂移的z向分量
0; %俯仰角弹性变形角
0; %滚转角弹性变形角
0; %偏航角弹性变形角
0; %俯仰角弹性变形角速度
0; %滚转角弹性变形角速度
0]; %偏航角弹性变形角速度

%误差估计协方差阵
P = zeros(16);
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);
P(11,11) = power(1*rad_deg,2);
P(12,12) = power(1*rad_deg,2);
P(13,13) = power(1*rad_deg,2);
P(14,14) = power(10/3600*rad_deg,2);
P(15,15) = power(10/3600*rad_deg,2);
P(16,16) = power(10/3600*rad_deg,2);

%系统噪声方差阵
%A_Bias为加速度计零偏，0.5*A_Bias为加速度计随机偏移，同理G_Drift为陀螺常值漂移，0.5*G_Drift为陀螺随机漂移
Q=zeros(16);
Q(1,1) = power(0.1*A_Bias(1),2);
Q(2,2) = power(0.1*A_Bias(2),2);
Q(3,3) = power(0.1*G_Drift(1),2);
Q(4,4) = power(0.1*G_Drift(2),2);

Q(5,5) = power(0.1*G_Drift(3),2);
R(14,14) = 4*(Beta_p)^3*((10/60)*rad_deg)^2;
R(15,15) = 4*(Beta_r)^3*((10/60)*rad_deg)^2;
R(16,16) = 4*(Beta_y)^3*((10/60)*rad_deg)^2;

R=zeros(5);
R(1,1) = power(0.1,2);
R(2,2) = power(0.1,2);
R(3,3) = power(0.1*rad_deg,2);
R(4,4) = power(0.1*rad_deg,2);
R(5,5) = power(0.1*rad_deg,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*sin2*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);

wpT2(i)=((2*pi/T_p)^2)*pitchm*cos(2*pi*(k-(3-i)/2)*Hn/T_p+p_pitch);
wrT2(i)=((2*pi/T_r)^2)*rollm*cos(2*pi*(k-(3-i)/2)*Hn/T_r+p_roll);
wyT2(i)=((2*pi/T_y)^2)*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)
Tnb31(2) Tnb32(2) Tnb33(2)];
Tnb3=[Tnb11(3) Tnb12(3) Tnb13(3)
Tnb21(3) Tnb22(3) Tnb23(3)
Tnb31(3) Tnb32(3) Tnb33(3)];

winb1=Tnb1*winn;
winb2=Tnb2*winn;
winb3=Tnb3*winn;

wibb1=winb1+wnbb1;
wibb2=winb2+wnbb2;
wibb3=winb3+wnbb3;
%%%%%%%%%%%%%%%%%%%%%%
fn(1,1) = a(1) - (2*wien(3)+wenn(3))*v0(2);
fn(2,1) = a(2) + (2*wien(3)+wenn(3))*v0(1);
fn(3,1) = a(3) + (2*wien(1)+wenn(1))*v0(2) - (2*wien(2)+wenn(2))*v0(1) + g1;
%%%%%%%%%%%%%%%%
fbb1=Tnb1*fn;
fbb2=Tnb2*fn;
fbb3=Tnb3*fn;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%__________---------因为假设主惯导和子惯导的载体坐标系是重合的，所以在模拟子惯导的加速度和角速度信息时
%%%%%%%%%----直接在主惯导输出上加上子惯导的漂移%%%%%%%%
wib1=TT*Kg*wibb1+G_Drift+0.1*G_Drift*randn(1);
wib2=TT*Kg*wibb2+G_Drift+0.1*G_Drift*randn(1);
wib3=TT*Kg*wibb3+G_Drift+0.1*G_Drift*randn(1);

fb1=TT*Ka*fbb1+A_Bias+0.1*A_Bias*randn(1);
fb2=TT*Ka*fbb2+A_Bias+0.1*A_Bias*randn(1);
fb3=TT*Ka*fbb3+A_Bias+0.1*A_Bias*randn(1);

%------------------------------------------------------------------
%杆臂效应和挠曲效应补偿，补偿在输出加速度上
%------------------------------------------------------------------
%杆臂效应补偿，利用更新之前的不带失准角的姿态矩阵
temp=[ 0 -wib3(3) wib3(2)
wib3(3) 0 -wib3(1)
-wib3(2) wib3(1) 0 ];

temp1=temp*r;
temp2=inv(Tbn)*temp*temp1;

%补偿由震颤引起的挠曲效应
omiga=[ 0 -wyT2(3) wrT2(3)
wyT2(3) 0 -wpT2(3)
-wrT2(3) wpT2(3) 0 ];
temp3=inv(Tbn)*omiga*r;

%经过补偿的比力fb3（在载体坐标系中表示）
fb3=temp2+temp3+fb3;%

%%%%%%%%%%%%%%
q0 = q;
%%%%%%%%%%%%%%%%%%%%%%%%
wpbb1=wib1-inv(T)*(wepp+wiep);
wpbb2=wib2-inv(T)*(wepp+wiep);
wpbb3=wib3-inv(T)*(wepp+wiep);

omiga1=[0 -wpbb1(1) -wpbb1(2) -wpbb1(3)
wpbb1(1) 0 wpbb1(3) -wpbb1(2)
wpbb1(2) -wpbb1(3) 0 wpbb1(1)
wpbb1(3) wpbb1(2) -wpbb1(1) 0 ];

omiga2=[0 -wpbb2(1) -wpbb2(2) -wpbb2(3)
wpbb2(1) 0 wpbb2(3) -wpbb2(2)
wpbb2(2) -wpbb2(3) 0 wpbb2(1)
wpbb2(3) wpbb2(2) -wpbb2(1) 0 ];

omiga3=[0 -wpbb3(1) -wpbb3(2) -wpbb3(3)
wpbb3(1) 0 wpbb3(3) -wpbb3(2)
wpbb3(2) -wpbb3(3) 0 wpbb3(1)
wpbb3(3) wpbb3(2) -wpbb3(1) 0 ];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5
k1 = 1/2*omiga1*q;
q=q0+k1*Hn/2;

k2=1/2*omiga2*q;
q=q0+k2*Hn/2;

k31/2*omiga2*q;
q=q0+k3*Hn;

k4=1/2*omiga3*q;

q=q0+(k1+2*k2+2*k3+k4)/6*Hn;

q=q/sqrt(q(1)^2+q(2)^2+q(3)^2+q(4)^2);

T=[q(1)^2+q(2)^2-q(3)^2-q(4)^2 2*(q(2)*q(3)-q(1)*q(4)) 2*(q(2)*q(4)+q(1)*q(3))
2*(q(2)*q(3)+q(1)*q(4)) q(1)^2-q(2)^2+q(3)^2-q(4)^2 2*(q(3)*q(4)-q(1)*q(2))
2*(q(2)*q(4)-q(1)*q(3)) 2*(q(3)*q(4)+q(1)*q(2)) q(1)^2-q(2)^2-q(3)^2+q(4)^2];

fp=T*fb3;

% %------------------------------------------------------------------
% %主惯导单元的杆臂效应
% M_temp=[0 -wibb3(3) wibb3(2)
% wibb3(3) 0 -wibb3(1)
% -wibb3(2) wibb3(1) 0];
%
% M_temp1=M_temp*r;
% M_temp2=M_temp*M_temp1;
%
% M_omiga=[0 -wyT2(3) wrT2(3)
% wyT2(3) 0 -wpT2(3)
% -wrT2(3) wpT2(3) 0 ];
%
% M_temp3=M_omiga*r+M_temp2;
% fp=fp-M_temp3;
%
% temp_v=M_temp*M_temp+M_omiga;
%
% out2=M_temp3;

%%%%%%%%%%%%%%%运行方法：将上面三条语句屏蔽后运行pinpufenxi.m，然后取消屏蔽再运行%%%%%%%%

f_vx = fp(1) + (2*wiep(3)+wepp(3))*v(2);
f_vy = fp(2) - (2*wiep(3)+wepp(3))*v(1);

v(1) = v(1) + f_vx*Hn;
v(2) = v(2) + f_vy*Hn;

wepp = [-v(2)/Ryp;
v(1)/Rxp;
v(1)*tan(phi)/Rxp];

phi = phi+v(2)*Hn/Ryp;
lamda= lamda+v(1)*Hn/(Rxp*cos(phi));
wiep = [0; wie*cos(phi); wie*sin(phi)];
g1 = g0+0.051799*sin(phi)*sin(phi);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
A =[0 2*wiep(3)+wepp(3) 0 -fp(3) fp(2) 0 0 0 0 0 0 0 0 0 0 0;
-(2*wiep(3)+wepp(3)) 0 fp(3) 0 -fp(1) 0 0 0 0 0 0 0 0 0 0 0;
0 -1/Ryp 0 wiep(3)+v(1)*tan(phi)/Rxp -(wiep(2)+wepp(2)) 0 0 0 0 0 0 0 0 0 0 0;
1/Rxp 0 -(wiep(3)+wepp(3)) 0 wepp(1) 0 0 0 0 0 0 0 0 0 0 0;
1*tan(phi)/Rxp 0 wiep(2)+wepp(2) v(2)/Ryp 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0