前言

严老师最近在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组合导航得到的欧拉角和光纤数据进行比较，得到组合导航误差，方便分析。

sinsgps函数

如下代码设置了GPS速度观测和位置观测的噪声，这个数据集中的GPS为单点GPS，并且提供了东北天导航坐标系下的GNSS速度。

        pos0=gps(1,1:3)'; r0=[10/glv.Re;10/glv.Re;30];  %设置GPS位置观测噪声if size(gps,2)>6pos0=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 glvif ~exist('fbstr', 'var'), fbstr='avped'; end %  fbstr:反馈量if ~exist('r0', 'var')% 注意r0pos0=gps(1,1:3)'; r0=[10/glv.Re;10/glv.Re;30];  %设置GPS位置观测噪声if size(gps,2)>6pos0=gps(1,4:6)'; r0=[[1;1;1]/10;r0];   %设置GPS速度观测噪声endendif ~exist('lever', 'var'), lever = [0;0;0]; endif ~exist('imuerr', 'var'), imuerr = imuerrset(0.01, 100, 0.001, 1); endif ~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;endgpspos_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]ugkf.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+1k1 = 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   % DOPins = 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]];endkf = kfupdate(kf, zk, 'M');%          kfs = kfstatistic(kfs, kf, 'M');     kfs1 = kfstat(kfs1, kf, 'M');
%             endend[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一开始的偏差就已经很大了。

PSINS源码阅读—STIM300/GNSS组合导航相关推荐

PSINS源码阅读—test_SINS_trj
提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档文章目录前言关于航迹仿真代码解读参考论文前言 test_SINS_trj是航迹仿真程序,具体实现可以参考严恭敏老师的硕士论文 ...
基于PSINS工具箱的卡尔曼滤波与SINS/GNSS组合导航
文章目录卡尔曼滤波与SINS/GNSS组合导航典型的SINS/GNSS组合滤波 POS处理卡尔曼滤波与SINS/GNSS组合导航用于Kalman滤波的函数有: psinstypedef(nnm ...
转-OpenJDK源码阅读导航跟编译
OpenJDK源码阅读导航 OpenJDK源码阅读导航博客分类: Virtual Machine HotSpot VM Java OpenJDK openjdk 这是链接帖.主体内容都在各链接中. ...
PSINS源码test_SINS_DR解析
PSINS源码test_SINS_DR解析前言源码解析整体介绍对源码参数的更改 test_SINS_DR脚本 glvf函数 drinit函数 odsimu函数 RMRN函数 imuerrset ...
sfm三维重建源码_OpenMVG源码阅读小记
"读一份好源码,就是和许多智慧的人谈话". 本文记录了笔者学习 openMVG 开源软件的一些初步经验和心得.如果你对计算机视觉和摄影测量有兴趣,需要用到相关技术,这篇文章正好就是 ...
gin context和官方context_gin 源码阅读（二）路由和路由组
" 上一篇讲的是gin 框架的启动原理,今天来讲一下 gin 路由的实现. 1 用法还是老样子,先从使用方式开始: func main() { r := gin.Default() r.G ...
koa源码阅读之koa-compose/application.js
koa源码阅读之koa-compose/application.js koa-Compose 为了理解方便特地把注释也粘进来 //这英语.我也来翻译一波 //大概就是把所有的中间件组合返回一个完整大块 ...
SpringMVC源码阅读：过滤器
SpringMVC源码阅读:过滤器目录 1.前言 2.源码分析 3.自定义过滤器 3.1 自定义过滤器继承OncePerRequestFilter 3.2 自定义过滤器实现Filter接口 4.过滤 ...
spark.mllib源码阅读：GradientBoostedTrees
Gradient-Boosted Trees(GBT或者GBDT) 和 RandomForests 都属于集成学习的范畴,相比于单个模型有限的表达能力,组合多个base model后表达能力更加丰富. ...
Soul网关源码阅读（八）路由匹配初探
Soul网关源码阅读(八)路由匹配初探简介今日看看路由的匹配相关代码,查看HTTP的DividePlugin匹配示例运行使用HTTP的示例,运行Soul-Admin,Sou ...

PSINS源码阅读—STIM300/GNSS组合导航

文章目录

前言