| PSINS组合导航工具箱基本概念与函数简介 | 您所在的位置：网站首页 › kissboys组合简介 › PSINS组合导航工具箱基本概念与函数简介 |
PSINS组合导航工具箱基本概念与函数简介
|
文章目录
习惯约定与常用变量符号PSINS全局变量结构体glv（global variable）坐标系定义姿态阵/姿态四元数/姿态角IMU采样数据AVP导航参数误差参数其他
导入数据文件与数据提取转换导入文件数据有以下方式：数据提取转换举例
绘图显示绘图辅助函数传感器数据绘图导航结果绘图进度条函数
姿态阵/姿态四元数/欧拉角/等效旋转矢量之间转换
习惯约定与常用变量符号
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。 陀螺仪和加速度计测量的值都是在惯性坐标系下的； 姿态阵：Cnb，书写一般遵从规律是从左到右从上到下，即为Cnb，它表示从b系到n系的坐标变换矩阵。对应姿态四元数写为qnb。 姿态/欧拉角向量：att=[俯仰pitch; 横滚roll; 方位yaw] 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 nargin0, 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 narginkk})==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
myfig：绘制白底全屏图； xygo：启用网格（grid on），给出坐标标识（特殊标识由labeldef给出）； labeldef：为简洁给出坐标简写标识，摘录如下（详见labeldef.m文件）
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 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 |
e系与n系 
b系
制造的数据如下格式：
是一个22列的数据，要从其中提取出来IMU，AVP，GPS的信息；使用到的就是imuidx、avpidx和gpsidx函数，具体可参见源码；这些函数都是按照指定列，将数据根据PSINS标准的数据进行读取并输出；tshift(imu,avp,gps,10)函数是将这些值的参考时间统一到10s起始的时间。
以上是博主利用实测IMU数据，在matlab中绘制的参数图，可以根据自己的情况来完成。
【本文地址】