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

《挠曲补偿的完整传递对准程序》由会员上传分享，免费在线阅读，更多相关内容在应用文档-天天文库。

1、clearall;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=[222]';Kg=[

2、0.00010000.00010000.0001];Ka=[0.00010000.00010000.0001];G_Drift=[0.01*rad_deg/36000.01*rad_deg/36000.01*rad_deg/3600];A_Bias=[1e-4*g01e-4*g01e-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;%-------------------------------------

3、-------------------------------------%挠曲变形角初值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

4、*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=[0wie*cos(lati)wie*sin(lati)];%%%p对子惯导phi=45.7796*rad_deg;lamda=126.6705*rad_deg;wiep=[0wie*cos(phi)wie*sin(phi)];g1=g0+0.051799*

5、(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;

6、Rxp=Re*(1+e*(sin(phi))^2);Ryp=Re*(1-2*e+3*e*(sin(phi))^2);wepp=[-v(2)/Rypv(1)/Rxpv(1)*tan(phi)/Rxp];%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%q=[1000]';%%%%%%建立初始捷联阵%%%%%%%%%pitch00=pitch0;roll00=roll0;yaw00=yaw0;Tbn=[cos(roll00)*cos(yaw00)+sin(roll00)*sin(pitch00)*si

7、n(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)