框架：
% KF filter
rk = posseterr([1;1;3]); %位置误差
kf = kfinit(ins, davp0, imuerr, rk); case psinsdef.kfinit153,
psinsdef.kffk = 15; psinsdef.kfhk = 153; psinsdef.kfplot = 15;
[davp, imuerr, rk] = setvals(varargin);
kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(9,1)])^2; %注意此处是Qt
kf.Rk = diag(rk)^2;
kf.Pxk = diag([davp; imuerr.eb; imuerr.db]*1.0)^2;
kf.Hk = kfhk(0);
kf = kfinit0(kf, nts);最终得到离散化的kf参数，如下：
“1”的计算：kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(3,1); imuerr.sqg; imuerr.sqa; zeros(3,1)])^2;
“2”的计算：kf.Qk = kf.Qt*kf.nts; end %离散化的Q
“3”的计算：kf.xtau = ones(size(kf.xk))*eps; 示例代码中给出的是一个matlab能够分辨的最小值；
“4”的计算：kf.xfb(idx) = kf.xfb(idx) + xfb_tmp(idx); % record total feedback，对进行误差反馈的状态量，每次有反馈都进行反馈的累加
“5”的计算：xtau = kf.xtau;
xtau(kf.xtau<kf.T_fb) = kf.T_fb; kf.coef_fb = kf.T_fb./xtau; %2015-2-22 反馈系数
其中 kf.T_fb = 1; end %时间标签，即kalman数据融合时间，此处等于1秒
kalman滤波流程图：
（1）时间更新：
采用双子样进行惯导解算，并且对误差状态进行、误差状态方差进行预测更新；
wvm = imu(k:k1,1:6); t = imu(k1,end);
if k>200*100
wvm(:,6) = wvm(:,6)+50*glv.ug*ts;
end
ins = insupdate(ins, wvm);
kf.Phikk_1 = kffk(ins); %discrete-time transition matrix 得到离散的状态转移矩阵
kf = kfupdate(kf); %执行时间更新状态转移矩阵的求取为：
离散化：
执行状态更新：kf.Gammak=1;
（2）量测更新：
主要进行：量测一步预测、量测一步预测均方误差阵、状态一步预测与量测一步预测之间的协均方误差阵、增益矩阵K计算、状态最优估计、状态估计均方误差阵；
代码如下：
if mod(t,1)==0 %整秒进行kalman滤波的量测更新；
posGPS = trj.avp(k1,7:9)' + davp0(7:9).*randn(3,1); % GPS pos simulation with some white noise
kf = kfupdate(kf, ins.pos-posGPS, 'M');
[kf, ins] = kffeedback(kf, ins, 1, 'vp');
avp(ki,:) = [ins.avp', t];
xkpk(ki,:) = [kf.xk; diag(kf.Pxk); t]'; ki = ki+1;
end量测更新：
if TimeMeasBoth=='M' % Meas Updating
kf.xkk_1 = kf.xk;
kf.Pxkk_1 = kf.Pxk;
elseif TimeMeasBoth=='B' % Time & Meas Updating
kf.xkk_1 = kf.Phikk_1*kf.xk;
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;
kf.ykk_1 = kf.Hk*kf.xkk_1;
kf.rk = yk-kf.ykk_1;
kf.Pykk_1 = kf.Py0 + kf.Rk;
kf.Kk = kf.Pxykk_1*invbc(kf.Pykk_1); % kf.Kk = kf.Pxykk_1*kf.Pykk_1^-1;
kf.xk = kf.xkk_1 + kf.Kk*kf.rk;
kf.Pxk = kf.Pxkk_1 - kf.Kk*kf.Pykk_1*kf.Kk';
kf.Pxk = (kf.Pxk+kf.Pxk')*(kf.fading/2); % symmetrization & forgetting factor 'fading'误差状态反馈、校正导航系统：[kf, ins] = kffeedback(kf, ins, 1, 'vp');
注意，因为系统是基于误差状态量的kalman组合导航系统，因此，当系统估计除了误差状态，毕竟对相应的状态量做了补偿，则需要在误差状态量中予以扣除；
（1）kalman组合导航结果输出：【avp】
（2）kalman组合导航结果与参考真值做差输出：avperr = avpcmp(avp, trj.avp)
仿真轨迹如下：参考链接Psins代码解析一之全局变量&轨迹仿真（test_SINS_trj.m）&惯性解算（test_SINS.m）
（3）kalman组合导航中，最优估计误差状态量输出：【kf.xk】
inserrplot(xkpk(:,[1:psinsdef.kfplot,end]));
case 'avped'
myfigure;
subplot(321), plot(t, err(:,1:2)/glv.sec); xygo('phiEN');
subplot(322), plot(t, err(:,3)/glv.min); xygo('phiU');
subplot(323), plot(t, err(:,4:6)); xygo('dV');
subplot(324), plot(t, [err(:,7:8)*glv.Re,err(:,9)]); xygo('dP');
subplot(325), plot(t, err(:,10:12)/glv.dph); xygo('eb');
subplot(326), plot(t, err(:,13:15)/glv.ug); xygo('db');
（4）粉红色线条为 avperr = avpcmp(avp, trj.avp)；即avperr与xkpk整合在一张图上；
（5）diag(kf.Pxk) ，sqrt(误差状态估计均方误差阵图)：对角线元素：
inserrplot([sqrt(xkpk(:,psinsdef.kfplot+1:end-1)),xkpk(:,end)]);
（1）杆臂效应，参数估计：
在组合导航中，一般以IMU器件所在的位置为姿态、速度、位置的基准；由于GNSS接收机天线的安装与IMU器件有偏差，存在杆臂效应，如下图所示：
杆臂：由IMU指向GNSS天线；基于此，推导出INS和GNSS速度、位置关系；
其中，Mpv为：
状态误差量为：18-state included: phi(3), dvn(3), dpos(3), eb(3), db(3), lever(3)
量测矩阵为：Hk = [zeros(6,3), eye(6), zeros(6,6), [-ins.CW;-ins.MpvCnb]];
（2）时间不同步，参数估计：
以图示为例：卫星信号和惯导信号时间不同步误差为δt，如果惯导在两个采样时刻间的速度率变化为an，则有如下计算公式：
即，速度、位置不同步误差为：
状态误差量为：19-state included: phi(3), dvn(3), dpos(3), eb(3), db(3), lever(3), dT(1)
量测矩阵为：Hk = [zeros(3,6), eye(3), zeros(3,6), -ins.MpvCnb, ins.Mpvvn]; %位置误差参与量测更新
参考：
状态空间模型为：19-state included: phi(3), dvn(3), dpos(3), eb(3), db(3), lever(3), dT(1)
（3）以test_SINS_GPS_193.m为例：
误差状态量为：phi(3), dvn(3), dpos(3), eb(3), db(3), lever(3), dT(1)；
量测状态为：Δpos
需要注意的几个点：
Hk = [zeros(3,6), eye(3), zeros(3,6), -ins.MpvCnb, -ins.Mpvvn];
其中： ”-ins.MpvCnb, -ins.Mpvvn“ 在每次进行量测更新时，都需要更新；其在ins = inslever(ins)中实现。
kf = kfupdate(kf, ins.posL-ins.Mpvvn*dt-posGPS, 'M')
画图展示：程序中采用“avp”反馈；预设lever=[1;2;3] 、dT = 0.1;
