imusolve2.m

惯性导航精解算程序

function [location,velocity,acceleration,attitude,q1,Cb_n] = Imusolve2(accelerometer0,gyro0,location0,velocity0,acceleration0,attitude0,q0,Cb_n0,caiyangT)
%%解算单步进行的程序，批处理方式用Imusolve


    Ve0 = velocity0(1);
    Vn0 = velocity0(2);
    Vu0 = velocity0(3);
    latitude = location0(1);
    global Ra e wie;
    RN = Ra*(1-2*e+3*e*sin(latitude)^2);%参考椭球子午面内的曲率半径
    RE = Ra*(1+e*sin(latitude)^2);%参考椭球子午面的法线平面内的曲率半径
    g = 9.7803267714*(1+0.00193185138639*sin(latitude)^2)/sqrt(1-0.00669437999013*sin(latitude)^2);
    gn=[0 0 -g]';%地理系上的重力
    
    
    dot_latitude = Vn0/RN;%纬度变化率
    dot_longitude = Ve0/(RE*cos(latitude));%经度变化率
    SWn_ie = [0;wie*cos(latitude);wie*sin(latitude)];%地球自转角速度在导航系上的分量
    SWn_en = [-dot_latitude;dot_longitude*cos(latitude);dot_longitude*sin(latitude)];%导航系相对地固系的角速度在导航系上的分量    
    SWb_nb = gyro0' - Cb_n0*(SWn_ie+SWn_en);%车体系相对当地水平系角速度
    Dtheta = SWb_nb*caiyangT;%角增量3x1
    detDtheta = sqrt(Dtheta'*Dtheta);%scalar vecter,norm(Dtheta)
    
    
    
    syDtheta = [0          -Dtheta(1) -Dtheta(2) -Dtheta(3);
                Dtheta(1)  0          Dtheta(3)  -Dtheta(2);
                Dtheta(2)  -Dtheta(3) 0          Dtheta(1);
                Dtheta(3)  Dtheta(2)  -Dtheta(1) 0         ];
     if detDtheta==0 
         q1=q0;
     else
        q1 = (cos(detDtheta/2)*eye(4) + sin(detDtheta/2)/detDtheta*syDtheta )*q0;
     end
     
     q1 = q1/norm(q1);
     Cb_n = [q1(1)^2+q1(2)^2-q1(3)^2-q1(4)^2  2*(q1(2)*q1(3)+q1(1)*q1(4))  2*(q1(2)*q1(4)-q1(1)*q1(3));
             2*(q1(2)*q1(3)-q1(1)*q1(4))      q1(3)^2-q1(4)^2+q1(1)^2-q1(2)^2  2*(q1(3)*q1(4)+q1(1)*q1(2));
             2*(q1(2)*q1(4)+q1(1)*q1(3))      2*(q1(3)*q1(4)-q1(1)*q1(2))      q1(4)^2-q1(3)^2-q1(2)^2+q1(1)^2];         
     Cb_n = Cb_n/(Cb_n'*Cb_n)^0.5;%保持正交化
     theta = asin(Cb_n(2,3));
     gama = atan(-Cb_n(1,3)/Cb_n(3,3));
     if Cb_n(1,3)<0 && Cb_n(3,3)<0 
        gama = gama+pi;
        elseif Cb_n(1,3)>0 && Cb_n(3,3)<0
        gama = gama-pi;
     end
     fai = atan(Cb_n(2,1)/Cb_n(2,2));
        if  Cb_n(2,2)<0
             fai = fai+pi;
        end
        if fai<0 && Cb_n(2,2)>0
            fai = fai+2*pi;
        end 
        
     attitude = [theta gama fai]';
     Fn = Cb_n'*accelerometer0';%导航系中的加速度??????????   
     acceleration = Fn -cross(2*SWn_ie+SWn_en,velocity0) +gn;%速度变化量☆☆☆☆☆☆☆☆计算的是下一个时刻的
%    velocity = velocity0 + acceleration0*caiyangT;
     velocity = velocity0 + 0.5*(acceleration0+acceleration)*caiyangT;%取中间值做计算 
     dot_latitude = velocity0(2)/RN;%纬度变化率★★★★★★★★★★这里用的是velocity0
     dot_longitude = velocity0(1)/(RE*cos(latitude));%经度变化率★★★★★★★★★★这里用的是velocity0
     dot_latitude2 = velocity(2)/RN;%纬度变化率★★★★★★★★★★这里用的是velocity
     dot_longitude2 = velocity(1)/(RE*cos(latitude));%经度变化率★★★★★★★★★★这里用的是velocity
     
     % location = location0 + [dot_latitude dot_longitude 0]'*caiyangT;%这是一阶算法
     location = location0+[0.5*(dot_latitude+dot_latitude2) 0.5*(dot_longitude+dot_longitude2) 0]'*caiyangT ;