文章目錄

• 習(xí)慣約定與常用變量符號(hào)
• • PSINS全局變量結(jié)構(gòu)體glv（global variable）
  • 坐標(biāo)系定義
  • 姿態(tài)陣/姿態(tài)四元數(shù)/姿態(tài)角
  • IMU采樣數(shù)據(jù)
  • AVP導(dǎo)航參數(shù)
  • 誤差參數(shù)
  • 其他
• 導(dǎo)入數(shù)據(jù)文件與數(shù)據(jù)提取轉(zhuǎn)換
• • 導(dǎo)入文件數(shù)據(jù)有以下方式：
  • 數(shù)據(jù)提取轉(zhuǎn)換
  • 舉例
• 繪圖顯示
• • 繪圖輔助函數(shù)
  • 傳感器數(shù)據(jù)繪圖
  • 導(dǎo)航結(jié)果繪圖
  • 進(jìn)度條函數(shù)
• 姿態(tài)陣/姿態(tài)四元數(shù)/歐拉角/等效旋轉(zhuǎn)矢量之間轉(zhuǎn)換

習(xí)慣約定與常用變量符號(hào)

PSINS全局變量結(jié)構(gòu)體glv（global variable）

運(yùn)行g(shù)lvs腳本文件，內(nèi)部實(shí)際調(diào)用的是glvf函數(shù)，這個(gè)函數(shù)就是可以初始化全局變量，代碼如下：

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;

其中傳入函數(shù)中的三個(gè)參數(shù)分別為地球半徑、地球扁率和地球自轉(zhuǎn)角速率(默認(rèn)WGS84坐標(biāo)系)；glv.mg表示毫g，glv.ug表示微g，工具箱中所有物理量在內(nèi)部計(jì)算都使用標(biāo)準(zhǔn)單位，比如角度用rad、比力用m/s^2等；只在初始化輸入?yún)?shù)時(shí)才會(huì)使用習(xí)慣單位，比如陀螺漂移用°/h。

坐標(biāo)系定義

慣性坐標(biāo)系(i)；
地球坐標(biāo)系(e)，即ECEF坐標(biāo)系；
導(dǎo)航坐標(biāo)系(n)：東E-北N-天U；
載體坐標(biāo)系(b)：右R-前F-上U。
陀螺儀和加速度計(jì)測(cè)量的值都是在慣性坐標(biāo)系下的；

e系與n系

b系

姿態(tài)陣/姿態(tài)四元數(shù)/姿態(tài)角

姿態(tài)陣：Cnb，書(shū)寫一般遵從規(guī)律是從左到右從上到下，即為Cnb，它表示從b系到n系的坐標(biāo)變換矩陣。對(duì)應(yīng)姿態(tài)四元數(shù)寫為qnb。
姿態(tài)/歐拉角向量：att=[俯仰pitch; 橫滾roll; 方位yaw]

IMU采樣數(shù)據(jù)

imu=[wm; vm; t]，通常時(shí)標(biāo)總是放在最后一列。
其中，wm為陀螺三軸角增量(角速率積分)、vm為加表三軸速度增量(比力的積分)，PSINS慣導(dǎo)算法里使用的陀螺和加表輸入都是增量信息（分別對(duì)應(yīng)單位rad和m/s），如果用戶數(shù)據(jù)中是角速度/比力信息，則簡(jiǎn)單地乘以采樣間隔ts處理即可。

AVP導(dǎo)航參數(shù)

avp=[att; vn; pos; t]。
其中，
姿態(tài)att=[俯仰pitch; 橫滾roll; 方位yaw]；
速度vn=[東速vE; 北速vN; 天速vU]；
位置向量：pos=[緯度lat; 經(jīng)度lon; 高度hgt]。
注意：俯仰/橫滾/方位/緯度/經(jīng)度均為弧度單位rad。

誤差參數(shù)

失準(zhǔn)角誤差phi=[phiE;phiN;phiU]；速度誤差dvn；位置誤差dpos=[dlat;dlon;dhgt]；
陀螺漂移eb=[ebx;eby;ebz]；加表零偏db=[dbx;dby;dbz]；web為陀螺角度隨機(jī)游走/角速率白噪聲；wdb為加計(jì)速度隨機(jī)游走/比力白噪聲；
陀螺標(biāo)定誤差矩陣dKg；加表標(biāo)定誤差矩陣dKa；
IMU誤差結(jié)構(gòu)體imuerr，包含較多成員，可見(jiàn)imuerrset函數(shù)。

其他

角速度wnie：表示w^n_{ie}即e系相對(duì)于i系的角速度在n系的投影，書(shū)寫一般遵從規(guī)律是從左到右從上到下；wnin和wnen等變量符號(hào)類似；
phim/dvbm：經(jīng)不可交換誤差補(bǔ)償后的等效旋轉(zhuǎn)矢量/比力速度增量；
gn：當(dāng)?shù)刂亓κ噶縢n=[0;0;-g]，g為重力大小；gcc有害加速度；
trj：仿真軌跡結(jié)構(gòu)體（參見(jiàn)trjsimu函數(shù)）；
ins：指北方位捷聯(lián)導(dǎo)航解算結(jié)構(gòu)體（參見(jiàn)insinit函數(shù)）；
eth：導(dǎo)航地球相關(guān)計(jì)算結(jié)構(gòu)體（參見(jiàn)ethinit函數(shù)）；
kf：Kalman濾波結(jié)構(gòu)體（參見(jiàn)kfinit函數(shù)）；
ps：POS信息融合結(jié)構(gòu)體（POS Fusion）；

導(dǎo)入數(shù)據(jù)文件與數(shù)據(jù)提取轉(zhuǎn)換

導(dǎo)入文件數(shù)據(jù)有以下方式：

二進(jìn)制（純double型）格式文件，使用binfile函數(shù)，這對(duì)導(dǎo)入C語(yǔ)言生成的數(shù)據(jù)文件快速方便；或者可參照binfile，使用fread自行編程導(dǎo)入特定格式的二進(jìn)制文件；
文本文件/或.mat格式文件，使用Matlab的load或importdata函數(shù)；
特殊格式的PSINS-IMU/AVP文件，可用imufile/avpfile等函數(shù)。
binfile函數(shù)內(nèi)容如下：

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);

數(shù)據(jù)提取轉(zhuǎn)換

從文件直接導(dǎo)入Matlab工作空間的數(shù)據(jù)通常是一個(gè)二維數(shù)組，其各列順序及量綱單位不一定符合PSINS的習(xí)慣，需再進(jìn)行數(shù)據(jù)提取和轉(zhuǎn)換：
使用imuidx提取IMU數(shù)據(jù)并進(jìn)行單位轉(zhuǎn)換，陀螺為角增量、加表為速度增量；如需要，還可借助于imurfu函數(shù)將IMU轉(zhuǎn)換至右-前-上坐標(biāo)系；
使用avpidx提取AVP數(shù)據(jù)并進(jìn)行單位轉(zhuǎn)換，結(jié)果姿態(tài)/緯經(jīng)為弧度、方位角北偏西為正；
使用gpsidx提取GNSS速度/定位數(shù)據(jù)并進(jìn)行單位轉(zhuǎn)換，緯經(jīng)度為弧度；通常GNSS的頻率低于IMU頻率，為刪除重復(fù)數(shù)據(jù)行可調(diào)用norep函數(shù)；為刪除數(shù)據(jù)為0行可調(diào)用no0函數(shù)；
使用tshift或adddt函數(shù)可將數(shù)據(jù)的起始時(shí)間轉(zhuǎn)換至指定的相對(duì)時(shí)間。
tshift和adddt函數(shù)代碼如下：

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;

舉例

打開(kāi)并運(yùn)行demos\test_IMUAVPGPS_extract_trans.m程序，通過(guò)Matlab\Variable Editor查看數(shù)據(jù)結(jié)果如下（注意數(shù)據(jù)單位及時(shí)標(biāo)變化）：

% 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

制造的數(shù)據(jù)如下格式：

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

是一個(gè)22列的數(shù)據(jù)，要從其中提取出來(lái)IMU，AVP，GPS的信息；使用到的就是imuidx、avpidx和gpsidx函數(shù)，具體可參見(jiàn)源碼；這些函數(shù)都是按照指定列，將數(shù)據(jù)根據(jù)PSINS標(biāo)準(zhǔn)的數(shù)據(jù)進(jìn)行讀取并輸出；tshift(imu,avp,gps,10)函數(shù)是將這些值的參考時(shí)間統(tǒng)一到10s起始的時(shí)間。

繪圖顯示

繪圖輔助函數(shù)

myfig：繪制白底全屏圖；

xygo：啟用網(wǎng)格（grid on），給出坐標(biāo)標(biāo)識(shí)（特殊標(biāo)識(shí)由labeldef給出）；

labeldef：為簡(jiǎn)潔給出坐標(biāo)簡(jiǎn)寫標(biāo)識(shí)，摘錄如下（詳見(jiàn)labeldef.m文件）

傳感器數(shù)據(jù)繪圖

以上是博主利用實(shí)測(cè)IMU數(shù)據(jù)，在matlab中繪制的參數(shù)圖，可以根據(jù)自己的情況來(lái)完成。

導(dǎo)航結(jié)果繪圖

insplot：導(dǎo)航結(jié)果AVP（甚至陀螺漂移eb、加表零偏db）繪圖；
inserrplot/avpcmpplot：導(dǎo)航誤差/比較繪圖；
kfplot/xpplot：Kalman濾波結(jié)果狀態(tài)或均方差陣對(duì)角線元素(Pk陣對(duì)角元素的開(kāi)方，注：當(dāng)水平失準(zhǔn)角均方差曲線有明顯下降，說(shuō)明加計(jì)零偏可能估計(jì)出來(lái)了；當(dāng)方位角有下降時(shí)，也可能說(shuō)明陀螺漂移估計(jì)出來(lái)了)繪圖。

進(jìn)度條函數(shù)

進(jìn)度條函數(shù)(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

姿態(tài)陣/姿態(tài)四元數(shù)/歐拉角/等效旋轉(zhuǎn)矢量之間轉(zhuǎn)換

各種姿態(tài)數(shù)學(xué)描述之間的轉(zhuǎn)換函數(shù)如下:

其中m代表姿態(tài)陣，a代表姿態(tài)角，rv代表等效旋轉(zhuǎn)矢量，q代表四元數(shù)。
常用的還有：

卡爾曼濾波中，如果姿態(tài)角用四元數(shù)表示，但是你估計(jì)出來(lái)的是失準(zhǔn)角，那么你就需要用到上述函數(shù)，對(duì)四元數(shù)進(jìn)行反饋修正；計(jì)算和參考的四元數(shù)差異就是失準(zhǔn)角；安裝誤差角，為兩套慣導(dǎo)放在一個(gè)車上跑，三個(gè)b系不平行，之間的關(guān)系

總結(jié)

以上是生活随笔為你收集整理的PSINS组合导航工具箱基本概念与函数简介的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。