严老师最近在PSINS网站上上传了一组STIM300-GNSS跑车数据，并且有光纤惯导数据作为真值参考，网站是一组STIM300-GNSS跑车数据(有姿态参考),为了使用该数据集，psins代码包采用最新的，即psins201210，严老师也提供了代码，算是提供了一套比较完整的SINS/GNSS松耦合算法。
STIM300属于业界顶级的MEMS IMU，STIM300只是一个IMU，本身不输出姿态角数据，STIM300在出厂之前已经做了严格的标定和温度补偿，代码中的姿态算法部分还是基于光纤惯导的思路，即基本上只是陀螺仪在参与姿态解算，加速度计参与速度解算。一般来说对于MEMS IMU，因为精度有限，加速度计也是要参与速度解算的，这部分可以参考MTI系列惯导数据手册，其对MTI中所使用的算法思路有一个大致的讲解。
首先代码中提供了一个18状态向量的卡尔曼滤波器，状态向量分别为：欧拉角误差、速度误差、位置误差、陀螺零偏误差、加速度计零偏误差以及杆臂误差，实际上通过设置Q矩阵和P矩阵杆臂部分为0，就没有考虑杆臂。
主要脚本如下：
close all;
glvs
ts = 1/125;
dd = binfile('mimuattgps.bin',16);
imu = dd(:,[1:6,10]); imuplot(imu);
att = dd(:,7:10); insplot(att,'a'); % FOG-Att
gps = no0(dd(:,[11:end,10]),1); gpsplot(gps);
% 设置IMU噪声，噪声含义如下：
% 陀螺仪偏置：100deg/h (设置初始零偏，设置P矩阵)
% 加速度计偏置：5000ug (设置初始零偏，设置P矩阵)
% 陀螺仪高斯噪声： 0.1deg/sqrt(h)
% 加速度计高斯噪声：10ug/sqrt(Hz)
imuerr = imuerrset(100, 5000, 0.1, 10);
% 欧拉角初始误差：pitch:3600角秒,roll:3600角秒,偏航角：300角秒
% 速度初始误差：1m/s
% 位置初始误差：1m
davp = avpseterr([3600;3600;300], [1;1;1], [1;1;1]*10);
ins = insinit([att(1,1:3),gps(1,1:6)]', ts); ins.nts=ts;
[avp, xkpk, ins1, kf1] = sinsgps(imu(10/ts:3000/ts,:), gps, ins, davp, imuerr);
avpcmpplot(att, avp(:,[1:3,end]), 'a', 'mu');
imuerrset中设置了陀螺仪和加速度计的零偏和高斯白噪声，分别用于设置P矩阵中对应的欧拉角误差以及速度误差，以及Q矩阵中对应的欧拉角误差以及速度误差。
avpseterr中设置了P矩阵对应的欧拉角误差、速度误差、位置误差，sinsgps函数是SINS/GNSS组合导航核心函数。
因为代码数据集中提供了光纤惯导的三个欧拉角，所以avpcmpplot函数通过STIM300/GNSS组合导航得到的欧拉角和光纤数据进行比较，得到组合导航误差，方便分析。
如下代码设置了GPS速度观测和位置观测的噪声，这个数据集中的GPS为单点GPS，并且提供了东北天导航坐标系下的GNSS速度。
pos0=gps(1,1:3)'; r0=[10/glv.Re;10/glv.Re;30]; %设置GPS位置观测噪声
if size(gps,2)>6
pos0=gps(1,4:6)';
r0=[[1;1;1]/10;r0]; %设置GPS速度观测噪声
end
在kf.Qt的设置中也考虑了零偏的马尔科夫游走，即建模了零偏的缓慢变化，代码如下：
% imuerr.sqg:一阶马尔科夫噪声
% imuerr.sqa:一阶马尔科夫噪声
% imuerr.web:高斯噪声
% imuerr.wdb:高斯噪声
kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(3,1); imuerr.sqg; imuerr.sqa; zeros(3,1)])^2;
设置了kf.Pmin和kf.Pmax，即P矩阵的幅度限制，kf.adaptive = 1虽然打开了自适应卡尔曼滤波的开关，但是代码中已经把自适应部分代码屏蔽了，可能用起来不太好吧。
代码中使用了部分反馈校正原理，这部分在严老师的一篇论文中有叙述，kf.xtau设置了反馈系数，如下所示：
kf.xtau = [ [1;1;1]*10; [1;1;1]; [1;1;1]; [10;10;10]; [10;10;10]; [1;1;1] ];
imugpssyn做了SINS和GNSS的软件同步，说是同步，其实不算，就是找两个相邻的IMU数据中间有没有GPS数据，从这里也可以看到时间同步精度理想情况下不超过IMU输出周期，即8ms。
kfs = kfstatistic(kf); kfs1 = kfstat([],kf); 这两个函数目前没太研究，kfs 和kfs1在代码中没有使用。
kf.Phikk_1 = kffk(ins)是计算状态转移矩阵，就是高精度惯导最常用的15*15矩阵。
观测量为惯导速度解算，位置解算和GPS速度，位置之间的差值，代码如下所示：
zk = [ins.vnL;ins.posL]-gps(kgps,1:6)'; % 同时观测GPS速度和位置
观测转移矩阵的设置如下所示：
kf.Hk = [zeros(6,3), eye(6), zeros(6,6), [-ins.CW;-ins.MpvCnb]];
kfupdate函数中调用了了卡尔曼滤波的5个方程。
kffeedback函数中使用了部分反馈校正方法来更新AVP，欧拉角、速度、位置、陀螺零偏、加速度计零偏都进行了反馈。
这部分的整体注释代码如下：
function [avp, xkpk, ins, kf] = sinsgps(imu, gps, ins, davp, imuerr, lever, r0, fbstr)
% %% init
% imuerr = imuerrset(0.03, 100, 0.001, 1);
% davp = avpseterr([300;300;300], [1;1;1], [1;1;3]*100);
% ins = insinit(avp(1,1:9), ts); ins.nts=ts;
% %% kf
% lever = [0;0;0];
% rk = [10/glv.Re;10/glv.Re;30];
% [avp1, xkpk, ins1, kf] = sinsgps(imu, gps, ins, davp, imuerr, lever, rk, 'avp');
global glv
if ~exist('fbstr', 'var'), fbstr='avped'; end % fbstr:反馈量
if ~exist('r0', 'var')
% 注意r0
pos0=gps(1,1:3)'; r0=[10/glv.Re;10/glv.Re;30]; %设置GPS位置观测噪声
if size(gps,2)>6
pos0=gps(1,4:6)';
r0=[[1;1;1]/10;r0]; %设置GPS速度观测噪声
end
end
if ~exist('lever', 'var'), lever = [0;0;0]; end
if ~exist('imuerr', 'var'), imuerr = imuerrset(0.01, 100, 0.001, 1); end
if ~exist('davp', 'var'), davp = avpseterr([300;300;300], [1;1;1], [1;1;3]*100); end
[nn, ts, nts] = nnts(2, diff(imu(1:2,end)));
if ~exist('ins', 'var')
[att0, res0] = aligni0(imu(1:fix(200/ts),:), pos0);
ins = insinit([res0.attk(1,1:3)'; 0;0;0; pos0], ts); ins.nts=ts;
end
gpspos_only = 0;
if size(gps,2)<=5, gpspos_only = 1; end
psinstypedef(186);
kf = [];
% 状态变量： 18状态：欧拉角,速度,位置,陀螺零偏,加速度计零偏,杆臂
% imuerr.sqg:一阶马尔科夫噪声
% imuerr.sqa:一阶马尔科夫噪声
% imuerr.web:高斯噪声
% imuerr.wdb:高斯噪声
kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(3,1); imuerr.sqg; imuerr.sqa; zeros(3,1)])^2;
kf.Rk = diag(r0)^2;
kf.Pxk = diag([davp; imuerr.eb; imuerr.db; lever]*1.0)^2;
% 最小欧拉角误差：[3;3;10]角秒
% 最小速度误差：[0.01;0.01;0.01]/100m/s
% 最小位置误差：
% 陀螺仪最小偏置误差：[1;1;1]/1000 deg/h
% 加速度计最小偏置误差：[1;1;10]ug
kf.Pmin = [[3;3;10]*glv.sec; [0.01;0.01;0.01]/100; [[1;1]/glv.Re;1]/100; [1;1;1]/1000*glv.dph; [1;1;10]*glv.ug; [0;0;0]].^2;
kf.Pmax = 10000*diag(kf.Pxk); kf.pconstrain = 1; %协方差限幅
kf.adaptive = 1; % 自适应卡尔曼滤波
kf.xtau = [ [1;1;1]*10; [1;1;1]; [1;1;1]; [10;10;10]; [10;10;10]; [1;1;1] ]; % ????
kf.Hk = zeros(length(r0),18); %观测转换矩阵
% kfinit0：初始化KF结构体
kf = kfinit0(kf, nts);
imugpssyn(imu(:,7), gps(:,end)); %imu,gps软件同步
len = length(imu); [avp, xkpk] = prealloc(fix(len/nn), 16, 2*kf.n+1); %预分配空间
timebar(nn, len, '18-state SINS/GPS simulation.'); ki = 1;
kfs = kfstatistic(kf); kfs1 = kfstat([],kf);
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：状态转移矩阵
kf.Phikk_1 = kffk(ins); % kf.Phikk_1(1:3,1:3) = eye(3);
kf = kfupdate(kf);
% kfs = kfstatistic(kfs, kf, 'T'); kfs1 = kfstat(kfs1, kf, 'T');
% 由imugpssyn的同步机制可以知道INS数据和GPS数据的时间同步误差最大为：惯导数据周期：8ms
% 找到了两个惯导数据之间的GPS数据，否则：kgps=0
[kgps, dt] = imugpssyn(k, k1, 'F');
if kgps>0 %如果有GPS数据
% if gps(kgps,4)<4 && gps(kgps,4)>0 % DOP
ins = inslever(ins); % 杆臂补偿
if gpspos_only==1
% ins.posL：加入杆臂补偿之后的位置
zk = ins.posL-gps(kgps,1:3)';
kf.Hk = [zeros(3,6), eye(3), zeros(3,6), -ins.MpvCnb];
else
% ins.vnL：加入杆臂补偿之后的速度
% ins.posL：加入杆臂补偿之后的位置
zk = [ins.vnL;ins.posL]-gps(kgps,1:6)'; % 同时观测GPS速度和位置
% kf.Hk：观测矩阵
% eye(6)：速度和位置
% [-ins.CW;-ins.MpvCnb]：考虑到杆臂
kf.Hk = [zeros(6,3), eye(6), zeros(6,6), [-ins.CW;-ins.MpvCnb]];
end
kf = kfupdate(kf, zk, 'M');
% kfs = kfstatistic(kfs, kf, 'M'); kfs1 = kfstat(kfs1, kf, 'M');
% end
end
[kf, ins] = kffeedback(kf, ins, nts, fbstr); % fbstr = 'avped'
avp(ki,:) = [ins.avp; ins.eb; ins.db; t]'; % 注意是有时间的
xkpk(ki,:) = [kf.xk; diag(kf.Pxk); t]'; ki = ki+1;
timebar;
end
% kfs = kfstatistic(kfs); kfs1 = kfstat(kfs1,kf);
avp(ki:end,:) = []; xkpk(ki:end,:) = [];
insplot(avp);
kfplot(xkpk);
2. 利用光纤惯导的欧拉角作为真值，可以间接估计出STIM300和光纤惯导之间的安装误差角，如下所示，整体较为稳定，中间有比较大的跳动，原因有待进一步分析。可以看到pitch在1500s处的误差在16角秒左右，roll的误差在21角秒左右，yaw的误差在2角秒左右。
3. 从2的分析中可以看到，欧拉角的组合导航精度还是比较高的，一方面是陀螺仪本身精度高，另一方面是GPS的修正作用。下面测试在没有GPS的修正作用时的欧拉角误差。屏蔽掉sinsgps函数中的kffeedback函数，欧拉角误差如下所示，可以看到yaw最大误差接近3°。
可见GNSS的修正作用对于欧拉角的正确性还是非常重要的。如下图所示，由于没有有效的估计零偏，roll和pitch一开始的偏差就已经很大了。
