PSINS组合导航工具箱基本概念与函数简介
文章目錄
- 習慣約定與常用變量符號
- PSINS全局變量結構體glv（global variable）
- 坐標系定義
- 姿態(tài)陣/姿態(tài)四元數(shù)/姿態(tài)角
- IMU采樣數(shù)據(jù)
- AVP導航參數(shù)
- 誤差參數(shù)
- 其他
- 導入數(shù)據(jù)文件與數(shù)據(jù)提取轉換
- 導入文件數(shù)據(jù)有以下方式：
- 數(shù)據(jù)提取轉換
- 舉例
- 繪圖顯示
- 繪圖輔助函數(shù)
- 傳感器數(shù)據(jù)繪圖
- 導航結果繪圖
- 進度條函數(shù)
- 姿態(tài)陣/姿態(tài)四元數(shù)/歐拉角/等效旋轉矢量之間轉換
習慣約定與常用變量符號
PSINS全局變量結構體glv（global variable）
運行glvs腳本文件，內(nèi)部實際調(diào)用的是glvf函數(shù)，這個函數(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ù)中的三個參數(shù)分別為地球半徑、地球扁率和地球自轉角速率(默認WGS84坐標系)；glv.mg表示毫g，glv.ug表示微g，工具箱中所有物理量在內(nèi)部計算都使用標準單位，比如角度用rad、比力用m/s^2等；只在初始化輸入?yún)?shù)時才會使用習慣單位，比如陀螺漂移用°/h。
坐標系定義
慣性坐標系(i)；
地球坐標系(e)，即ECEF坐標系；
導航坐標系(n)：東E-北N-天U；
載體坐標系(b)：右R-前F-上U。
陀螺儀和加速度計測量的值都是在慣性坐標系下的；
e系與n系
b系
姿態(tài)陣/姿態(tài)四元數(shù)/姿態(tài)角
姿態(tài)陣：Cnb，書寫一般遵從規(guī)律是從左到右從上到下，即為Cnb，它表示從b系到n系的坐標變換矩陣。對應姿態(tài)四元數(shù)寫為qnb。
姿態(tài)/歐拉角向量：att=[俯仰pitch; 橫滾roll; 方位yaw]
IMU采樣數(shù)據(jù)
imu=[wm; vm; t]，通常時標總是放在最后一列。
其中，wm為陀螺三軸角增量(角速率積分)、vm為加表三軸速度增量(比力的積分)，PSINS慣導算法里使用的陀螺和加表輸入都是增量信息（分別對應單位rad和m/s），如果用戶數(shù)據(jù)中是角速度/比力信息，則簡單地乘以采樣間隔ts處理即可。
AVP導航參數(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ù)
失準角誤差phi=[phiE;phiN;phiU]；速度誤差dvn；位置誤差dpos=[dlat;dlon;dhgt]；
陀螺漂移eb=[ebx;eby;ebz]；加表零偏db=[dbx;dby;dbz]；web為陀螺角度隨機游走/角速率白噪聲；wdb為加計速度隨機游走/比力白噪聲；
陀螺標定誤差矩陣dKg；加表標定誤差矩陣dKa；
IMU誤差結構體imuerr，包含較多成員，可見imuerrset函數(shù)。
其他
角速度wnie：表示w^n_{ie}即e系相對于i系的角速度在n系的投影，書寫一般遵從規(guī)律是從左到右從上到下；wnin和wnen等變量符號類似；
phim/dvbm：經(jīng)不可交換誤差補償后的等效旋轉矢量/比力速度增量；
gn：當?shù)刂亓κ噶縢n=[0;0;-g]，g為重力大小；gcc有害加速度；
trj：仿真軌跡結構體（參見trjsimu函數(shù)）；
ins：指北方位捷聯(lián)導航解算結構體（參見insinit函數(shù)）；
eth：導航地球相關計算結構體（參見ethinit函數(shù)）；
kf：Kalman濾波結構體（參見kfinit函數(shù)）；
ps：POS信息融合結構體（POS Fusion）；
導入數(shù)據(jù)文件與數(shù)據(jù)提取轉換
導入文件數(shù)據(jù)有以下方式：
二進制（純double型）格式文件，使用binfile函數(shù)，這對導入C語言生成的數(shù)據(jù)文件快速方便；或者可參照binfile，使用fread自行編程導入特定格式的二進制文件；
文本文件/或.mat格式文件，使用Matlab的load或importdata函數(shù)；
特殊格式的PSINS-IMU/AVP文件，可用imufile/avpfile等函數(shù)。
binfile函數(shù)內(nèi)容如下：
數(shù)據(jù)提取轉換
從文件直接導入Matlab工作空間的數(shù)據(jù)通常是一個二維數(shù)組，其各列順序及量綱單位不一定符合PSINS的習慣，需再進行數(shù)據(jù)提取和轉換：
使用imuidx提取IMU數(shù)據(jù)并進行單位轉換，陀螺為角增量、加表為速度增量；如需要，還可借助于imurfu函數(shù)將IMU轉換至右-前-上坐標系；
使用avpidx提取AVP數(shù)據(jù)并進行單位轉換，結果姿態(tài)/緯經(jīng)為弧度、方位角北偏西為正；
使用gpsidx提取GNSS速度/定位數(shù)據(jù)并進行單位轉換，緯經(jīng)度為弧度；通常GNSS的頻率低于IMU頻率，為刪除重復數(shù)據(jù)行可調(diào)用norep函數(shù)；為刪除數(shù)據(jù)為0行可調(diào)用no0函數(shù)；
使用tshift或adddt函數(shù)可將數(shù)據(jù)的起始時間轉換至指定的相對時間。
tshift和adddt函數(shù)代碼如下：
舉例
打開并運行demos\test_IMUAVPGPS_extract_trans.m程序，通過Matlab\Variable Editor查看數(shù)據(jù)結果如下（注意數(shù)據(jù)單位及時標變化）：
% 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ù)如下格式：
是一個22列的數(shù)據(jù)，要從其中提取出來IMU，AVP，GPS的信息；使用到的就是imuidx、avpidx和gpsidx函數(shù)，具體可參見源碼；這些函數(shù)都是按照指定列，將數(shù)據(jù)根據(jù)PSINS標準的數(shù)據(jù)進行讀取并輸出；tshift(imu,avp,gps,10)函數(shù)是將這些值的參考時間統(tǒng)一到10s起始的時間。
繪圖顯示
繪圖輔助函數(shù)
myfig：繪制白底全屏圖；
xygo：啟用網(wǎng)格（grid on），給出坐標標識（特殊標識由labeldef給出）；
labeldef：為簡潔給出坐標簡寫標識，摘錄如下（詳見labeldef.m文件）
傳感器數(shù)據(jù)繪圖
以上是博主利用實測IMU數(shù)據(jù)，在matlab中繪制的參數(shù)圖，可以根據(jù)自己的情況來完成。
導航結果繪圖
insplot：導航結果AVP（甚至陀螺漂移eb、加表零偏db）繪圖；
inserrplot/avpcmpplot：導航誤差/比較繪圖；
kfplot/xpplot：Kalman濾波結果狀態(tài)或均方差陣對角線元素(Pk陣對角元素的開方，注：當水平失準角均方差曲線有明顯下降，說明加計零偏可能估計出來了；當方位角有下降時，也可能說明陀螺漂移估計出來了)繪圖。
進度條函數(shù)
進度條函數(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ù)/歐拉角/等效旋轉矢量之間轉換
各種姿態(tài)數(shù)學描述之間的轉換函數(shù)如下:
其中m代表姿態(tài)陣，a代表姿態(tài)角，rv代表等效旋轉矢量，q代表四元數(shù)。
常用的還有：
卡爾曼濾波中，如果姿態(tài)角用四元數(shù)表示，但是你估計出來的是失準角，那么你就需要用到上述函數(shù)，對四元數(shù)進行反饋修正；計算和參考的四元數(shù)差異就是失準角；安裝誤差角，為兩套慣導放在一個車上跑，三個b系不平行，之間的關系
總結
以上是生活随笔為你收集整理的PSINS组合导航工具箱基本概念与函数简介的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： AppScan扫描报告
- 下一篇： 关于rj11、rj12、rj45