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

clear all; clc; g0=9.78049; %重力加速度 rad_deg=0 %表示1°的弧度值，即π/180 wie=7-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=roll