imusolve.m

惯性导航精解算程序

function [location,velocity,acceleration,attitude,q1,Cb_n] = Imusolve(trajectory_type)
%%没有用欧拉法估计
%读取加速度计和陀螺仪的数据

if trajectory_type==1;
    load acc_gyro_line ;
elseif trajectory_type==2
    load acc_gyro_line2;
elseif trajectory_type==3
    load acc_gyro_circle;
end
%存储加速度计和陀螺仪数据，第一行记录轨迹类型，采样时间，采样点数的参数以及初始位置
%第二行记录初始速度,加速度，第三行记录初始姿态,角速度

parameter2 = acc_gyro_data(1:5,1:6);
trajectory_t = parameter2(1,1);
if trajectory_t~=trajectory_type
    error('轨迹类型不一致')
end
caiyangT = parameter2(1,2);
step = parameter2(1,3);
location0 = parameter2(1,4:6)';
velocity0 = parameter2(2,1:3)';

acceleration0 = parameter2(2,4:6)';
attitude0 = parameter2(3,1:3)';
angle_ve0 = parameter2(3,4:6)';
Fe_chang = parameter2(4,1);
Fe_randn = parameter2(4,2);
We_chang = parameter2(5,1);
We_randn = parameter2(5,2);
accelerometer = acc_gyro_data(6:step+5,1:3);
gyro = acc_gyro_data(6:step+5,4:6);
%%
global Ra e wie;
Ra = 6378140;%6000000%地球参考椭球长轴半径
e = 1/298.257;%0%地球椭球的椭圆度

wie = 15.04088/3600/180*pi; %0%地球自转角速度
%初始化q0，Cb_n0,存储的
Storedata_l(1,:) = location0';
Storedata_v(1,:) = velocity0';
Storedata_a(1,:) = acceleration0';
Storedata_z(1,:) = attitude0';
theta = attitude0(1);
gama = attitude0(2);
fai = attitude0(3);

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);
%%
%具体解算过程
hWaitBar = waitbar(0,'Please wait...');

for i=1:step
    Ve0 = velocity0(1);
    Vn0 = velocity0(2);
    Vu0 = velocity0(3);
    latitude = location0(1);
    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 = gyro(i,:)' - 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'*accelerometer(i,:)';%导航系中的加速度??????????   
     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 ;
     Storedata_l(i+1,:) = location';
     Storedata_v(i+1,:) = velocity';
     Storedata_a(i+1,:) = acceleration';   
     Storedata_z(i+1,:) = attitude'; 
%%
%更新
    location0 = location;
    velocity0 = velocity;
    acceleration0 = acceleration;
    q0 = q1;
    Cb_n0 = Cb_n;
    waitbar(i/step,hWaitBar,[num2str( fix(i/step*100) ),'%']);
    
end
%%
close(hWaitBar);
%存储解算后的数据Storedata_solve,没有估算角速率











parameter = zeros(1,12);
parameter(1) = trajectory_type;
parameter(2) = caiyangT;
parameter(3) = step;
Storedata_solve = [parameter;Storedata_l Storedata_v Storedata_a Storedata_z];
if trajectory_type==1 
    save imusolve_line.mat Storedata_solve -ASCII -DOUBLE -MAT;
elseif trajectory_type==2
    
    save imusolve_line2.mat Storedata_solve -ASCII -DOUBLE -MAT;
elseif trajectory_type==3
    save imusolve_circle.mat Storedata_solve -ASCII -DOUBLE -MAT;
end