imualiment2.m

惯性导航精解算程序

function [q0,Cb_n0] = ImuAliment2(theta,gama,fai)
q0=zeros(4,1);
Cb_n0 = [sin(fai)*sin(theta)*sin(gama)+cos(fai)*cos(gama)   cos(fai)*sin(theta)*sin(gama)-sin(fai)*cos(gama)   -cos(theta)*sin(gama);
        sin(fai)*cos(theta)                                cos(fai)*cos(theta)              sin(theta);
        cos(fai)*sin(gama)-sin(fai)*sin(theta)*cos(gama)   -sin(fai)*sin(gama)-cos(fai)*sin(theta)*cos(gama)    cos(theta)*cos(gama) ];
Cb_n0 = Cb_n0/(Cb_n0'*Cb_n0)^0.5;%保持正交化
%%%%%%%%%%SIN_DVL_GPS组合导航在远程AUV上的应用研究%%%%%%%%%%%%%%%不一样
q0(1) = cos(fai/2)*cos(theta/2)*cos(gama/2)+  sin(fai/2)*sin(theta/2)*sin(gama/2); 
q0(2) = cos(fai/2)*sin(theta/2)*cos(gama/2)+  sin(fai/2)*cos(theta/2)*sin(gama/2); 
q0(3) = cos(fai/2)*cos(theta/2)*sin(gama/2)-  sin(fai/2)*sin(theta/2)*cos(gama/2); 
q0(4) = cos(fai/2)*sin(theta/2)*sin(gama/2)-  sin(fai/2)*cos(theta/2)*cos(gama/2); 
q0 = q0/norm(q0);