习惯约定与常用变量符号

PSINS全局变量结构体glv（global variable）

运行glvs脚本文件，内部实际调用的是glvf函数，这个函数就是可以初始化全局变量，代码如下：

function glv1 = glvf(Re, f, wie)
% PSINS Toolbox global variable structure initialization.
%
% Prototype: glv = glvf(Re, f, wie)
% Inputs: Re - the Earth's semi-major axis
%         f - flattening
%         wie - the Earth's angular rate
% Output: glv1 - output global variable structure array
%
% See also  psinsinit.% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 14/08/2011, 10/09/2013, 09/03/2014
global glvif ~exist('Re', 'var'),  Re = [];  endif ~exist('f', 'var'),   f = [];  endif ~exist('wie', 'var'), wie = [];  endif isempty(Re),  Re = 6378137;  endif isempty(f),   f = 1/298.257;  endif isempty(wie), wie = 7.2921151467e-5;  endglv.Re = Re;                    % the Earth's semi-major axisglv.f = f;                      % flatteningglv.Rp = (1-glv.f)*glv.Re;      % semi-minor axisglv.e = sqrt(2*glv.f-glv.f^2); glv.e2 = glv.e^2; % 1st eccentricityglv.ep = sqrt(glv.Re^2-glv.Rp^2)/glv.Rp; glv.ep2 = glv.ep^2; % 2nd eccentricityglv.wie = wie;                  % the Earth's angular rateglv.meru = glv.wie/1000;        % milli earth rate unitglv.g0 = 9.7803267714;          % gravitational forceglv.mg = 1.0e-3*glv.g0;         % milli gglv.ug = 1.0e-6*glv.g0;         % micro gglv.mGal = 1.0e-3*0.01;         % milli Gal = 1cm/s^2 ~= 1.0E-6*g0glv.uGal = glv.mGal/1000;       % micro Galglv.ugpg2 = glv.ug/glv.g0^2;    % ug/g^2glv.ws = 1/sqrt(glv.Re/glv.g0); % Schuler frequencyglv.ppm = 1.0e-6;               % parts per millionglv.deg = pi/180;               % arcdegglv.min = glv.deg/60;           % arcminglv.sec = glv.min/60;           % arcsecglv.mas = glv.sec/1000;         % milli arcsecglv.hur = 3600;                 % time hour (1hur=3600second)glv.dps = pi/180/1;             % arcdeg / secondglv.rps = 360*glv.dps;          % revolutions per secondglv.dph = glv.deg/glv.hur;      % arcdeg / hourglv.dpss = glv.deg/sqrt(1);     % arcdeg / sqrt(second)glv.dpsh = glv.deg/sqrt(glv.hur);  % arcdeg / sqrt(hour)glv.dphpsh = glv.dph/sqrt(glv.hur); % (arcdeg/hour) / sqrt(hour)glv.dph2 = glv.dph/glv.hur;    % (arcdeg/hour) / hourglv.Hz = 1/1;                   % Hertzglv.dphpsHz = glv.dph/glv.Hz;   % (arcdeg/hour) / sqrt(Hz)glv.dphpg = glv.dph/glv.g0;     % (arcdeg/hour) / gglv.dphpg2 = glv.dphpg/glv.g0;  % (arcdeg/hour) / g^2glv.ugpsHz = glv.ug/sqrt(glv.Hz);  % ug / sqrt(Hz)glv.ugpsh = glv.ug/sqrt(glv.hur); % ug / sqrt(hour)glv.mpsh = 1/sqrt(glv.hur);     % m / sqrt(hour)glv.mpspsh = 1/1/sqrt(glv.hur); % (m/s) / sqrt(hour), 1*mpspsh~=1700*ugpsHzglv.ppmpsh = glv.ppm/sqrt(glv.hur); % ppm / sqrt(hour)glv.mil = 2*pi/6000;            % milglv.nm = 1853;                  % nautical mileglv.kn = glv.nm/glv.hur;        % knot%%glv.wm_1 = [0,0,0]; glv.vm_1 = [0,0,0];   % the init of previous gyro & acc sampleglv.cs = [                      % coning & sculling compensation coefficients[2,    0,    0,    0,    0    ]/3[9,    27,   0,    0,    0    ]/20[54,   92,   214,  0,    0    ]/105[250,  525,  650,  1375, 0    ]/504 [2315, 4558, 7296, 7834, 15797]/4620 ];glv.csmax = size(glv.cs,1)+1;  % max subsample numberglv.v0 = [0;0;0];    % 3x1 zero-vectorglv.qI = [1;0;0;0];  % identity quaternionglv.I33 = eye(3); glv.o33 = zeros(3);  % identity & zero 3x3 matricesglv.pos0 = [34.246048*glv.deg; 108.909664*glv.deg; 380]; % position of INS Lab@NWPUglv.eth = []; glv.eth = earth(glv.pos0);glv.t0 = 0;glv.tscale = 1;  % =1 for second, =60 for minute, =3600 for hour, =24*3600 for dayglv.isfig = 1;%%[glv.rootpath, glv.datapath, glv.mytestflag] = psinsenvi;glv1 = glv;

其中传入函数中的三个参数分别为地球半径、地球扁率和地球自转角速率(默认WGS84坐标系)；glv.mg表示毫g，glv.ug表示微g，工具箱中所有物理量在内部计算都使用标准单位，比如角度用rad、比力用m/s^2等；只在初始化输入参数时才会使用习惯单位，比如陀螺漂移用°/h。

坐标系定义

惯性坐标系(i)；
地球坐标系(e)，即ECEF坐标系；
导航坐标系(n)：东E-北N-天U；
载体坐标系(b)：右R-前F-上U。
陀螺仪和加速度计测量的值都是在惯性坐标系下的；

e系与n系

b系

姿态阵/姿态四元数/姿态角

姿态阵：Cnb，书写一般遵从规律是从左到右从上到下，即为Cnb，它表示从b系到n系的坐标变换矩阵。对应姿态四元数写为qnb。
姿态/欧拉角向量：att=[俯仰pitch; 横滚roll; 方位yaw]

IMU采样数据

imu=[wm; vm; t]，通常时标总是放在最后一列。
其中，wm为陀螺三轴角增量(角速率积分)、vm为加表三轴速度增量(比力的积分)，PSINS惯导算法里使用的陀螺和加表输入都是增量信息（分别对应单位rad和m/s），如果用户数据中是角速度/比力信息，则简单地乘以采样间隔ts处理即可。

AVP导航参数

avp=[att; vn; pos; t]。
其中，
姿态att=[俯仰pitch; 横滚roll; 方位yaw]；
速度vn=[东速vE; 北速vN; 天速vU]；
位置向量：pos=[纬度lat; 经度lon; 高度hgt]。
注意：俯仰/横滚/方位/纬度/经度均为弧度单位rad。

误差参数

失准角误差phi=[phiE;phiN;phiU]；速度误差dvn；位置误差dpos=[dlat;dlon;dhgt]；
陀螺漂移eb=[ebx;eby;ebz]；加表零偏db=[dbx;dby;dbz]；web为陀螺角度随机游走/角速率白噪声；wdb为加计速度随机游走/比力白噪声；
陀螺标定误差矩阵dKg；加表标定误差矩阵dKa；
IMU误差结构体imuerr，包含较多成员，可见imuerrset函数。

其他

角速度wnie：表示w^n_{ie}即e系相对于i系的角速度在n系的投影，书写一般遵从规律是从左到右从上到下；wnin和wnen等变量符号类似；
phim/dvbm：经不可交换误差补偿后的等效旋转矢量/比力速度增量；
gn：当地重力矢量gn=[0;0;-g]，g为重力大小；gcc有害加速度；
trj：仿真轨迹结构体（参见trjsimu函数）；
ins：指北方位捷联导航解算结构体（参见insinit函数）；
eth：导航地球相关计算结构体（参见ethinit函数）；
kf：Kalman滤波结构体（参见kfinit函数）；
ps：POS信息融合结构体（POS Fusion）；

导入数据文件与数据提取转换

导入文件数据有以下方式：

二进制（纯double型）格式文件，使用binfile函数，这对导入C语言生成的数据文件快速方便；或者可参照binfile，使用fread自行编程导入特定格式的二进制文件；
文本文件/或.mat格式文件，使用Matlab的load或importdata函数；
特殊格式的PSINS-IMU/AVP文件，可用imufile/avpfile等函数。
binfile函数内容如下：

function data1 = binfile(fname, data, row0, row1)
% Save or load double format binary file, it can be exchange with C
% language. When loaded, be sure of the acurate number of data columns.
%
% Prototype: data1 = binfile(fname, data)
% Inputs: fname - file name, with default extension '.bin'
%         data - binary data array to save, but for read process 'data'
%                is the column number of the data saved.
% Output: data1 - data array read from the binary file
% Usages:
%    Save: binfile(fname, data)
%    Read: data1 = binfile(fname, column)
%
% See also  imufile, avpfile, kffile, matbinfile, importdata, ld2528.% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 20/02/2013, 30/03/2015%fname = fnamechk(fname, 'bin');if length(data)==1 % load: data1 = binfile(fname, columns)if nargin<3, row0=0; row1=inf; endif nargin==3, row1=row0; row0=0; endcolumns = data;fid = fopen(fname, 'rb');if row0>0, fseek(fid, columns*row0*8, 'bof'); enddata1 = fread(fid, [columns,row1-row0], 'double')';else               % save: binfile(fname, data)fid = fopen(fname, 'wb');fwrite(fid, data', 'double');endfclose(fid);

数据提取转换

从文件直接导入Matlab工作空间的数据通常是一个二维数组，其各列顺序及量纲单位不一定符合PSINS的习惯，需再进行数据提取和转换：
使用imuidx提取IMU数据并进行单位转换，陀螺为角增量、加表为速度增量；如需要，还可借助于imurfu函数将IMU转换至右-前-上坐标系；
使用avpidx提取AVP数据并进行单位转换，结果姿态/纬经为弧度、方位角北偏西为正；
使用gpsidx提取GNSS速度/定位数据并进行单位转换，纬经度为弧度；通常GNSS的频率低于IMU频率，为删除重复数据行可调用norep函数；为删除数据为0行可调用no0函数；
使用tshift或adddt函数可将数据的起始时间转换至指定的相对时间。
tshift和adddt函数代码如下：

function data = adddt(data, dt)
% Add the time tag data(:,end) with dt.
%
% Prototype: data = adddt(data, dt)
%
% See also  getat, sortt, tshift, delbias, scalet.% Copyright(c) 2009-2020, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 23/11/2020if nargin<2, dt=-data(1,end); enddata(:,end) = data(:,end)+dt;

function varargout = tshift(varargin)
% Time tag shift to specific start time t0.
%
% Prototype: varargout = tshift(varargin)
% Examples: 1) [o1, o2, o3, dt] = tshift(i1, i2, i3, t0)
%           2) [o1, o2, o3, dt] = tshift(i1, i2, i3)   % t0=0
%
% See also  adddt.% Copyright(c) 2009-2015, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 06/04/2015t0 = varargin{1}(1,end); t00 = 0;kk = nargin;if length(varargin{kk})==1, t00=varargin{kk}(1); kk=kk-1; endfor k=2:kkt0 = min(t0, varargin{k}(1,end));endvarargout = varargin(1:kk);dt = t0-t00;for k=1:kkvarargout{k}(:,end) = varargin{k}(:,end)-dt;endvarargout{k+1} = dt;

举例

打开并运行demos\test_IMUAVPGPS_extract_trans.m程序，通过Matlab\Variable Editor查看数据结果如下（注意数据单位及时标变化）：

% IMU/AVP/GNSS data extract&transform. Please run
% 'test_SINS_trj.m' to generate 'trj10ms.mat' beforehand!!!
% Copyright(c) 2009-2021, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 17/04/2021
glvs
trj = trjfile('trj10ms.mat');
%% data fabrication
imu1 = [[trj.imu(:,[2,1]),-trj.imu(:,3)]/trj.ts/glv.dps, [trj.imu(:,[5,4]),-trj.imu(:,6)]/trj.ts/glv.g0];  % FRD,deg/s,g
avp1 = [[trj.avp(:,1:2),yawcvt(trj.avp(:,3),'cc180c360')]/glv.deg, trj.avp(:,4:6), trj.avp(:,[8,7])/glv.deg, trj.avp(:,9)]; % deg,c360
gps1 = [trj.avp(1:10:end,[5,4]), -trj.avp(1:10:end,6), trj.avp(1:10:end,[8,7])/glv.deg, trj.avp(1:10:end,9)]; % FRD,deg
gps1(:,1:3)=gps1(:,1:3)+randn(size(gps1(:,1:3)))*0.01;
dd = [imu1,avp1,trj.avp(:,4:9)*0,trj.avp(:,10)+100]; dd(1:10:end,end-6:end-1)=gps1;
binfile('imuavpgps.bin', dd);
%% IMU/AVP/GNSS data extract&transform
dd = binfile('imuavpgps.bin', 22);
open dd;
imu = imurfu(imuidx(dd, [1:6,22],glv.dps,glv.g0,trj.ts),'frd');
avp = avpidx(dd,[7:12,14,13,15,22],1,1);
gps = gpsidx(dd,[17,16,-18,20,19,21,22],1);
[imu,avp,gps] = tshift(imu,avp,gps,10);
imuplot(imu); % imuplot(trj.imu);
insplot(avp); % insplot(trj.avp);
gpsplot(gps);
open imu
open avp
open gps

制造的数据如下格式：

0.00345371550166032  0   -0.00235120254134903    3.09681103586339e-12    1.33765898551336e-17    -1.00155166074260   0   0   0   0   0   0   108.909664000000    34.2460480000000    380 -0.00170967514845368    -0.00138702820012772    -0.0166210130181725 108.909664000000    34.2460480000000    380 100.100000000000
0.00345371550166032 1.99245340815899e-26    -0.00235120254134903    3.09681103586339e-12    1.33770140287612e-17    -1.00155166074260   0   0   0   0   0   0   108.909664000000    34.2460480000000    380 0   0   0   0   0   0   100.200000000000
0.00345371550166032 1.99245340815899e-26    -0.00235120254134903    3.09681103586339e-12    1.33770140287612e-17    -1.00155166074260   0   0   0   0   0   0   108.909664000000    34.2460480000000    380 0   0   0   0   0   0   100.300000000000
0.00345371550166032 1.99245340815899e-26    -0.00235120254134903    3.09681103586339e-12    1.33770140287612e-17    -1.00155166074260   0   0   0   0   0   0   108.909664000000    34.2460480000000    380 0   0   0   0   0   0   100.400000000000
0.00345371550166032 1.99245340815899e-26    -0.00235120254134903    3.09681103586339e-12    1.33770140287612e-17    -1.00155166074260   0   0   0   0   0   0   108.909664000000    34.2460480000000    380 0   0   0   0   0   0   100.500000000000

是一个22列的数据，要从其中提取出来IMU，AVP，GPS的信息；使用到的就是imuidx、avpidx和gpsidx函数，具体可参见源码；这些函数都是按照指定列，将数据根据PSINS标准的数据进行读取并输出；tshift(imu,avp,gps,10)函数是将这些值的参考时间统一到10s起始的时间。

绘图显示

绘图辅助函数

myfig：绘制白底全屏图；

xygo：启用网格（grid on），给出坐标标识（特殊标识由labeldef给出）；

labeldef：为简洁给出坐标简写标识，摘录如下（详见labeldef.m文件）

传感器数据绘图

以上是博主利用实测IMU数据，在matlab中绘制的参数图，可以根据自己的情况来完成。

导航结果绘图

insplot：导航结果AVP（甚至陀螺漂移eb、加表零偏db）绘图；
inserrplot/avpcmpplot：导航误差/比较绘图；
kfplot/xpplot：Kalman滤波结果状态或均方差阵对角线元素(Pk阵对角元素的开方，注：当水平失准角均方差曲线有明显下降，说明加计零偏可能估计出来了；当方位角有下降时，也可能说明陀螺漂移估计出来了)绘图。

进度条函数

进度条函数(timebar)

function tk = timebar(tStep, tTotal, msgstr)
% In PSINS Toolbox, a waitbar is always used to show the program running
% progress when needs a long time to processing. If the waitbar closed by user,
% the processing abort; if the processing done, the waitbar will disappear
% automaticly.
%
% Prototype: tk = timebar(tStep, tTotal, msgstr)
% For initialization usage:
%       tk = timebar(tStep, tTotal, msgstr);
%           where tStep is the step increasing when called timebar once,
%           tTotlal is the total steps, if reached then waitbar disappears,
%           msgstr is a message string to be showed in the waitbar figure.
% In loop usage:
%       tk = timebar;
%
% See also  resdisp, trjsimu, insupdate, kfupdate.% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 07/10/2013
global tb_argif nargin>=2tb_arg.tk = 1; tk = tb_arg.tk;tb_arg.tStep = tStep;tb_arg.tTotal = tTotal;tb_arg.tTotal001 = tTotal*0.01;tb_arg.tCur = 0;tb_arg.tOld = 0;tb_arg.rClosed = min(0.985, 1-2*tStep/tTotal);
%         tb_arg.time0 = cputime;if isfield(tb_arg, 'handle')if ishandle(tb_arg.handle)close(tb_arg.handle);endendif nargin<3,  msgstr = [];    endtb_arg.handle = waitbar(0,[msgstr, ' Please wait...'], ...'Name','PSINS Toolbox', 'WindowStyle', 'modal', ...'CreateCancelBtn', 'delete(gcbf);');return;endtb_arg.tk = tb_arg.tk + 1; tk = tb_arg.tk;tb_arg.tCur = tb_arg.tCur + tb_arg.tStep;if tb_arg.tCur-tb_arg.tOld > tb_arg.tTotal001r = tb_arg.tCur/tb_arg.tTotal;if ishandle(tb_arg.handle)if r>tb_arg.rClosedclose(tb_arg.handle);
%                 fprintf('\tCPU time used is %.3f sec.\n\n', cputime-tb_arg.time0);elsewaitbar(r, tb_arg.handle);tb_arg.tOld = tb_arg.tCur;end            elseif r<tb_arg.rClosedclear global tb_arg;error('PSINS processing is terminated by user.');
%                 disp('PSINS processing is terminated by user.\n');
%                 quit;endendend

姿态阵/姿态四元数/欧拉角/等效旋转矢量之间转换

各种姿态数学描述之间的转换函数如下:

其中m代表姿态阵，a代表姿态角，rv代表等效旋转矢量，q代表四元数。
常用的还有：

卡尔曼滤波中，如果姿态角用四元数表示，但是你估计出来的是失准角，那么你就需要用到上述函数，对四元数进行反馈修正；计算和参考的四元数差异就是失准角；安装误差角，为两套惯导放在一个车上跑，三个b系不平行，之间的关系

PSINS组合导航工具箱基本概念与函数简介相关推荐

1. 基于PSINS工具箱的卡尔曼滤波与SINS/GNSS组合导航

   文章目录 卡尔曼滤波与SINS/GNSS组合导航 典型的SINS/GNSS组合滤波 POS处理 卡尔曼滤波与SINS/GNSS组合导航 用于Kalman滤波的函数有: psinstypedef(nnm ...

2. PSINS工具箱学习（一）下载安装初始化、SINS-GPS组合导航仿真、习惯约定与常用变量符号、数据导入转换、绘图显示

   文章目录 一.前言 二.相关资源 三.下载安装初始化 1.下载PSINSyymmdd.rar工具箱文件 2.解压文件 3.初始化 4.启动工具箱导览 四.习惯约定与常用变量符号 1.PSINS全局变量 ...

3. SINS/GNSS组合导航仿真应用详细版(基于PSINS工具箱 )

   文章目录 轨迹仿真 生成轨迹数据 绘制仿真数据 惯导仿真 纯惯导仿真 SINS/GPS组合导航 总结 1 2 3 4 5 轨迹仿真 生成轨迹数据 首先,打开demos\test_trj.m文件,运行仿 ...

4. PSINS工具箱15状态组合导航仿真程序(test_SINS_GPS_153)浅析-卡尔曼滤波设置+导航解算

   文章目录 test_SINS_GPS_153源码 poserrset kfinit gabias kfupdate 流程 test_SINS_GPS_153源码 poserrset function ...

5. PSINS开源代码初体验——航迹仿真与组合导航

   文章目录 航迹仿真 组合导航仿真 PSINS是西北工业大学严恭敏教授开发的高精度捷联惯性系统及其组合导航系统开源软件(matlab, C++),软件及其他参考资料下载可以参见www.psins.org ...

6. Psins代码解析之kalman松组合导航融合算法 test_SINS_GPS_153.mtest_SINS_GPS_186.mtest_SINS_GPS_193.m

   框架: 设置松组合导航算法中状态量.观测量数目:比如:psinstypedef(153),状态误差量为15维,量测量为3维: 对仿真生成的飞行轨迹(test_SINS_trj.m),设置IMU误差:并 ...

7. 组合导航原理-松组合+紧组合概念

   文章目录 组合导航系统 组合导航的概念 广义 狭义 什么是滤波器 互补滤波 惯性导航中的卡尔曼滤波思想 GNSS/INS组合导航分类 松组合 概念 特点 紧组合 概念 特点 深组合 概念 特点 GNS ...

8. 多源传感器GNSS INS 视觉 LiDAR 组合导航与SLAM开源项目总结

   多源传感器GNSS INS 视觉 LiDAR 组合导航与SLAM开源项目总结 本文基于 吴桐wutong 微信公众号文章完善而来. 开源代码总览 名称 传感器类型 组合类型 滤波方法 备注 RTKLI ...

9. GNSS/INS组合导航学习-GINAV（一）

   从今天开始整理一下,最近半年学习的组合导航算法 目前开源程序 1.严老师PSINS工具箱 (MATLAB--卡尔曼滤波算法) 2.GINAV (MATLAB--卡尔曼滤波算法 ) 3.KF-GINS ...

最新文章

1. SSM框架之批量增加示例(同步请求jsp视图解析)
2. SMA携手LG推出住宅光伏储能逆变器
3. linux同步工具rsync​
4. Android 取得 ListView中每一个Item项目的值
5. IIS 7.0 的 ASP.NET 应用程序生命周期概述
6. Helloworld【C#】
7. 最简易上手的Numpy学习笔记一
8. 7-5 统计大写辅音字母 (15 分)
9. 【NOIP2015】【Vijos1979】信息传递（有向图最小环大小）
10. Physical Plausible Shading
11. WPF 3D：简单的Point3D和Vector3D动画创造一个旋转的正方体
12. 【VS2015】 C++实现硬件ID的查询
13. xcode13创建mac控制台应用程序
14. 网络安全工程师面试题合集（不全，暂不整理了）
15. R语言加载UCI糖尿病数据集、并启动Rattle GUI、调用party包中的ctree函数构建条件推理树模型、Rattle混淆矩阵、使用R自定义编写函数通过混淆矩阵计算特异度、敏感度、PPV、NPV
16. Excel如何批量删除公式只保留数值
17. mui+html5+实现扫描二维码操作
18. MySQL 免安装版的安装过程
19. NLP学习基础入门（上）
20. 《模型轻量化-剪枝蒸馏量化系列》YOLOv5无损剪枝（附源码）

热门文章

1. aix系统怎么修改服务器时间同步,AIX时间同步
2. ThreadLocal应用实例
3. 数独窟(Sudoku) -- Silverlight
4. 续ShaderEditor、Inspector之后又一成功爆品，2周260+单！
5. 计算机实训安全考试答案,(实验室安全在线测试题答案.doc
6. 世界五大黑帮现状扫瞄
7. 红蓝攻防演练怎样构建实战化网络安全防御体系
8. 安卓的SMS 短信的增删改查
9. python爬取豆瓣排名前250部电影封面
10. Recycleview模仿瑞幸咖啡菜单物品列表