严恭敏老师PSINS工具箱学习笔记-6

psins用于捷联惯导系统的数据处理和算法验证开发，包括惯性传感器数据分析、惯组标定、初始对准、惯导avp更新解算、组合导航卡尔曼滤波。

算法经过了验证测试，实用性强稳定可靠。我们需要：主程序的框架+设置参数(专业，对惯导的原理比较熟)。

test_SINS_trj.m

涉及的结构体：glv, trj, seg

 test_SINS.m

纯惯导解算，bhsimu气压测高计仿真高度（左为高度补偿后的，右无高度补偿）

 涉及的中间结构体ins，相关函数inspure，insupdate

 test_SINS_DR.m

组合导航：使用卡尔曼滤波将惯导解算的结果与DR测量值融合，改善惯性导航结果。

里程仪相关参数：inst（安装角），kod（刻度系数），qe（暂为默认值），dT（时间延迟）

glvs
psinstypedef('test_SINS_DR_def');
trj = trjfile('trj10ms_3.mat');
[nn, ts, nts] = nnts(2, trj.ts);
inst = [3;60;6]*glv.min;  kod = 1;  qe = 0; dT = 0.01; % od parameters
trjod = odsimu(trj, inst, kod, qe, dT, 0); % od simulation
imuerr = imuerrset(0.01, 50, 0.001, 5);
imu = imuadderr(trjod.imu, imuerr);
davp = avperrset([30;30;10], 0, 10);
ins = insinit(avpadderr(trjod.avp0,davp), ts); % SINS init
dinst = [15;0;10]*glv.min; dkod = 0.01;
dr = drinit(avpadderr(trjod.avp0,davp), d2r((inst+dinst)/60), kod*(1+dkod), ts); % DR init
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;
    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 !
    dr = drupdate(dr, wvm(:,1:3), dS);
    kf.Phikk_1 = kffk(ins);
    kf = kfupdate(kf);
    if mod(k1,10)==0
        kf.Hk(:,22) = -ins.Mpvvn;
        kf = kfupdate(kf, ins.pos-dr.pos);
        [kf, ins] = kffeedback(kf, ins, 1, 'v');
    end
    insavp(ki,:) = [ins.avp; t]';
    dravp(ki,:) = [dr.avp; t]';
    xkpk(ki,:) = [kf.xk; diag(kf.Pxk); t]';
    ki = timebar;
end
dr.distance,
close all
% insplot(dravp, 'DR', insavp);
kfplot(xkpk, trjod.avp, insavp, dravp, imuerr, dinst, dkod, dT);

误差设置相关函数：

imuerr = imuerrset(0.01, 50, 0.001, 5);
imuadderr(trjod.imu, imuerr);
davp = avperrset([30;30;10], 0, 10);
avpadderr(trjod.avp0,davp)

 涉及的中间结构体ins dr kf

绘图函数

以test_SINS_GPS_153为例：

imuplot(trj.imu)；对惯导输出数据进行绘图

insplot(avp);对姿态速度位置增量等解算出来的信息，以及轨迹进行绘图

avperr = avpcmpplot(trj.avp, avp);计算解算信息误差以及误差绘图

kfplot(xkpk, avperr, imuerr);绘图估计的xk与真实的xk对比图以及pk（是否收敛并趋于零），