8.2捷联惯导算法仿真 代码整理分析（一）

1.全局变量

global GM Re ff wie ge gp g0 ug arcdeg arcmin arcsec hur dph dpsh ugpsHz lsc % Global VARables
GM = 3.986004415e14;  Re = 6.378136998405e6;  wie = 7.2921151467e-5;  % WGS-84 model
ff = 1/298.257223563; ee = sqrt(2*ff-ff^2); e2 = ee^2; Rp = (1-ff)*Re;
ge = 9.780325333434361;赤道重力
gp = 9.832184935381024;极点重力 
g0 = ge;
ug = g0*1e-6;   % gravity, ug
arcdeg = pi/180;
arcmin = arcdeg/60; 
arcsec = arcmin/60;   % angle unit
hur = 3600; dph = arcdeg/hur; dpsh = arcdeg/sqrt(hur); % hour, deg/hour, deg/sqrt(hour)
ugpsHz = ug/sqrt(1); % ug/sqrt(Hz)
lsc = [' -k';' -b';' -r';'-.m';'--g';' :c']; % line shape & color

2.三维向量的反对称矩阵

function m = askew(v)
    m = [ 0,     -v(3),   v(2); 
         v(3),   0,     -v(1); 
        -v(2),   v(1),    0   ];

3.姿态角转换为姿态阵
输入姿态角向量att含三个分量，依次为俯仰角、横滚角和方位角，特别注意：程序中定义方位角北偏西为正（而非北偏东为正），参考公式为：

function Cnb = a2mat(att)
    s = sin(att); c = cos(att);
    si = s(1); sj = s(2); sk = s(3);   ci = c(1); cj = c(2); ck = c(3);
    Cnb = [ cj*ck-si*sj*sk,   -ci*sk,  sj*ck+si*cj*sk;   
           cj*sk+si*sj*ck,   ci*ck,  sj*sk-si*cj*ck;
          -ci*sj,           si,     ci*cj         ];

4.姿态阵转换为姿态角

function att = m2att(Cnb)
    if abs(Cnb(3,2))<=0.999999
        att = [ asin(Cnb(3,2)); -atan2(Cnb(3,1),Cnb(3,3)); -atan2(Cnb(1,2),Cnb(2,2)) ];
    else
        att = [ asin(Cnb(3,2));  atan2(Cnb(1,3),Cnb(1,1));  0 ];
    end

5.姿态角转换为四元数
参考公式：

function qnb = a2qua(att)
    s = sin(att/2); c = cos(att/2);
    si = s(1); sj = s(2); sk = s(3); ci = c(1); cj = c(2); ck = c(3); 
    qnb = [ ci*cj*ck - si*sj*sk;
          si*cj*ck - ci*sj*sk;
          ci*sj*ck + si*cj*sk;
          ci*cj*sk + si*sj*ck ];
    % qnb = m2qua(a2mat(att));

6.四元数转换为姿态角

function att = q2att(qnb)
    att = m2att(q2mat(qnb));

7.姿态阵转换为四元数

function qnb = m2qua(Cnb)
    C11 = Cnb(1,1); C12 = Cnb(1,2); C13 = Cnb(1,3); 
    C21 = Cnb(2,1); C22 = Cnb(2,2); C23 = Cnb(2,3); 
    C31 = Cnb(3,1); C32 = Cnb(3,2); C33 = Cnb(3,3); 
    if C11>=C22+C33
        q1 = 0.5*sqrt(1+C11-C22-C33);
        q0 = (C32-C23)/(4*q1); q2 = (C12+C21)/(4*q1); q3 = (C13+C31)/(4*q1);
    elseif C22>=C11+C33
        q2 = 0.5*sqrt(1-C11+C22-C33);
        q0 = (C13-C31)/(4*q2); q1 = (C12+C21)/(4*q2); q3 = (C23+C32)/(4*q2);
    elseif C33>=C11+C22
        q3 = 0.5*sqrt(1-C11-C22+C33);
        q0 = (C21-C12)/(4*q3); q1 = (C13+C31)/(4*q3); q2 = (C23+C32)/(4*q3);
    else
        q0 = 0.5*sqrt(1+C11+C22+C33);
        q1 = (C32-C23)/(4*q0); q2 = (C13-C31)/(4*q0); q3 = (C21-C12)/(4*q0);
    end
    qnb = [q0; q1; q2; q3];

8.四元数转换为姿态阵
参考公式：

function Cnb = q2mat(qnb)
    q11 = qnb(1)*qnb(1); q12 = qnb(1)*qnb(2); q13 = qnb(1)*qnb(3); q14 = qnb(1)*qnb(4); 
    q22 = qnb(2)*qnb(2); q23 = qnb(2)*qnb(3); q24 = qnb(2)*qnb(4);     
    q33 = qnb(3)*qnb(3); q34 = qnb(3)*qnb(4);  
    q44 = qnb(4)*qnb(4);
    Cnb = [ q11+q22-q33-q44,  2*(q23-q14),      2*(q24+q13);
           2*(q23+q14),     q11-q22+q33-q44,  2*(q34-q12);
           2*(q24-q13),      2*(q34+q12),     q11-q22-q33+q44 ];

9.旋转矢量转换为变换矩阵
参考公式：

function m = rv2m(rv)
    nm2 = rv'*rv;  % 旋转矢量的模方
    if nm2<1.e-8   % 如果模方很小，则可用泰勒展开前几项求三角函数
        a = 1-nm2*(1/6-nm2/120); b = 0.5-nm2*(1/24-nm2/720);
    else
        nm = sqrt(nm2);
        a = sin(nm)/nm;  b = (1-cos(nm))/nm2;
    end
    VX = askew(rv);
    m = eye(3) + a*VX + b*VX^2;

10.旋转矢量转换为变换四元数

function q = rv2q(rv)
    nm2 = rv'*rv;  % 旋转矢量的模方
    if nm2<1.0e-8  % 如果模方很小，则可用泰勒展开前几项求三角函数
        q0 = 1-nm2*(1/8-nm2/384); s = 1/2-nm2*(1/48-nm2/3840);
    else
        nm = sqrt(nm2);
        q0 = cos(nm/2); s = sin(nm/2)/nm;
    end
    q = [q0; s*rv];

11.变换四元数转换为旋转矢量
首先，将四元数转化为标量非负的四元数；其次，根据公式

，先由四元数的标量关系
求旋转矢量模值的一半，再由矢量关系
求等效旋转矢量

function rv = q2rv(q) 
    if q(1)<0,  q = -q;  end
    nmhalf = acos(q(1));  % 等效旋转矢量模值的一半
    if nmhalf>1e-20,  b = 2*nmhalf/sin(nmhalf);
    else            b = 2;                end
    rv = b*q(2:4);

12.四元数共轭

function qout = qconj(qin)
qout = [qin(1); -qin(2:4)];   

13.四元数归一化

function qnb = qnormlz(qnb)
    nm = qnb'*qnb;
    if nm<1e-6,  qnb = [1; 0; 0; 0];  % 表示姿态的四元数，其模值应约为1
    else        qnb = qnb/sqrt(nm);    end

14.四元数相乘

function q = qmul(q1, q2)
    q = [ q1(1) * q2(1) - q1(2) * q2(2) - q1(3) * q2(3) - q1(4) * q2(4);
        q1(1) * q2(2) + q1(2) * q2(1) + q1(3) * q2(4) - q1(4) * q2(3);
        q1(1) * q2(3) + q1(3) * q2(1) + q1(4) * q2(2) - q1(2) * q2(4);
        q1(1) * q2(4) + q1(4) * q2(1) + q1(2) * q2(3) - q1(3) * q2(2) ]

15.四元数乘向量（三维向量坐标变换）
参考公式：

function vo = qmulv(q, vi)
qi = [0;vi];
      qo = qmul(qmul(q,qi),qconj(q));
     vo = qo(2:4,1);
% vo = q2mat(q)*vi;  

16.四元数加失准角误差

function qpb = qaddphi(qnb, phi)
qpb = qmul(rv2q(-phi),qnb);

17.四元数减失准角误差

function qnb = qdelphi(qpb, phi)
    qnb = qmul(rv2q(phi), qpb);

18.由计算四元数和真实四元数计算失准角误差

function phi = qq2phi(qpb, qnb)
    qerr = qmul(qnb, qconj(qpb));
    phi = q2rv(qerr);

19.圆锥/划船误差补偿
参考公式：

function [phim, dvbm] = cnscl(wm, vm)
  cs = [  [2,    0,    0,    0,   0    ]/3
          [9,    27,   0,    0,    0    ]/20
          [54,   92,   214,  0,    0    ]/105
          [250,  525,  650,  1375, 0    ]/504
          [2315, 4558, 7296,  7834, 15797]/4620  ];  % 2-6子样补偿系数
wmm = sum(wm,1);  vmm = sum(vm,1); 
%sum(A,1) 对 A 的列中的连续元素进行求和并返回一个包含每列之和的行向量。
 dphim = zeros(1,3);  scullm = zeros(1,3);
    n = size(wm, 1);  % 子样数，获取wm的行数，并赋值给n，为字样数。
    if n>1
        csw = cs(n-1,1:n-1)*wm(1:n-1,:); csv = cs(n-1,1:n-1)*vm(1:n-1,:);
        dphim = cross(csw,wm(n,:));  % 圆锥补偿量
        scullm = cross(csw,vm(n,:))+cross(csv,wm(n,:));  % 划船补偿量
    end
    phim = (wmm+dphim)';  %圆锥补偿后的旋转矢量
    dvbm = (vmm+0.5*cross(wmm,vmm)+scullm)';  %旋转和划船补偿后的速度增量