初始对准程序探讨.docVIP

——————————————————————————————————————————————— 初始对准程序探讨 close all; clear all; clc; format long %初始化参数 %vx(1)=0.000048637; %vy(1)=0.000206947; %vz(1)=0.007106781; %初始速度 vx(1)=0; vy(1)=0; vz(1)=0; jd(1)=116.344695283*2*pi/360; L(1)=39.975172*2*pi/360; %初始经纬度 %B(1)=-91.637207*2*pi/360; %初始航向角 tian %C(1)=0.120992605*2*pi/360; %初始俯仰角 dong %D(1)=0.010445947*2*pi/360; %初始横滚角 bei %初始姿态角 B(1)=0; C(1)=0; D(1)=0; re=6378245; Wie=7.27221e-5; e=1/298.3; Ti=1/20; j=100; N=200; g=9.78049; Wibxb_INSc=randn(j,1); Wibyb_INSc=randn(j,1); Wibzb_INSc=randn(j,1);%陀螺在各个方向上测量的数据 Fbx_INSc=randn(j,1); Fby_INSc=randn(j,1); Fbz_INSc=randn(j,1);%加速度计在各个方向上测量的数据 for i=1:j Rx=re/(1-e*sin(L(1))^2); Ry=re/(1+2*e-3*e*sin(L(1))^2); T31=Fbx_INSc(i,1)/(g*Ti); T32=Fby_INSc(i,1)/(g*Ti); T33=Fbz_INSc(i,1)/(g*Ti); T21=(Wibxb_INSc(i,1)-Ti*T31*Wie*sin(L(1)))/(Ti*Wie*cos(L(1))); T22=(Wibyb_INSc(i,1)-Ti*T32*Wie*sin(L(1)))/(Ti*Wie*cos(L(1))); T23=(Wibzb_INSc(i,1)-Ti*T33*Wie*sin(L(1)))/(Ti*Wie*cos(L(1))); T11=T22*T33-T23*T32; T12=T23*T31-T21*T33; T13=T21*T32-T22*T31; Cbn=[T11 T12 T13;T21 T22 T23;T31 T32 T33];%粗对准后确定的姿态矩阵 Cnb=Cbn#39;; gnx=0; gny=0; gnz=g; gn=[gnx;gny;gnz];%3*1 gb=gn#39;*Cbn; %重力加速度在载体系的表示 1*3*3*3=1*3 Wien_x=0; Wien_y=Wie*cos(L(1)); Wien_z=Wie*sin(L(1)); Wien=[Wien_x;Wien_y;Wien_z];%3*1 Wieb=Wien#39;*Cbn; %地球自转角速度在机体系的表示 Wibb=[Wibxb_INSc(i,1) Wibyb_INSc(i,1) Wibzb_INSc(i,1)]#39;; %陀螺输出的各个轴表示 Fb=[Fbx_INSc(i,1) Fby_INSc(i,1) Fbz_INSc(i,1)]#39;; %加速度计输出的各个轴表示 %Wibb#39;=Wien#39;*Cbn; %[Fbx_INSc(i,1) Fby_INSc(i,1) Fbz_INSc(i,1)]=-1*gn#39;*Cbn; %姿态角的计算 if abs(Cnb(2,2))gt;1e-10 if Cnb(2,2)gt;0 B(i+1)=atan(Cnb(2,1)/Cnb(2,2)); elseif Cnb(2,1)gt;0 B(i+1)=atan(Cnb(2,1)/Cnb(2,2)