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

clear all;clc;g0=9.78049; %重力加速度rad_deg=0.01745329; %表示1的弧度值，即/180wie=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_mile*cos(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+roll_error)*cos(pitch0+pitch_error);TT=T*inv(Tbn)dp=TT(2,3)/rad_degdr=TT(3,1)/rad_degdy=TT(1,2)/rad_degq(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.12; P(2,2) = 0.12; 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*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); 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; k3=1/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/R