PSINS组合导航工具箱基本概念与函数简介
文章目录习惯约定与常用变量符号PSINS全局变量结构体glv（global variable）坐标系定义姿态阵/姿态四元数/姿态角IMU采样数据AVP导航参数误差参数其他导入数据文件与数据提取转换导入文件数据有以下方式：数据提取转换举例绘图显示绘图辅助函数传感器数据绘图导航结果绘图进度条函数姿态阵/姿态四元数/欧拉角/等效旋转矢量之间转换习惯约定与常用变量符号PSINS全局变量结构体glv（glo
文章目录
习惯约定与常用变量符号
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 glv
if ~exist('Re', 'var'), Re = []; end
if ~exist('f', 'var'), f = []; end
if ~exist('wie', 'var'), wie = []; end
if isempty(Re), Re = 6378137; end
if isempty(f), f = 1/298.257; end
if isempty(wie), wie = 7.2921151467e-5; end
glv.Re = Re; % the Earth's semi-major axis
glv.f = f; % flattening
glv.Rp = (1-glv.f)*glv.Re; % semi-minor axis
glv.e = sqrt(2*glv.f-glv.f^2); glv.e2 = glv.e^2; % 1st eccentricity
glv.ep = sqrt(glv.Re^2-glv.Rp^2)/glv.Rp; glv.ep2 = glv.ep^2; % 2nd eccentricity
glv.wie = wie; % the Earth's angular rate
glv.meru = glv.wie/1000; % milli earth rate unit
glv.g0 = 9.7803267714; % gravitational force
glv.mg = 1.0e-3*glv.g0; % milli g
glv.ug = 1.0e-6*glv.g0; % micro g
glv.mGal = 1.0e-3*0.01; % milli Gal = 1cm/s^2 ~= 1.0E-6*g0
glv.uGal = glv.mGal/1000; % micro Gal
glv.ugpg2 = glv.ug/glv.g0^2; % ug/g^2
glv.ws = 1/sqrt(glv.Re/glv.g0); % Schuler frequency
glv.ppm = 1.0e-6; % parts per million
glv.deg = pi/180; % arcdeg
glv.min = glv.deg/60; % arcmin
glv.sec = glv.min/60; % arcsec
glv.mas = glv.sec/1000; % milli arcsec
glv.hur = 3600; % time hour (1hur=3600second)
glv.dps = pi/180/1; % arcdeg / second
glv.rps = 360*glv.dps; % revolutions per second
glv.dph = glv.deg/glv.hur; % arcdeg / hour
glv.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) / hour
glv.Hz = 1/1; % Hertz
glv.dphpsHz = glv.dph/glv.Hz; % (arcdeg/hour) / sqrt(Hz)
glv.dphpg = glv.dph/glv.g0; % (arcdeg/hour) / g
glv.dphpg2 = glv.dphpg/glv.g0; % (arcdeg/hour) / g^2
glv.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*ugpsHz
glv.ppmpsh = glv.ppm/sqrt(glv.hur); % ppm / sqrt(hour)
glv.mil = 2*pi/6000; % mil
glv.nm = 1853; % nautical mile
glv.kn = glv.nm/glv.hur; % knot
%%
glv.wm_1 = [0,0,0]; glv.vm_1 = [0,0,0]; % the init of previous gyro & acc sample
glv.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 number
glv.v0 = [0;0;0]; % 3x1 zero-vector
glv.qI = [1;0;0;0]; % identity quaternion
glv.I33 = eye(3); glv.o33 = zeros(3); % identity & zero 3x3 matrices
glv.pos0 = [34.246048*glv.deg; 108.909664*glv.deg; 380]; % position of INS Lab@NWPU
glv.eth = []; glv.eth = earth(glv.pos0);
glv.t0 = 0;
glv.tscale = 1; % =1 for second, =60 for minute, =3600 for hour, =24*3600 for day
glv.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; end
if nargin==3, row1=row0; row0=0; end
columns = data;
fid = fopen(fname, 'rb');
if row0>0, fseek(fid, columns*row0*8, 'bof'); end
data1 = fread(fid, [columns,row1-row0], 'double')';
else % save: binfile(fname, data)
fid = fopen(fname, 'wb');
fwrite(fid, data', 'double');
end
fclose(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/2020
if nargin<2, dt=-data(1,end); end
data(:,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/2015
t0 = varargin{1}(1,end); t00 = 0;
kk = nargin;
if length(varargin{kk})==1, t00=varargin{kk}(1); kk=kk-1; end
for k=2:kk
t0 = min(t0, varargin{k}(1,end));
end
varargout = varargin(1:kk);
dt = t0-t00;
for k=1:kk
varargout{k}(:,end) = varargin{k}(:,end)-dt;
end
varargout{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_arg
if nargin>=2
tb_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);
end
end
if nargin<3, msgstr = []; end
tb_arg.handle = waitbar(0,[msgstr, ' Please wait...'], ...
'Name','PSINS Toolbox', 'WindowStyle', 'modal', ...
'CreateCancelBtn', 'delete(gcbf);');
return;
end
tb_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.tTotal001
r = tb_arg.tCur/tb_arg.tTotal;
if ishandle(tb_arg.handle)
if r>tb_arg.rClosed
close(tb_arg.handle);
% fprintf('\tCPU time used is %.3f sec.\n\n', cputime-tb_arg.time0);
else
waitbar(r, tb_arg.handle);
tb_arg.tOld = tb_arg.tCur;
end
else
if r<tb_arg.rClosed
clear global tb_arg;
error('PSINS processing is terminated by user.');
% disp('PSINS processing is terminated by user.\n');
% quit;
end
end
end
姿态阵/姿态四元数/欧拉角/等效旋转矢量之间转换
各种姿态数学描述之间的转换函数如下:
其中m代表姿态阵，a代表姿态角，rv代表等效旋转矢量，q代表四元数。
常用的还有：
卡尔曼滤波中，如果姿态角用四元数表示，但是你估计出来的是失准角，那么你就需要用到上述函数，对四元数进行反馈修正；计算和参考的四元数差异就是失准角；安装误差角，为两套惯导放在一个车上跑，三个b系不平行，之间的关系
开放原子开发者工作坊旨在鼓励更多人参与开源活动，与志同道合的开发者们相互交流开发经验、分享开发心得、获取前沿技术趋势。工作坊有多种形式的开发者活动，如meetup、训练营等，主打技术交流，干货满满，真诚地邀请各位开发者共同参与！
更多推荐
10
0- 0
已为社区贡献17条内容
所有评论(0)