组合导航——捷联惯导更新算法含PSINS代码解释

本人也是新接触组合导航，出现错误希望大家指出！
PSINS代码中，关于捷联惯导更新算法主要在insupdate.m中

1 圆锥和划桨误差补偿

    nn = size(imu,1);
    nts = nn*ins.ts;  nts2 = nts/2;  ins.nts = nts;
    [phim, dvbm] = cnscl(imu,0);    % coning & sculling compensation
    phim = ins.Kg*phim-ins.eb*nts; dvbm = ins.Ka*dvbm-ins.db*nts;  % calibration

2 地球相关参数更新

主程序

    %% earth & angular rate updating 
    vn01 = ins.vn+ins.an*nts2; pos01 = ins.pos+ins.Mpv*vn01*nts2;  % extrapolation at t1/2
    if ins.openloop==0, ins.eth = ethupdate(ins.eth, pos01, vn01);
    elseif ins.openloop==1, ins.eth = ethupdate(ins.eth, ins.pos0, ins.vn0); end
    ins.wib = phim/nts; ins.fb = dvbm/nts;  % same as trjsimu  wib:角速度；ins.fb：加速度
    ins.web = ins.wib - ins.Cnb'*ins.eth.wnie;
%     ins.wnb = ins.wib - ins.Cnb'*ins.eth.wnin;
    ins.wnb = ins.wib - (ins.Cnb*rv2m(phim/2))'*ins.eth.wnin;  % 2014-11-30

咱们一点点学习这段代码

2.1 速度和位置进行外推

（1）vn01和pos01是通过在时间间隔的中点（t1/2）对速度和位置进行外推得到的中间变量，用于后续地球参数的计算。
代码是一个简单的速度更新，v = v0+a*t

vn01 = ins.vn + ins.an*nts2; 

（2）这是用东北天速度去更新 [纬度（弧度制），经度（弧度制），高度]，

pos01 = ins.pos + ins.Mpv*vn01*nts2;

公式推导可见《捷联惯导算法与组合导航原理》3.1.4，这里直接给最后的位置变化公式了：
$$\begin{array}{c}\dot{\lambda }=\frac{v_x}{(R_N+h)\cos L}\\ \dot{L}=\frac{v_y}{R_M+h}\\ \dot{h}={\upsilon }_z\end{array}$$

简化后可得
$$\left[\begin{array}{c}\dot{\lambda }\\ \dot{L}\\ \dot{h}\end{array}\right]=ins.Mpv\ast \left[\begin{array}{c}{V}_x\\ V_y\\ V_z\end{array}\right]$$
ins.Mpv是：
$${\mathbf{M}}_{pv}=\left[\begin{array}{ccc}0& 1/{\mathbf{R}}_{Mh}& 0\\ & & \\ \sec L/{\mathbf{R}}_{Nh}& 0& 0\\ 0& 0& 1\end{array}\right]$$

2.2地球相关参数的更新

% 地球相关参数更新函数
ins.eth = ethupdate(ins.eth, pos01, vn01);

% 地球相关参数更新函数
function eth = ethupdate(eth, pos, vn)
% Update the Earth related parameters, much faster than 'earth'.
%
% Prototype: eth = ethupdate(eth, pos, vn)
% Inputs: eth - input earth structure array
%         pos - geographic position [lat;lon;hgt]
%         vn - velocity
% Outputs: eth - parameter structure array
%
% See also  ethinit, earth.

% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 23/05/2014
    if nargin==2,  vn = [0; 0; 0];  end
    eth.pos = pos;  eth.vn = vn;
    eth.sl = sin(pos(1));  eth.cl = cos(pos(1));  eth.tl = eth.sl/eth.cl; 
    eth.sl2 = eth.sl*eth.sl;  sl4 = eth.sl2*eth.sl2;
    sq = 1-eth.e2*eth.sl2;  RN = eth.Re/sqrt(sq); 
    eth.RNh = RN+pos(3);  eth.clRNh = eth.cl*eth.RNh;
    eth.RMh = RN*(1-eth.e2)/sq+pos(3);
%     eth.wnie = [0; eth.wie*eth.cl; eth.wie*eth.sl];
    eth.wnie(2) = eth.wie*eth.cl; eth.wnie(3) = eth.wie*eth.sl;
%     eth.wnen = [-vn(2)/eth.RMh; vn(1)/eth.RNh; vn(1)/eth.RNh*eth.tl];
    eth.wnen(1) = -vn(2)/eth.RMh; eth.wnen(2) = vn(1)/eth.RNh; eth.wnen(3) = eth.wnen(2)*eth.tl;
%     eth.wnin = eth.wnie + eth.wnen;
    eth.wnin(1) = eth.wnie(1) + eth.wnen(1); eth.wnin(2) = eth.wnie(2) + eth.wnen(2); eth.wnin(3) = eth.wnie(3) + eth.wnen(3); 
%     eth.wnien = eth.wnie + eth.wnin;
    eth.wnien(1) = eth.wnie(1) + eth.wnin(1); eth.wnien(2) = eth.wnie(2) + eth.wnin(2); eth.wnien(3) = eth.wnie(3) + eth.wnin(3);
%     eth.gn = [0;0;-eth.g];
    eth.g = eth.g0*(1+5.27094e-3*eth.sl2+2.32718e-5*sl4)-3.086e-6*pos(3); % grs80
    eth.gn(3) = -eth.g;
%     eth.gcc = eth.gn - cros(eth.wnien,vn); % Gravitational/Coriolis/Centripetal acceleration
%     eth.gcc =  [ eth.wnien(3)*vn(2)-eth.wnien(2)*vn(3);  % faster than previous line
%                  eth.wnien(1)*vn(3)-eth.wnien(3)*vn(1);
%                  eth.wnien(2)*vn(1)-eth.wnien(1)*vn(2)+eth.gn(3) ];
    eth.gcc(1) = eth.wnien(3)*vn(2)-eth.wnien(2)*vn(3);
    eth.gcc(2) = eth.wnien(1)*vn(3)-eth.wnien(3)*vn(1);
    eth.gcc(3) = eth.wnien(2)*vn(1)-eth.wnien(1)*vn(2)+eth.gn(3);
    if isfield(eth, 'dgn')
        while eth.dgnt>eth.dgn(eth.dgnk,end) && eth.dgnk<eth.dgnlen, eth.dgnk=eth.dgnk+1; end
        eth.gcc = eth.gcc + eth.dgn(eth.dgnk,1:3)';
        eth.gn = eth.gn + eth.dgn(eth.dgnk,1:3)';
    end

（1）初始参数计算

	% 把sin,cos,tan值先计算出来
	eth.sl = sin(pos(1));  eth.cl = cos(pos(1));  eth.tl = eth.sl/eth.cl; 
	% sl2即sin^2
    eth.sl2 = eth.sl*eth.sl;  sl4 = eth.sl2*eth.sl2;
    % RN - 卯酉圈曲率半径,计算公式参考p48,RNh就是RN+高度h,RMh同理
    sq = 1-eth.e2*eth.sl2;  RN = eth.Re/sqrt(sq); 
    eth.RNh = RN+pos(3);  eth.clRNh = eth.cl*eth.RNh;
    % RM - 子午圈主曲率半径,计算公式参考p48
    eth.RMh = RN*(1-eth.e2)/sq+pos(3);  

（2）地球相关角速度

	%  eth.wnie - 地球自转角速度
    eth.wnie(2) = eth.wie*eth.cl; eth.wnie(3) = eth.wie*eth.sl;
    % wnen - 因地表弯曲引起的导航系旋转
    eth.wnen(1) = -vn(2)/eth.RMh; eth.wnen(2) = vn(1)/eth.RNh; eth.wnen(3) = eth.wnen(2)*eth.tl;
     % wnin - n系相当于i系的变化 wnin = eth.wnie + wnen
    eth.wnin(1) = eth.wnie(1) + eth.wnen(1); eth.wnin(2) = eth.wnie(2) + eth.wnen(2); eth.wnin(3) = eth.wnie(3) + eth.wnen(3); 
    % wnin = 2*eth.wnie + wnen
    eth.wnien(1) = eth.wnie(1) + eth.wnin(1); eth.wnien(2) = eth.wnie(2) + eth.wnin(2); eth.wnien(3) = eth.wnie(3) + eth.wnin(3);

这里求出的地球相关的一些角速度，也为下面求有害加速度服务
（3）有害加速度求解

    eth.g = eth.g0*(1+5.27094e-3*eth.sl2+2.32718e-5*sl4)-3.086e-6*pos(3); % grs80       % 正常重力公式出自书本p54-55
    eth.gn(3) = -eth.g;

%     eth.gcc = eth.gn - cros(eth.wnien,vn); % Gravitational/Coriolis/Centripetal acceleration
%     eth.gcc =  [ eth.wnien(3)*vn(2)-eth.wnien(2)*vn(3);  % faster than previous line
%                  eth.wnien(1)*vn(3)-eth.wnien(3)*vn(1);
%                  eth.wnien(2)*vn(1)-eth.wnien(1)*vn(2)+eth.gn(3) ];
    eth.gcc(1) = eth.wnien(3)*vn(2)-eth.wnien(2)*vn(3);
    eth.gcc(2) = eth.wnien(1)*vn(3)-eth.wnien(3)*vn(1);
    eth.gcc(3) = eth.wnien(2)*vn(1)-eth.wnien(1)*vn(2)+eth.gn(3);

cros函数是计算（wnien 叉乘 vn），物理意义是计算向心加速度；
正如前面介绍，wnien = 2 * eth.wnie + wnen； 2 * eth.wnie叉乘vn 为由载体运动和地球自转引起的哥式加速度，wnen叉乘vn 为载体运动引起的对地向心加速度；
eth.gcc = eth.gn - cros(eth.wnien,vn) 统称有害加速度,后面在用加速度计输出时需要去除这个有害加速度。书本p82

2.3 IMU补偿

phim: 表示陀螺仪测量的角增量经过相应的圆锥误差补偿后得到的结果。
dvbm: 表示加速度计测量的速度增量经过相应的旋转和划桨误差补偿后得到的结果。

    ins.wib = phim/nts; ins.fb = dvbm/nts;  % same as trjsimu  wib:角速度；ins.fb：加速度
    ins.web = ins.wib - ins.Cnb'*ins.eth.wnie; %  eth.wnie - 地球自转角速度
%     ins.wnb = ins.wib - ins.Cnb'*ins.eth.wnin;  % wbnb = wbib-wbin
    ins.wnb = ins.wib - (ins.Cnb*rv2m(phim/2))'*ins.eth.wnin;  % 2014-11-30

3姿态更新

3.1 完整算法

3.1.1 公式

首先是导航系（n）下的姿态微分方程
$${\dot{\mathbf{C}}}_b^n={\mathbf{C}}_b^n({\mathbf{\omega }}_{nb}^b\times )$$
通过一系列推导得到姿态递推算法


$C_{b(m)}^{b(m-1)}$是陀螺仪的角增量有关，代码中是phim；
$C_{n(m-1)}^{n(m)}$是地球自转角增量有关，代码中是ins.eth.wnin。
使用等效旋转矢量考虑了转动不可交换误差的补偿，详细可见书的2.5.1
代码中主要是用四元数进行计算，非定轴转动情况下算法精度更高。

3.1.2 PSINS代码

    %% (3)attitude updating
    ins.Cnb0 = ins.Cnb;
    % phim: 表示陀螺仪测量的角增量经过相应的圆锥误差补偿后得到的结果
    % eth.wnin - n系相当于i系的变化 wnin = eth.wnie + wnen
    ins.qnb = qupdt2(ins.qnb, phim, ins.eth.wnin*nts);
    [ins.qnb, ins.att, ins.Cnb] = attsyn(ins.qnb);

1、qupdt2
简单介绍下这个代码
（1）把陀螺仪角增量转换成四元数Qb(m-1)b(m)

	% rv2q(rv_ib)  
    n2 = rv_ib(1)*rv_ib(1)+rv_ib(2)*rv_ib(2)+rv_ib(3)*rv_ib(3);
    if n2<1.0e-8
        rv_ib0 = 1-n2*(1/8-n2/384); s = 1/2-n2*(1/48-n2/3840);
    else
        n = sqrt(n2); n_2 = n/2;
        rv_ib0 = cos(n_2); s = sin(n_2)/n;
    end
    rv_ib(1) = s*rv_ib(1); rv_ib(2) = s*rv_ib(2); rv_ib(3) = s*rv_ib(3);

（2）四元数乘法，计算Qn(m-1)b(m-1) * Qb(m-1)b(m)，（这里的 * 表示的是四元数乘法）

% qnb1 = qmul(qnb0, q);
    qb1 = qnb0(1) * rv_ib0   - qnb0(2) * rv_ib(1) - qnb0(3) * rv_ib(2) - qnb0(4) * rv_ib(3);
    qb2 = qnb0(1) * rv_ib(1) + qnb0(2) * rv_ib0   + qnb0(3) * rv_ib(3) - qnb0(4) * rv_ib(2);
    qb3 = qnb0(1) * rv_ib(2) + qnb0(3) * rv_ib0   + qnb0(4) * rv_ib(1) - qnb0(2) * rv_ib(3);
    qb4 = qnb0(1) * rv_ib(3) + qnb0(4) * rv_ib0   + qnb0(2) * rv_ib(2) - qnb0(3) * rv_ib(1);

四元数乘法公式：

（3）把地球自转角增量转换成四元数Qn(m)n(m-1)

% rv2q(-rv_in)
    n2 = rv_in(1)*rv_in(1)+rv_in(2)*rv_in(2)+rv_in(3)*rv_in(3);
    if n2<1.0e-8
        rv_in0 = 1-n2*(1/8-n2/384); s = -1/2+n2*(1/48-n2/3840);
    else
        n = sqrt(n2); n_2 = n/2;
        rv_in0 = cos(n_2); s = -sin(n_2)/n;
    end
    rv_in(1) = s*rv_in(1); rv_in(2) = s*rv_in(2); rv_in(3) = s*rv_in(3); 

这里稍微要注意下，这个执行的是rv2q(-rv_in)，也就是对角增量加了个负号；
这个与四元数性质有关：

由上面可知：
$$Q_{n(m)}^{n(m-1)}可由\varphi 求得；Q_{n(m-1)}^{n(m)}可由-\varphi 求得$$

（4）最后再进行四元数乘法
$$Q_{n(m)}^{n(m-1)}\circ Q_{n(m-1)}^{b(m-1)}\circ Q_{b(m-1)}^{b(m)}$$
理解起来就是上一周期欧拉角的四元数 $Q_{n(m-1)}^{b(m-1)}$ 加上陀螺仪角度增量 $Q_{b(m-1)}^{b(m)}$减去地球自转等有害角增量 $Q_{n(m)}^{n(m-1)}$

2、attsyn
在此处就是把四元数（qnb）转换成：欧拉角（att）、方向余弦矩阵（Cnb）

具体转换规则可以看我的另一篇博文：组合导航——欧拉角、姿态阵、四元数的转换关系

低成本简化算法

在低成本 MEMS 惯导系统中,陀螺仪精度(零偏重复性)为 0.1°/s 量级,加速度计精度为5 mg 量级

PSINS代码

    %% (3)attitude updating  低成本简化算法
    ins.qnb = qupdt(ins.qnb, ins.wnb*nts);  % lower accuracy than the next line
    [ins.qnb, ins.att, ins.Cnb] = attsyn(ins.qnb);
    ins.avp = [ins.att; ins.vn; ins.pos];

1、这里qupdt中的 ins.qnb对应了上图中的 Qnb(m-1)，ins.wnb可以简单理解为imu输出的角速度
2、qupdt中是先将ins.wnb*nts这个角度增量 转换成四元数Qb(m-1)b(m)，再执行四元数乘法，输出Qnb(m)

速度更新

PSINS代码

    %% (1)velocity updating
    ins.fn = qmulv(ins.qnb, ins.fb);
%     ins.an = qmulv(rv2q(-ins.eth.wnin*nts2),ins.fn) + ins.eth.gcc;
% ins.anbar我的理解就是做一个简单的平滑，代码中没有用到，应该就是平滑后ins.an?
    ins.an = rotv(-ins.eth.wnin*nts2, ins.fn) + ins.eth.gcc;  ins.anbar = 0.9*ins.anbar + 0.1*ins.an;
    vn1 = ins.vn + ins.an*nts;

1、这里的qmulv函数是把载体坐标系（b）上的加速度转换到导航系（n），也就是把imu的数据从载体坐标系转换到大地坐标系
2、rotv是吧ins.fn通过旋转向量-ins.eth.wninnts2旋转。先是将ins.fn这个旋转向量转换成四元数形式，再与-ins.eth.wninnts2转换成的四元数相乘，最后再乘ins.fn四元数的共轭转换回旋转向量

位置更新

完整算法

首先是位置微分方程

改写成矩阵形式

经过离散化得到最终的位置更新方程
$$p_m=p_{m-1}+M_{pv(m-1/2)}(v_{m-1}^{n(m-1)}+v_m^{n(m)})\frac{T}{2}$$
这里的Mpv(m-1/2)是通过对Mpv通过线性外推求出，先对 L，h外推，再带入Mpv(m-1/2)的公式;
L，h外推程序可见上文地球相关参数更新算法主程序的第一行；

PSINS代码

位置更新这的代码比较简单，不做太多的介绍，

    %% (2)position updating
%     ins.Mpv = [0, 1/ins.eth.RMh, 0; 1/ins.eth.clRNh, 0, 0; 0, 0, 1];
    ins.Mpv(4)=1/ins.eth.RMh; ins.Mpv(2)=1/ins.eth.clRNh; % 更新Mpv矩阵
%     ins.Mpvvn = ins.Mpv*((ins.vn+vn1)/2+(ins.an-ins.an0)*nts^2/3);  % 2014-11-30
    ins.Mpvvn = ins.Mpv*(ins.vn+vn1)/2;
    ins.pos = ins.pos + ins.Mpvvn*nts;