PSINS源码test_SINS_DR解析

前言

PSINS是西工大严恭敏老师开源的一套高精度惯导算法，算法涉及内容非常丰富，算法相关推导一般在严老师论文中可以找到，因为最近研究水下SINS/DVL组合导航相关算法，所以对代码中的demo示例test_SINS_DR进行了详细的阅读和分析。现整理如下，涉及到具体公式的推导的，我会给出算法推导的具体出处。

源码解析

整体介绍

这块主要涉及到SINS/DR组合导航，主要面对的场景是无人机，车辆等的SINS/DR组合导航，因为做水下组合导航，我一般假设AUV大部分时间工作在一个二维平面，机动性不是很高，我觉得这块代码具有很高的参考价值。
源码主要分为如下几个步骤：

1. 轨迹生成器：轨迹生成器模拟的是飞机的运动轨迹，通过设定动作组，动作组持续的时间等参数生成轨迹的描述参数和IMU数据，相当于一个反解算的过程。
2. 设定IMU噪声，设定DR噪声，包括IMU和DR之间的安装误差，以及DR的标度因子等。
3. 卡尔曼滤波的过程：状态向量为22维，包括惯性解算的15维：欧拉角，速度，位置，陀螺仪偏置，加速度计偏置，DR位置（3维），SINS和DR之间的安装误差（2维，没有考虑滚转角），DR标度因子（1维），时间不同步误差（1维）。观测量为惯性解算位置和DR观测位置之间的误差。
4. 比较状态变量和真值之间的差异，可视化协方差矩阵的变化趋势等，方便分析和调试。

代码中涉及到三个坐标系：DR坐标系，IMU坐标系，车体坐标系，分别对应代码中的dravp，insavp，avp数据。avp相当于真值（ground_truth）。代码中的使用场景可以描述为：IMU和车体坐标系有一个安装误差角，DR，可以理解为为编码器，或者多普勒测速仪，和IMU有一个安装误差角，并且DR还有标度因子误差。我为了简单起见，把IMU和车体之间的安装误差设置为0，这样在IMU没有设置噪声的理想条件下，可以认为IMU数据的角度解算，速度解算，位置解算得到了avp数据。

对源码参数的更改

在脚本test_SINS_DR中我把参数设置更改为如下：
inst = [0;0;0]; kod = 1; qe = 0; dT = 0; % od parameters
dinst和dkod的含义是明确的，就是SINS和DR之间的安装误差，以及DR的标度因子，可以从代码中看出来。因为从函数odsimu中可以看到IMU数据是左乘了a2mat(-d2r(inst/60))，AVP（1:3），也就是欧拉角真值也左乘a2mat(-d2r(inst/60))，这样比较这两个数据就可以知道惯导欧拉角解算的误差。但是AVP（4:9）是没有变的，也就是轨迹真值速度的位置。在函数drinit中DR_POS左乘kod*(1+dkod)a2mat(-d2r((inst+dinst)/60))，而SINS/DR组合导航估计出来的安装误差和标度因子的输入数据是IMU，DR数据，所以可以看到IMU和DR之间差的是kod(1+dkod)*a2mat(-d2r(dinst)/60)，也就是前面提到的SINS和DR之间的安装误差，以及DR的标度因子。所以卡尔曼滤波估计出来的安装误差，标度因子通过和dinst，dkod比较，就可以看出来估计精度。

test_SINS_DR脚本

1.glvs函数主要定义了一些全局变量
2.因为使用的二字样算法，所以nnts函数的输出ts为2。
3.odsimu函数中模拟得到里程计数据
4.imuerrset函数设定IMU的噪声
5.imuadderr函数吧设定的IMU噪声加入到IMU数据中
6.avpseterr函数中设定初始误差，在后面设置P矩阵可以作为参数使用
7.insinit函数中设置了有关SINS的全局变量
8.drinit函数初始化dr结构体，考虑了设置的噪声
9.kfinit函数中设置PQR，以及H观测转移矩阵
10.insupdate函数包括IMU的姿态解算，速度解算，位置解算
11.drupdate函数中是关于DR的解算，包括姿态，位置等
12.kffk函数中计算状态转移矩阵
13.kfupdate函数中主要实现了卡尔曼滤波的5个方程
14.kffeedback函数中实现状态的更新，使用了部分反馈校正方法。
15.insplot和kfplot是实现可视化参数的功能
代码如下：

% SINS/DR integrated navigation simulation.
% Please run 'test_SINS_trj.m' to generate 'trj10ms.mat' beforehand!!!
% See also  test_SINS_trj, test_DR.
% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 24/02/2012, 08/04/2014
close all;
glvs
psinstypedef('test_SINS_DR_def');
trj = trjfile('trj10ms.mat');
% 二字样算法
% ts = 0.01
% ts = 2
% nts = 0.02
[nn, ts, nts] = nnts(2, trj.ts);
% inst：IMU和车体之间的安装误差角 单位：角分
% dT：里程计数据和IMU数据之间的时间延迟
% dT = 0:意味这不加入时间不同步误差
inst = [3;60;6];  kod = 1;  qe = 0; dT = 0; % od parameters

% trj.od = [dS,t];
% dS:模拟仿真得到的里程计数据
% t:里程计数据对应的时间戳
trjod = odsimu(trj, inst, kod, qe, dT, 1);     % od simulation
% 设置IMU噪声，噪声含义如下：
% 陀螺仪偏置：0.01deg/h
% 加速度计偏置：50ug
% 陀螺仪高斯噪声： 0.001deg/sqrt(h)
% 加速度计高斯噪声：5ug/sqrt(Hz)
imuerr = imuerrset(0.01, 50, 0.001, 5);
imu = imuadderr(trjod.imu, imuerr);

% 设置轨迹噪声，噪声如下：
% 欧拉角噪声：30角秒；30角秒；10角分 
% 速度噪声：0m/s
% 位置噪声：10m  dpos0(1:2)/glv.Re
% 
% 只对初始值有关: ins.att; ins.vn; ins.pos
% P矩阵的设置
% kf.Rk：davp(7:9):位置初始估计误差
% 这里x,y,z都是10m
% 卡尔曼滤波中设置初始误差P
davp = avpseterr([30;30;10], 0, 10);  
% trjod.avp0:初始姿态 

% avpadderr(trjod.avp0,davp):9个数
% SINS init 全局变量的设置
% avpadderr:这里要详细看一下
ins = insinit(avpadderr(trjod.avp0,davp), ts); 
dinst = [0;0;120]; dkod = 0.01;

% (inst+dinst)/60： inst和dinst的关系？
% kod*(1+dkod)：kod和dkod的关系？
% dr数据结构初始化:avpadderr(trjod.avp0,davp)
% kod:单位：m/脉冲 kod*(1+dkod)
% DR参数的初始化
% inst = [3;60;6]
% dinst = [15;0;10]
% d2r:angle unit from degree to radian
% inst+dinst = [18,60,16]/60 = [0.3000,1.0000,0.2667],单位:arcmin
% 一般安装误差是deg量级的
% DR属性的设置
dr = drinit(avpadderr(trjod.avp0,davp), d2r((inst+dinst)/60), kod*(1+dkod), ts); % DR Init

% kfinit: PQR参数的设置,观测矩阵Hk的设置
kf = kfinit(nts, davp, imuerr, dinst, dkod, dT); % kf init
len = length(imu); 
[dravp, insavp, xkpk] = prealloc(fix(len/nn), 10, 10, 2*kf.n+1);
ki = timebar(nn, len, 'SINS/DR Simulation.');
for k=1:nn:len-nn+1
    k1 = k+nn-1;
    % trjod.od
    % dS：里程计信息 观测数据
    % 注意下面dS并没有加入标度因子，在函数drupdate中加入了
    wvm = imu(k:k1,1:6);  dS = sum(trjod.od(k:k1,1)); t = imu(k1,end);
    % 捷联惯导姿态解算，速度解算，位置解算
    ins = insupdate(ins, wvm);
    % 具有相同的失准角误差
    dr.qnb = ins.qnb; % DR quaternion setting!!!! 在drupdate函数中要改变
   
    % 航位推算：姿态和位置更新
    % wvm：陀螺仪角增量
    % dS：里程计数据
    dr = drupdate(dr, wvm(:,1:3), dS); % 注意：dr.pos
    kf.Phikk_1 = kffk(ins);  % 计算状态转移矩阵
    kf = kfupdate(kf);       %==============前向预测阶段===========卡尔曼滤波前两个公式===========%
    if mod(k1,10)==0  % 多长时间做一次观测====模拟实际里程计速率
        % ins.pos = ins.pos + ins.Mpvvn*nts;  
%         % kf.Hk(:,22) ：观测转换矩阵
        % 参考严老师书公式7.3.12
        %======================注意负号=================
        % kf.Hk(:,22) = -ins.Mpvvn; % ins.Mpvvn:位置不同步误差
        kf = kfupdate(kf, ins.pos-dr.pos);  %ins.pos-dr.pos：观测误差
        % 只反馈更新速度？
        [kf, ins] = kffeedback(kf, ins, 1, 'v');
    end
    insavp(ki,:) = [ ins.avp; t ]'; % ins.avp = [ins.att; ins.vn; ins.pos]; 
    dravp(ki,:) = [ dr.avp; t ]';   % dr.avp = [dr.att; dr.vn; dr.pos];
    % kf.xk(idx)
    xkpk(ki,:) = [kf.xk; diag(kf.Pxk); t]';
    ki = timebar;
end
fprintf("dr.distance = %d m\n",dr.distance);
insplot(dravp, 'DR', insavp);
% dinst:SINS和DVL之间的安装误差角真值
% dkod:DVL标度因子真值
kfplot(xkpk, trjod.avp, insavp, dravp, imuerr, dinst, dkod, dT);

figure;
plot(trj.avp(:,3),'b-*');

glvf函数

glvf函数主要定义了一些全局变量，像地球自转角速度等，和地球计算相关的内容可以参考严老师第3章。部分代码如下所示：

    %sq：参考严老师书第3章
    glv.Re = Re;                    % the Earth's semi-major axis 地球长半轴
    glv.f = f;                      % flattening 地球椭圆扁率
    glv.Rp = (1-glv.f)*glv.Re;      % semi-minor axis 地球短半轴
    glv.e = sqrt(2*glv.f-glv.f^2);  % 1st eccentricity  % 第一偏心率
    glv.e2 = glv.e^2; 
    glv.ep = sqrt(glv.Re^2-glv.Rp^2)/glv.Rp;% 2nd eccentricity  %第二偏心率
    glv.ep2 = glv.ep^2; 
    glv.wie = wie;                  % the Earth's angular rate 地球自转角速度

drinit函数

该函数主要用于初始化DR结构，并把设置的SINS和DR之间的安装误差，以及DR刻度因子加入到结构中。

function dr = drinit(avp0, inst, kod, ts)
% Dead Reckoning(DR) structure array initialization.
%
% Prototype: dr = drinit(avp0, inst, ts)
% Inputs: avp0 - initial avp0 = [att0; vn0; pos0]
%         inst - ints=[dpitch;0;dyaw], where dpitch and dyaw are
%            installation error angles(in rad) from odometer to SIMU
%         kod - odometer scale factor in meter/pulse.
%         ts - SIMU % odometer sampling interval
% Output: dr - DR structure array
%
% See also  drupdate, drcalibrate, insinit.

% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 17/12/2008, 8/04/2014
	dr = [];
    avp0 = avp0(:);
	dr.qnb = a2qua(avp0(1:3)); 
    dr.vn = zeros(3,1); 
    dr.pos = avp0(7:9);
    
    [dr.qnb, dr.att, dr.Cnb] = attsyn(dr.qnb); % 姿态同步
    dr.avp = [dr.att; dr.vn; dr.pos];
	dr.kod = kod; % odometer scale factor in meter/pulse

    % inst：installation error angles(in rad) from odometer to SIMU 
    dr.Cbo = a2mat(-inst)*kod; % drupdate中Cbo使用
    % drupdate中prj使用
    % dr.prj :里程计坐标系到导航坐标系的转换
    
    % 先是里程计坐标系转到IMU坐标系
    % 再是IMU坐标系转到导航坐标系
    % dSn = qmulv(dr.qnb, dr.Cbo*dS);    
    
    % 只取里程计前向的速度(Y轴)
	dr.prj = dr.Cbo*[0;1;0]; % from OD to SIMU
    % dSn = qmulv(dr.qnb, dr.prj*dS);
    % dSn = qmulv(dr.qnb, a2mat(-inst)*[0;1;0]*dS*kod);  
    % 导航系下的航位推算位置
	dr.ts = ts; 
	dr.distance = 0; % drupdate中dr.distance更新
	dr.eth = earth(dr.pos);
    dr.Mpv = [0, 1/dr.eth.RMh, 0; 1/dr.eth.clRNh, 0, 0; 0, 0, 1];

odsimu函数

odsimu函数主要是把模拟设置的SINS和里程计之间的安装误差，以及里程计的标度因子误差加入到IMU数据和轨迹参数中。代码如下：

function trj = odsimu(trj, inst, kod, qe, dt, ifplot)
% Odometer distance increment simulator. (In this version, the lever-arm
% between odometer and SIMU is not considered.)
%
% Prototype: trj = odsimu(trj, inst, kod, qe, dt, ifplot)
% Inputs: trj - from trjsimu
%         inst - installation error angles from odometer(vehicle) to SIMU,
%                inst=[dpitch;droll;dyaw] in arcmin, default 0 for no 
%                installation error
%                是坐标系吗？ 从里程计坐标系到IMU坐标系??
%         kod - odometer scale factor, default 1 for no scale factor error
%         qe - quantitative equivalent, default 0 for no quantization
%         dt - odometer time delay w.r.t. SIMU, >0 for laging; <0 for 
%              leading, the default value is 0
%         ifplot - plot results after simulation
% Output: trj - the same as trj input, but with trj.imu, Eular angles 
%              trj.avp0(:,1:3) and trj.avp(:,1:3) rotated due to 
%              installation errors, besides, the Odometer increment field
%              'od' is attached to this structure.
%          
% See also  trjsimu, bhsimu, gpssimu.

% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 19/02/2014

% Cnb：命令规则：从n坐标系到b坐标系的转换矩阵
% inst：里程计和IMU之间的安装误差 单位：角分
% 从里程计到IMU，C(上面IMU，下面里程计)，假设里程计和设备之间没有安装误差
% kod：里程计刻度因子
% dt：里程计数据和IMU数据之间的时间延迟
% qe：
% 假设里程计和车体坐标系是重合的，即以里程计坐标系为基准
    if nargin<6,  ifplot=0;   end
    if nargin<5,  dt=0;       end
    if nargin<4,  qe=0;       end    % default 0 meter/pulse, for no quantization
    if nargin<3,  kod=1;      end    % default 1 for no scale factor error
    if nargin<2,  inst=0;     end
    if length(inst)==1,  inst=[1;1;1]*inst;  end
    % 注意负号
    % inst/60：转换到角度
    % Cb0b1：惯导到里程计之间的坐标转换？？
    % d2r:角度转弧度
    % a2mat:欧拉角转旋转矩阵
    % 
    Cb1b0 = a2mat(-d2r(inst/60));  Cb0b1 = Cb1b0';
    % SIMU rotation
    % b0：imu坐标系
    % b1：里程计坐标系
    trj.imu(:,1:6) = [trj.imu(:,1:3)*Cb0b1, trj.imu(:,4:6)*Cb0b1];    % Cb0b1
    
    % add
    % trj.imu(:,1:6) = [trj.imu(:,1:3)*Cb1b0', trj.imu(:,4:6)*Cb1b0']; % Cb0b1
    % attitude rotation
    % m2att：旋转矩阵转欧拉角
    % trj.avp(：,1:3)：真实欧拉角
    % trj.avp0(1:3)：初始的欧拉角
    % trj.avp和trj.imu对变换矩阵的处理是一致的
    % 在没有误差的情况下，IMU数据是对轨迹参数反向解算
    trj.avp0(1:3) = m2att(a2mat(trj.avp0(1:3))*Cb0b1);
    for k=1:length(trj.avp)
        trj.avp(k,1:3) = m2att(a2mat(trj.avp(k,1:3)')*Cb0b1)';
    end
    % distance increments
    % 没有考虑杆臂，所以下面不会改变
    % pos：纬度，经度，高度
    pos = [trj.avp0(7:9)'; trj.avp(:,7:9)];
    [RMh, clRNh] = RMRN(pos); %和全局参数有关的计算
    dpos = diff(pos);
    % pos单位：
    % 转换到大地坐标系(单位:m)
    dxyz = [[RMh(1:end-1), clRNh(1:end-1)].*dpos(:,1:2), dpos(:,3)];
    dS = sqrt(sum(dxyz.^2,2)); %转换到里程计坐标系下的里程值
    t = trj.avp(:,10); % 时间戳
    % 插补，没仔细看
    dS = interp1([t(1)-1;t;t(end)+1],[dS(1);dS;dS(end)], t+dt); % time delay
    
    dS = cumsum([0;dS]); %累积的里程
    % 更新周期：0.01s
    % 更新里程：0.1m
    % 约等于速度10m/s
    if qe==0
        dS = diff(dS*kod); % 加入里程计刻度因子误差
        if ifplot==1,  myfigure; plot(t,dS); xygo('Odometer / m');  end
    else
        dS = fix(diff(dS*kod/qe));
        if ifplot==1,  myfigure; plot(t,dS); xygo('Odometer / pulse');  end
    end
    trj.od = [dS,t];

RMRN函数

可以参考严老师书第三章的内容，这里也用到了glvf函数的内容，代码如下：

function [RMh, clRNh, RNh] = RMRN(pos)
% Just for fast calculation(batch processing) if input pos 
% is a postion vector.
%
% See also  earth, pos2dxyz.

% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 15/03/2014
global glv
	sl=sin(pos(:,1)); cl=cos(pos(:,1)); sl2=sl.*sl;
    % sq：参考严老师书公式3.1.23分母部分
	sq = 1-glv.e2*sl2; sq2 = sqrt(sq);
    % RM：子午圈曲率半径
    % RMh:子午圈曲率半径+高程
    % RMh:参考严老师书公式3.1.23
	RMh = glv.Re*(1-glv.e2)./sq./sq2+pos(:,3); 
    % RN：卯酉圈曲率半径
    % RNh:卯酉圈曲率半径+高程
    % RNh:参考严老师书公式3.1.14
	RNh = glv.Re./sq2+pos(:,3);    
    clRNh = cl.*RNh;  %经度用

imuerrset函数

该函数是设置IMU的噪声，常用的如零偏稳定性、角度随机游走噪声、标度因子、器件非正交误差，零偏等。加速度计的非正交误差参数有3个，陀螺仪的非正交误差有6个，这是因为是以加速度计的轴作为基准来定义的。

默认设置的噪声参数如下：
1.陀螺仪偏置：0.01deg/h
2.加速度计偏置：50ug
3. 陀螺仪高斯噪声： 0.001deg/sqrt(h)
4. 加速度计高斯噪声：5ug/sqrt(Hz)
代码中主要做了单位的转换，转换为标准单位，代码如下：

%为IMU增加噪声
% 示例：imuerrset(0.03, 100, 0.001, 5)
%
%
function imuerr = imuerrset(eb, db, web, wdb, sqrtR0G, TauG, sqrtR0A, TauA, dKGii, dKAii, dKGij, dKAij, KA2)
% SIMU errors setting, including gyro & acc bias, noise and installation errors, etc.
%
% Prototype: imuerr = imuerrset(eb, db, web, wdb, sqrtR0G, TauG, sqrtR0A, TauA, dKGii, dKAii, dKGij, dKAij)
% 注意输入的单位
% imuerrset(0.03, 100, 0.001, 5);
% 0.03deg/h
% 100ug
% 0.001 deg/sqrt(h)
% 5 ug/sqrt(Hz)
% Inputs: including information as follows
%     eb - gyro constant bias (deg/h) 常值偏置
%     db - acc constant bias (ug)     常值偏置
%     web - angular random walk (deg/sqrt(h))  角度随机游走 高斯噪声 数据手册数据
%     wdb - velocity random walk (ug/sqrt(Hz)) 速度随机游走 高斯噪声
%     sqrtR0G,TauG - gyro correlated bias, sqrtR0G in deg/h and TauG in s
%     sqrtR0A,TauA - acc correlated bias, sqrtR0A in ug and TauA in s
%     dKGii - gyro scale factor error (ppm)
%     dKAii - acc scale factor error (ppm)
%     dKGij - gyro installation error (arcsec) 安装误差 角秒
%     dKAij - acc installation error (arcsec)  安装误差 角秒
%     KA2 - acc quadratic coefficient (ug/g^2) 二次系数
%     where, 
%             |dKGii(1) dKGij(4) dKGij(5)|         |dKAii(1) 0        0       |
%       dKg = |dKGij(1) dKGii(2) dKGij(6)| , dKa = |dKAij(1) dKAii(2) 0       |
%             |dKGij(2) dKGij(3) dKGii(3)|         |dKAij(2) dKAij(3) dKAii(3)|
% Output: imuerr - SIMU error structure array
%
% Example: 重要！！！
%     For inertial grade SIMU, typical errors are:
%       eb=0.01dph, db=50ug, web=0.001dpsh, wdb=10ugpsHz
%       scale factor error=10ppm, askew installation error=10arcsec
%       sqrtR0G=0.001dph, taug=1000s, sqrtR0A=10ug, taug=1000s
%    then call this funcion by
%       imuerr = imuerrset(0.01,100,0.001,10, 0.001,1000,10,1000, 10,10,10,10);
%  eb, db, web, wdb, sqrtR0G, TauG, sqrtR0A, TauA, dKGii, dKAii, dKGij, dKAij, KA2
% See also  imuadderr, avpseterr, insinit, kfinit.

% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 11/09/2013, 06/03/2014, 15/02/2015, 22/08/2015, 17/08/2016
global glv
    if nargin==1  % for specific defined case 
        switch eb
            case 1,  imuerr = imuerrset(0.01, 100, 0.001, 10);
            case 2,  imuerr = imuerrset(0.01,100,0.001,10, 0.001,300,10,300, 10,10,10,10);
        end
        return;
    end
    o31 = zeros(3,1); o33 = zeros(3);
    imuerr = struct('eb',o31, 'db',o31, 'web',o31, 'wdb',o31,...
        'sqg',o31, 'taug',inf(3,1), 'sqa',o31, 'taua',inf(3,1), 'dKg',o33, 'dKa',o33, 'dKga',zeros(15,1),'KA2',o31); 
    %% constant bias & random walk（角度随机游走）
    % 基础噪声
    % 参考：glvf函数
    % glv.ug:1.0e-6*glv.g0
    % glv.dph:pi/3600/180:4.8481e-06
    % glv.deg = pi/180; glv.hur = 3600;
    % glv.dpsh = glv.deg/sqrt(glv.hur);  % arcdeg / sqrt(hour)
    % glv.ug = 1.0e-6*glv.g0;            % micro g
    % glv.Hz = 1/1;
    % glv.ugpsHz = glv.ug/sqrt(glv.Hz);  % ug / sqrt(Hz)
    
    % 只是单位的转换，转化为标准单位
    % eb(1:3):deg/h转rad/s
    % db(1:3):ug转g
    % web(1:3):deg/sqrt(h)转rad/sqrt(s)
    % wdb(1:3):ug/sqrt(Hz)转g/sqrt(Hz)
    imuerr.eb(1:3) = eb*glv.dph;    imuerr.web(1:3) = web*glv.dpsh;
    imuerr.db(1:3) = db*glv.ug;     imuerr.wdb(1:3) = wdb*glv.ugpsHz;
    %% correlated bias
    if exist('sqrtR0G', 'var')
        if TauG(1)==inf, imuerr.sqg(1:3) = sqrtR0G*glv.dphpsh;   % algular rate random walk !!!
        elseif TauG(1)>0, imuerr.sqg(1:3) = sqrtR0G*glv.dph.*sqrt(2./TauG); imuerr.taug(1:3) = TauG; % Markov process
        end
    end
    if exist('sqrtR0A', 'var')
        if TauA(1)==inf, imuerr.sqa(1:3) = sqrtR0A*glv.ugpsh;   % specific force random walk !!!
        elseif TauA(1)>0, imuerr.sqa(1:3) = sqrtR0A*glv.ug.*sqrt(2./TauA); imuerr.taua(1:3) = TauA; % Markov process
        end
    end
    %% scale factor error
    % glv.ppm = 1.0e-6; 
    % parts per million
    if exist('dKGii', 'var')
        imuerr.dKg = setdiag(imuerr.dKg, dKGii*glv.ppm);
    end
    if exist('dKAii', 'var')
        imuerr.dKa = setdiag(imuerr.dKa, dKAii*glv.ppm);
    end
    %% installation angle error
    if exist('dKGij', 'var')
        % glv.sec:角秒
        dKGij = ones(6,1).*dKGij*glv.sec;
        imuerr.dKg(2,1) = dKGij(1); imuerr.dKg(3,1) = dKGij(2); imuerr.dKg(3,2) = dKGij(3); 
        imuerr.dKg(1,2) = dKGij(4); imuerr.dKg(1,3) = dKGij(5); imuerr.dKg(2,3) = dKGij(6);
    end
    if exist('dKAij', 'var')
        dKAij = ones(3,1).*dKAij*glv.sec;
        imuerr.dKa(2,1) = dKAij(1); imuerr.dKa(3,1) = dKAij(2); imuerr.dKa(3,2) = dKAij(3); 
    end
    
    %
    imuerr.dKga = [imuerr.dKg(:,1); imuerr.dKg(:,2);   imuerr.dKg(:,3);
                   imuerr.dKa(:,1); imuerr.dKa(2:3,2); imuerr.dKa(3,3)];
    if exist('KA2', 'var')
        imuerr.KA2(1:3) = KA2*glv.ugpg2; 
    end

imuadderr函数

imuadderr函数是将imuerrset函数设定的IMU噪声加入到模拟轨迹中，代码中给出了注释，代码如下：

function imu = imuadderr(imu, imuerr, ts)
% SIMU outputs adding errors simulation, denoted as:
%    imu = K*imu + drift error.
%
% Prototype: imu = imuadderr(imu, imuerr, ts)
% Inputs: imu - raw SIMU data
%         imuerr - SIMU error struture array
% Output: imu - output SIMU data added errors
%
% See also  imuerrset, imuclbt, imudeldrift, avpseterr, trjsimu, insupdate.

% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 11/09/2013, 06/03/2014, 21/07/2015, 17/08/2016, 24/02/2017

% ts:采样率100Hz
    if nargin<3,  ts = imu(2,7)-imu(1,7);  end  % the last column implies sampling interval
    % m:数据个数
    % n:陀螺仪数据，加速度计数据，时间戳（7）
    [m, n] = size(imu); sts = sqrt(ts);
    % 连续域到离散域之间的转换
    % 备注：Hz = 1/ts
    
    % imuerr单位：
    % imuerr.eb:rad/s 连续域
    % eb在连续域的单位:rad/s
    % 常值漂移项单位是rad，所以rad/s*ts = rad,所以常值漂移项乘以ts,即为ts*imuerr.eb
    % imuerr.db:g 连续域
    % imuerr.web连续域单位:rad/sqrt(s) =rad/s/sqrt(Hz)
    % 到离散域转换需要/sqrt(s),转换过程如下：rad/s/sqrt(Hz)/(sqrt(1/Hz)) = rad/s，
    % 又因为是增量形式，所以再*s，相当于*sqrt(s)
    % 注意:1/(sqrt(1/Hz)) = 1/(sqrt(T))
    % imuerr.wdb:g/sqrt(Hz) 高斯噪声 连续域
    % 离散域单位：g/sqrt(Hz)/(sqrt(1/Hz)) = g
    
    % 备注：imu数据为增量形式！！！！
    % 所以这时候的关系是：高斯噪声非增量形式下是/sqrt(T),增量形式下是*sqrt(T)
    drift = [ ts*imuerr.eb(1) + sts*imuerr.web(1)*randn(m,1), ...
              ts*imuerr.eb(2) + sts*imuerr.web(2)*randn(m,1), ...
              ts*imuerr.eb(3) + sts*imuerr.web(3)*randn(m,1), ...
              ts*imuerr.db(1) + sts*imuerr.wdb(1)*randn(m,1), ...
              ts*imuerr.db(2) + sts*imuerr.wdb(2)*randn(m,1), ...
              ts*imuerr.db(3) + sts*imuerr.wdb(3)*randn(m,1) ];
    
    % 马尔可夫过程      
    if min(abs(imuerr.sqg))>0
        mvg = markov1(imuerr.sqg.*sqrt(imuerr.taug/2), imuerr.taug, ts, m);   % q = 2*sigma.^2.*beta
        drift(:,1:3) = drift(:,1:3) + mvg*ts;
    end
    % 马尔可夫过程      
    if min(abs(imuerr.sqa))>0
        mva = markov1(imuerr.sqa.*sqrt(imuerr.taua/2), imuerr.taua, ts, m);
        drift(:,4:6) = drift(:,4:6) + mva*ts;
    end
    if min(abs(imuerr.KA2))>0 
        imu(:,4:6) = [ imu(:,4)+imuerr.KA2(1)/ts*imu(:,4).^2, ...
                       imu(:,5)+imuerr.KA2(2)/ts*imu(:,5).^2, ...
                       imu(:,6)+imuerr.KA2(3)/ts*imu(:,6).^2 ];
    end
    if isfield(imuerr, 'dKg') % 增加刻度因子误差和非正交误差
        Kg = eye(3)+imuerr.dKg; Ka = eye(3)+imuerr.dKa;
        imu(:,1:6) = [imu(:,1:3)*Kg', imu(:,4:6)*Ka'];
    end
    imu(:,1:6) = imu(:,1:6) + drift;

insupdate函数

这个函数是惯性解算部分代码，包括姿态解算，速度解算，位置解算，在卡尔曼滤波中一般把解算作为前向预测过程。在姿态解算中考虑了圆锥误差，并使用二字样圆锥误差补偿算法，在速度更新中考虑了划桨误差和速度的旋转误差，同样使用了二字样划桨误差补偿算法。另外需要注意这里的陀螺仪数据是角增量形式，加速度是速度增量形式。代码如下：

function ins = insupdate(ins, imu)
% SINS Updating Alogrithm including attitude, velocity and position
% updating.
%
% Prototype: ins = insupdate(ins, imu)
% Inputs: ins - SINS structure array created by function 'insinit'
%         imu - gyro & acc incremental sample(s)
% Output: ins - SINS structure array after updating
%
% See also  insinit, cnscl, earth, trjsimu, imuadderr, avpadderr, q2att,
%           inslever, alignvn, aligni0, etm, kffk, kfupdate, insplot.

% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 22/03/2008, 12/01/2013, 18/03/2014, 09/09/2014
    nn = size(imu,1);
    % ins.ts:0.01
    nts = nn*ins.ts;  nts2 = nts/2;  ins.nts = nts;
    
    % phim：加速度计数据(增量数据)
    % dvbm：陀螺仪数据(增量数据)
    [phim, dvbm] = cnscl(imu,0);     % coning & sculling compensation
    % [phim, dvbm] = cnscl0(imu);    % coning & sculling compensation
    
    % ins.Kg：默认单位矩阵
    % ins.Ka：默认单位矩阵
    % ins.eb：默认为0 bias
    % ins.db：默认为0 bias
    % 注意：在KF中对ins.eb和ins.db进行了估计
    phim = ins.Kg*phim-ins.eb*nts; dvbm = ins.Ka*dvbm-ins.db*nts;  % calibration
    %% earth & angular rate updating 
    % ins.Mpv:insinit函数中
    vn01 = ins.vn+ins.an*nts2; pos01 = ins.pos+ins.Mpv*vn01*nts2;  % extrapolation at t1/2
    
    ins.eth = ethupdate(ins.eth, pos01, vn01);
    
    % phim：加速度计数据
    % dvbm：陀螺仪数据
    % ins.fb:补偿划桨效应后的比力
    % 注意这里ins.wib是两倍周期的角度增量
    ins.wib = phim/nts; ins.fb = dvbm/nts;  % same as trjsimu
    % ins.eth.wnie：地球自转角速度在导航系下的投影
    % ins.Cnb'*ins.eth.wnie：地球自转角速度在载体系下的投影
    ins.web = ins.wib - ins.Cnb'*ins.eth.wnie; %ie+eb = ib
    % ins.wnb = ins.wib - ins.Cnb'*ins.eth.wnin; %这个不行??
    % ins.Cnb*rv2m(phim/2)：
    % 注意：phim/2
    %
    % 以下公式应该更精确
    ins.wnb = ins.wib - (ins.Cnb*rv2m(phim/2))'*ins.eth.wnin;  % 2014-11-30   ib = in+nb 
    %% (1)velocity updating
    % ins.fb：载体系下的比力 :补偿划桨效应后的比力
    % ins.fn：导航系下的比力
    % 以下公式参考严老师书4.1.50
    ins.fn = qmulv(ins.qnb, ins.fb);
    % ins.an = qmulv(rv2q(-ins.eth.wnin*nts2),ins.fn) + ins.eth.gcc;
    % 参考严老师书4.1.50
    % nts2 = nts/2
    % 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);
    % eth.wnin = eth.wnie + eth.wnen;
    % 参考ethupdate函数和严老师书公式4.1.19和4.1.26
    % 注意rotv是用四元数表示的，和书中4.1.20的不太一样，书中做了一阶近似
    ins.an = rotv(-ins.eth.wnin*nts2, ins.fn) + ins.eth.gcc;   
    vn1 = ins.vn + ins.an*nts; 
    %% (2)position updating
    % 参考公式7.4.17
    ins.Mpv(4)=1/ins.eth.RMh; ins.Mpv(2)=1/ins.eth.clRNh;  % eth.clRNh = eth.cl*eth.RNh;
    % ins.Mpvvn = ins.Mpv*((ins.vn+vn1)/2+(ins.an-ins.an0)*nts^2/3);  % 2014-11-30
    % 中值法
    % ins.vn：上一个速度
    % ins.Mpvvn
    % ins.Mpv = [0, 1/ins.eth.RMh, 0; 1/ins.eth.clRNh, 0, 0; 0, 0, 1];
    % ins.Mpv：转换为弧度形式的经纬度
    ins.Mpvvn = ins.Mpv*(ins.vn+vn1)/2;
    % 注意这里是nts：两倍周期
    ins.pos = ins.pos + ins.Mpvvn*nts; 
    ins.vn = vn1;
    ins.an0 = ins.an;
    %% (3)attitude updating
    ins.Cnb0 = ins.Cnb;
    % ins.qnb = qupdt(ins.qnb, ins.wnb*nts);  % lower accuracy than next line
    % 参考严老师书公式4.1.8,注意这里使用四元数形式表示的
    % eth.wnin = eth.wnie + eth.wnen;
    % rv2q(-ins.eth.wnin*nts)*ins.qnb*rv2q(phim)
    % ins.eth.wnin*nts：参考严老师书公式4.1.11
    % nts：两倍周期
    ins.qnb = qupdt2(ins.qnb, phim, ins.eth.wnin*nts);
    [ins.qnb, ins.att, ins.Cnb] = attsyn(ins.qnb);
    ins.avp = [ins.att; ins.vn; ins.pos];

cnscl函数

圆锥误差和划桨误差的补偿在cnscl函数中。代码如下：

function [phim, dvbm, dphim, rotm, scullm] = cnscl(imu, coneoptimal)
% Coning & sculling compensation.
%
% Prototype: [phim, dvbm, dphim, rotm, scullm] = cnscl(imu, coneoptimal)
% Inputs:  imu(:,1:3) - gyro angular increments
%          imu(:,4:6) - acc velocity increments (may not exist)
%          coneoptimal - 1 for optimal coning compensation method,
%                        0 for polinomial compensation method.
%                        2 single sample+previous sample
% Outputs: phim - rotation vector after coning compensation
%          dvbm - velocity increment after rotation & sculling compensation
%          dphim - attitude coning error
%          rotm - velocity rotation error
%          scullm - velocity sculling error
%
% See also  cnscl0, conepolyn, scullpolyn, conetwospeed, conecoef,
%           insupdate, DR, imuadderr.

% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 07/12/2012, 04/12/2016
global glv
    if nargin<2,  coneoptimal=0;  end
    [n, m] = size(imu);
    if n>glv.csmax  % the maximun subsample number is glv.csmax, if n exceeds, then reshape imu
        [imu, n] = imureshape(imu, n, m);
    end
    %% coning compensation
    wm = imu(:,1:3); % 角增量数据
	if n==1 %n=2
        wmm = wm;
        if coneoptimal==2
            dphim = 1/12*cros(glv.wm_1,wm);  glv.wm_1 = wm;
        else
            dphim = [0, 0, 0];
        end
    else %n=2
        wmm = sum(wm,1); %两次采样的数据之和?
        if coneoptimal==0
            % glv.cs(n-1,1:n-1)：2/3 = 0.66667 ：
            % 参考严老师书：公式4.1.10
            cm = glv.cs(n-1,1:n-1)*wm(1:n-1,:);
            % dphim = cm x wm(n,:)(叉乘)
            % 0.66667*wm(1)x wm(2) 叉乘
            % wm(1:n-1,:):前一次采样
            % wm(n,:):当前采样
            dphim = cros(cm,wm(n,:));  % 
        elseif coneoptimal==1 % else: using polynomial fitting coning compensation method
            dphim = conepolyn(wm);
        else
            dphim = coneuncomp(wm);
        end
    end
    % 参考严老师书：公式4.1.10
    % 二字样圆锥误差补偿算法
    phim = (wmm+dphim)';  dvbm = [0; 0; 0];
    %% sculling compensation
    if m>=6
        vm = imu(:,4:6);  % 速度增量数据
        if n==1
            vmm = vm;
            if coneoptimal==0
                scullm = [0, 0, 0];
            else
                scullm = 1/12*(cros(glv.wm_1,vm)+cros(glv.vm_1,wm));  glv.vm_1 = vm;
            end
        else % n=2
            vmm = sum(vm,1); %注意这里两倍时间周期
            if coneoptimal==0
                sm = glv.cs(n-1,1:n-1)*vm(1:n-1,:);
                % 参考严老师书：公式4.1.43 
                % vm(1:n-1,:):前一个测量值
                % wm(n,:):后一个测量值
                scullm = (cros(cm,vm(n,:))+cros(sm,wm(n,:)));
            else  % else: using polynomial fitting sculling compensation method
                scullm = scullpolyn(wm, vm);
            end
        end
        % 参考严老师书：公式4.1.36
        rotm = 1.0/2*cros(wmm,vmm);
        % rotm：速度的旋转误差补偿量
        % scullm：划桨误差补偿量
        % 参考严老师书：公式4.1.50 后半拉公式
        % 注意dvbm是在IMU载体坐标系下的
        dvbm = (vmm+rotm+scullm)';
    end

function [imu, n] = imureshape(imu0, n0, m0)
% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 09/03/2014
global glv
    for n=glv.csmax:-1:1
        if mod(n0,n)==0,  break;  end
    end
    nn = n0/n;
    imu = zeros(n, m0);
    for k=1:n
        imu(k,:) = sum(imu0((k-1)*nn+1:k*nn,:),1);
    end

kfupdate函数

卡尔曼滤波的5个方程是在这个函数中描述的。代码中我做了相当多的注释。所有的公式都是可以在严老师的书中得到具体的推导过程的，有兴趣的可以详细看看书中的推导过程。这个函数是根据TimeMeasBoth可以选择前向预测过程，后验过程中的1个或者2个，另外还加入了协方差的限幅和状态向量的限幅，还有需要注意的是这里的状态向量是误差形式。具体代码和注释如下：

function kf = kfupdate(kf, yk, TimeMeasBoth)
% Discrete-time Kalman filter. 
%
% Prototype: kf = kfupdate(kf, yk, TimeMeasBoth)
% Inputs: kf - Kalman filter structure array
%         yk - measurement vector
%         TimeMeasBoth - described as follows,
%            TimeMeasBoth='T' (or nargin==1) for time updating only, ：什么意思？
%            TimeMeasBoth='M' for measurement updating only, 
%            TimeMeasBoth='B' (or nargin==2) for both time and 
%                             measurement updating.
% Output: kf - Kalman filter structure array after time/meas updating
% Notes: (1) the Kalman filter stochastic models is
%      xk = Phikk_1*xk_1 + wk_1
%      yk = Hk*xk + vk
%    where E[wk]=0, E[vk]=0, E[wk*wk']=Qk, E[vk*vk']=Rk, E[wk*vk']=0
%    (2) If kf.adaptive=1, then use Sage-Husa adaptive method (but only for 
%    measurement noise 'Rk'). The 'Rk' adaptive formula is:
%      Rk = b*Rk_1 + (1-b)*(rk*rk'-Hk*Pxkk_1*Hk')
%    where  minimum constrain 'Rmin' and maximum constrain 'Rmax' are
%    considered to avoid divergence.
%    (3) If kf.fading>1, the use fading memory filtering method.
%    (4) Using Pmax&Pmin to constrain Pxk, such that Pmin<=diag(Pxk)<=Pmax.
%
% See also  kfinit, kfupdatesq, kffk, kfhk, kfc2d, kffeedback, kfplot, RLS, ekf, ukf.

% Copyright(c) 2009-2015, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 08/12/2012, 29/08/2013, 16/04/2015, 01/06/2017, 11/03/2018

% 误差形式表示的卡尔曼滤波器
% yk：观测，用误差形式来表示
    if nargin==1
        TimeMeasBoth = 'T';
    elseif nargin==2
        TimeMeasBoth = 'B';
    end
    
    % 注意下面的kf.xk和kf.Pxk，和elseif TimeMeasBoth=='B'中的kf.xkk_1和kf.Pxkk_1的差异
    % 前者是最终的结果，没有后面的观测环节，后者后面还有观测环节
    if TimeMeasBoth=='T'   % Time Updating 只有前向预测部分 不进行观测
        % 前两个方程
        kf.xk = kf.Phikk_1*kf.xk;   %？ 
        % kf.Gammak？
        kf.Pxk = kf.Phikk_1*kf.Pxk*kf.Phikk_1' + kf.Gammak*kf.Qk*kf.Gammak';
    else
        if TimeMeasBoth=='M'        % Meas Updating 不进行前向预测 ,后面进行观测
            kf.xkk_1 = kf.xk;    
            kf.Pxkk_1 = kf.Pxk; 
        elseif TimeMeasBoth=='B'    % Time & Meas Updating 前向预测部分，后面进行观测
            % 卡尔曼滤波第一个方程
            % 参考严老师书公式5.3.29a
            kf.xkk_1 = kf.Phikk_1*kf.xk;   
            % kf.Gammak = 1
            % 卡尔曼滤波第二个方程
            % 参考严老师书公式5.3.29b
            kf.Pxkk_1 = kf.Phikk_1*kf.Pxk*kf.Phikk_1' + kf.Gammak*kf.Qk*kf.Gammak';
        else
            error('TimeMeasBoth input error!');
        end
        % 以下这种方式可以减少计算的时间复杂度
        kf.Pxykk_1 = kf.Pxkk_1*kf.Hk'; % 增益计算公式中的分子 实现矩阵的可复用性
        kf.Py0 = kf.Hk*kf.Pxykk_1; % 用于计算增益中的分母项中的一部分 5.3.29d
        kf.ykk_1 = kf.Hk*kf.xkk_1; % 更新公式中的一部分 参考严老师书公式5.3.29d
        % 从下式可以看出卡尔曼滤波是基于误差形式的
        % yk：误差形式
        kf.rk = yk-kf.ykk_1;  % 观测误差 用于计算后验X 参考严老师书公式 5.3.29d
        if kf.adaptive==1     % for adaptive KF, make sure Rk is diag 24/04/2015
            for k=1:kf.m
                if yk(k)>1e10, continue; end  % 16/12/2019
                ry = kf.rk(k)^2-kf.Py0(k,k);
                if ry<kf.Rmin(k,k), ry = kf.Rmin(k,k); end
                if ry>kf.Rmax(k,k),     kf.Rk(k,k) = kf.Rmax(k,k);
                else                	kf.Rk(k,k) = (1-kf.beta)*kf.Rk(k,k) + kf.beta*ry;
                end
            end
            kf.beta = kf.beta/(kf.beta+kf.b);
        end
        % Rk：测量噪声
        kf.Pykk_1 = kf.Py0 + kf.Rk; % Pykk_1：增益计算中的分母  参考严老师书公式5.3.29c
        % invbc:矩阵求逆
        kf.Kk = kf.Pxykk_1*invbc(kf.Pykk_1); % kf.Kk = kf.Pxykk_1*kf.Pykk_1^-1; 增益计算 参考严老师书公式5.3.29c
        kf.xk = kf.xkk_1 + kf.Kk*kf.rk; %计算后验估计 参考严老师书公式5.3.29d
        
        % 计算后验协方差矩阵
        % 参考严老师书公式5.3.31b
        % kf.Pykk_1:增益计算中的分母
        % Pxk:协方差矩阵
        kf.Pxk = kf.Pxkk_1 - kf.Kk*kf.Pykk_1*kf.Kk'; 
        % kf.fading = 1
        kf.Pxk = (kf.Pxk+kf.Pxk')*(kf.fading/2); % symmetrization & forgetting factor 'fading'
%         if kf.adaptive==1
%             krrk = kf.Kk*kf.rk*kf.rk'*kf.Kk';
%             for k=3:3
%                 krrki = krrk(k,k) + kf.Pxk(k,k) - kf.Pxkk_1(k,k);
%                 if krrki>kf.Qmax(k,k),     kf.Qk(k,k) = kf.Qmax(k,k);
%                 elseif krrki<kf.Qmin(k,k), kf.Qk(k,k) = (1-kf.beta)*kf.Qk(k,k) + kf.beta*kf.Qmin(k,k);
%                 else                	kf.Qk(k,k) = (1-kf.beta)*kf.Qk(k,k) + kf.beta*krrki;
%                 end
%             end
%         end

        % 约束条件
        % 状态变量限幅
        if kf.xconstrain==1  % 16/3/2018
            for k=1:kf.n
                if kf.xk(k)<kf.xmin(k)
                    kf.xk(k)=kf.xmin(k);
                elseif kf.xk(k)>kf.xmax(k)
                    kf.xk(k)=kf.xmax(k);
                end
            end
        end
        
        % 协方差矩阵限幅
        % 策略的依据??
        if kf.pconstrain==1  % 1/6/2017
            for k=1:kf.n
                if kf.Pxk(k,k)<kf.Pmin(k)
                    kf.Pxk(k,k)=kf.Pmin(k);
                elseif kf.Pxk(k,k)>kf.Pmax(k)
                    ratio = sqrt(kf.Pmax(k)/kf.Pxk(k,k));
                    kf.Pxk(:,k) = kf.Pxk(:,k)*ratio;  
                    kf.Pxk(k,:) = kf.Pxk(k,:)*ratio;
                end
            end
        end
    end

kffeedback函数

这个函数的作用是根据卡尔曼滤波的解算结果更新状态，因为卡尔曼滤波的结果是误差形式，不是最终的解算形式，另外一点是严老师这里用了部分反馈校正算法来更新状态，具体可以参考严老师论文《基于实测轨迹的高精度捷联惯导模拟器》，这种方法可以增加解算的平滑性。

function [kf, ins] = kffeedback(kf, ins, T_fb, fbstring)
% Kalman filter state estimation feedback to SINS.
%
% Prototype: [kf, ins] = kffeedback(kf, ins, T_fb, fbstring)
% Inputs: kf - Kalman filter structure array
%         ins - SINS structure array
%         T_fb - feedback time interval
%         fbstring - feedback tag string
% Outputs: kf, ins - Kalman filter % SINS structure array after feedback
%
% See also  kfinit, kffk, kftypedef.

% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 05/10/2014
% T_fb的含义
% 参考论文：基于实测轨迹的高精度捷联惯导模拟器

global navtype
    if kf.T_fb~=T_fb
        kf.T_fb = T_fb;
        
%         kf.T_fb:SINS更新周期Tm
%         kf.coef_fb: 反馈修正系数 
%         kf.xtau：反馈修正系数中的调节系数
%         kf.coef_fb = (1.0-exp(-kf.T_fb./kf.xtau));
%         kf.coef_fb = ar1coefs(kf.T_fb, kf.xtau);
%         kf.coef_fb:反馈系数
%         
        xtau = kf.xtau; % ？
        % kf.coef_fb：论文公式21 ：（1-aerfa）形式
    	xtau(kf.xtau<kf.T_fb) = kf.T_fb;   %保证kf.coef_fb小于0
        kf.coef_fb = kf.T_fb./xtau;  %2015-2-22  
    end
    if nargin>3 % commonly used、
        % 公式22
        xfb_tmp = kf.coef_fb.*kf.xk;  % coef_fb = 0 这里是全反馈！！！ kf.xk是误差形式的状态向量
        for k = 1:length(fbstring)
            switch fbstring(k) % 反馈的量
                % 下面的量都是非误差形式
                case 'a'
                    % ins.qnb：姿态角 四元数形式
                    idx = 1:3; ins.qnb = qdelphi(ins.qnb, xfb_tmp(idx));
                case 'v'
                    idx = 4:6; ins.vn = ins.vn - xfb_tmp(idx); 
                case 'p'
                    idx = 7:9; ins.pos = ins.pos - xfb_tmp(idx);
                case 'e'
                    idx = 10:12; ins.eb = ins.eb + xfb_tmp(idx);
                case 'd'
                    idx = 13:15; ins.db = ins.db + xfb_tmp(idx);
                case 'A'
                    idx = 1:2; ins.qnb = qdelphi(ins.qnb, [xfb_tmp(idx);0]);
                case 'V'
                    idx = 6; ins.vn(3) = ins.vn(3) - xfb_tmp(idx);
                case 'P'
                    idx = 9; ins.pos(3) = ins.pos(3) - xfb_tmp(idx);
                case 'E'
                    idx = 10:11; ins.eb(1:2) = ins.eb(1:2) + xfb_tmp(idx);
                case 'D'
                    idx = 15; ins.db(3) = ins.db(3) + xfb_tmp(idx);
                case 'L'
                    idx = 16:18; ins.lever = ins.lever + xfb_tmp(idx);
                case 'T'
                    idx = 19; ins.tDelay = ins.tDelay + xfb_tmp(idx);
                case 'G'
                    idx = 20:28; dKg = xfb_tmp(idx);  dKg = [dKg(1:3),dKg(4:6),dKg(7:9)];
                    ins.Kg = (eye(3)-dKg)*ins.Kg;
                case 'C'
                    idx = 29:34; dKa = xfb_tmp(idx);  dKa = [dKa(1:3),[0;dKa(4:5)],[0;0;dKa(6)]];
                    ins.Ka = (eye(3)-dKa)*ins.Ka;
                case 'H'
                    idx = [6,9,15];                         ins.vn(3) = ins.vn(3) - xfb_tmp(6);
                    ins.pos(3) = ins.pos(3) - xfb_tmp(9);	ins.db(3) = ins.db(3) + xfb_tmp(15);
                otherwise
                    error('feedback string mismatch in kf_feedback');
            end
            
            % kf.xk是误差形式的状态向量
            % kf.xk:kj卡尔曼滤波输出量
            % 参考论文——公式23
            % 当xfb_tmp为0时，为全反馈
            kf.xk(idx) = kf.xk(idx) - xfb_tmp(idx);  
            % 累积的反馈量：调试使用 
            kf.xfb(idx) = kf.xfb(idx) + xfb_tmp(idx);  % record total feedback
        end
        
        % 输出四元数，欧拉角，旋转矩阵
        [ins.qnb, ins.att, ins.Cnb] = attsyn(ins.qnb); 
        ins.avp = [ins.att; ins.vn; ins.pos];  % 2015-2-22
        return;
    end
    switch(navtype.curtype) % for specail feedback, add code here
        case navtype.xxx
            NA;
        otherwise
            error('navtype mismatch in kf_feedback');
    end

仿真结果

从运行结果中可以看到，惯性估计位置的精度要优于DR估计的位置，在卡尔曼反馈校正中只校正了惯性速度，其他状态变量没有反馈校正。

参考

1.《捷联惯导算法与组合导航原理》严恭敏
2.《惯性仪器测试与数据分析》 严恭敏