严恭敏老师PSINS工具箱解读——test_SINS_GPS_153

 感谢严老师的无私奉献

---

 test_SINS_GPS_153——15维状态和位置量测的SINS/GPS松组合

glvs——'全局变量,请参考glvf函数解读'
psinstypedef(153);——'全局变量,主要设置状态转移矩阵的维数、量测矩阵的维数和画图'
trj = trjfile('trj10ms.mat');——'加载轨迹数据，请参考trjfile函数解读'

% initial settings
[nn, ts, nts] = nnts(2, trj.ts);——'子样数和采样间隔'
imuerr = imuerrset(0.03, 100, 0.001, 5);——'imu误差，分别为陀螺常值零偏、加速度计常值零偏、角度随机游走和速度随机游走，请参考imuerrset函数解读'
imu = imuadderr(trj.imu, imuerr);——'对参考imu数据添加误差生成实际imu测量值，请参考imuadderr解读'
davp0 = avperrset([0.5;-0.5;20], 0.1, [1;1;3]);——'初始姿态、速度和位置误差，注意姿态的单位为：分，请参考avperrset函数解读'
ins = insinit(avpadderr(trj.avp0,davp0), ts);——'对参考初始姿态、速度和位置添加误差，并进行惯导初始化用于后面更新'

% KF filter
rk = poserrset([1;1;3]);——'位置量测噪声'
kf = kfinit(ins, davp0, imuerr, rk);——'卡尔曼滤波器初始化'
kf.Pmin = [avperrset(0.01,1e-4,0.1); gabias(1e-3, [1,10])].^2;——'方差阵最小值'
kf.pconstrain=1;——'方差阵上下限约束条件，1：表示约束'
len = length(imu); [avp, xkpk] = prealloc(fix(len/nn), 10, 2*kf.n+1);——'对变量预先分配内存'
timebar(nn, len, '15-state SINS/GPS Simulation.'); ——'程序运行的进度条'

ki = 1;——'量测更新的计数器'
for k=1:nn:len-nn+1
    k1 = k+nn-1;
    wvm = imu(k:k1,1:6);  t = imu(k1,end);——'角度增量和速度增量，采样时刻'
    ins = insupdate(ins, wvm);——'惯导更新'
    kf.Phikk_1 = kffk(ins);——'计算状态转移矩阵'
    kf = kfupdate(kf);——'卡尔曼滤波器预测更新'
   
    if mod(t,1)==0——'判断是否有GNSS位置量测，此处认为GNSS为整秒量测，mod为求余数函数'
        posGPS = trj.avp(k1,7:9)’ + davp0(7:9).*randn(3,1);——'模拟GNSS位置量测值'
        kf = kfupdate(kf, ins.pos-posGPS, 'M');——'卡尔曼滤波器量测更新'
        [kf, ins] = kffeedback(kf, ins, 1, 'avp');——'卡尔曼滤波状态反馈'
        avp(ki,:) = [ins.avp’, t];——'姿态、速度、位置和时间'
        xkpk(ki,:) = [kf.xk; diag(kf.Pxk); t]’;——'反馈后的剩余状态、方差阵和时间'
        ki = ki+1;
    end
    
   timebar;——'进度条的计数器'
end

avp(ki:end,:) = [];  xkpk(ki:end,:) = [];——'清除多余的内存'
% show results
insplot(avp);——'画姿态、速度和位置'
avperr = avpcmpplot(trj.avp, avp);——'组合导航计算的avp与参考avp的差值'
kfplot(xkpk, avperr, imuerr);——'画avp误差、imu误差、反馈后的剩余状态和方差阵'

‘glvf’函数解读；‘trjfile’函数解读；‘imuerrset’函数解读；‘imuadderr’函数解读；‘avperrset’函数解读