MTI-G-710/GNSS組合導航代碼分析

這裡寫自定義目錄标題

• 前言
• 代碼分析
• • 主函數
  • sinsgps核心函數
• IMU參數辨識
• 結果分析
• • 1.原始資料分析
  • 2.陀螺儀零偏估計
  • 3.加速度計零偏估計
  • 4.自适應卡爾曼濾波
  • 5.組合導航下的方位角分析
  • 6. 調參
  • • 6.1 P矩陣

前言

車載測試采集了MTI-G-710 Mems IMU，GNSS，光纖慣導的資料，MTI-G-710産品輸出姿态角資料，現場測試圖如下所示：

非常感謝嚴老師，他對資料進行了分析群組合導航算法調參，代碼和資料可以從如下網站下載下傳：

代碼和資料

代碼分析

代碼運作需要嚴老師psins210406組合導航函數庫的支援，這個開源組合導航函數庫由嚴老師開發和維護。

主函數

主函數我做了大量的詳細注釋，如下所示：

glvs;

close all;

ts = 1/100;

t1 = 700; t2 = 1300;
%t1 = 700; t2 = 10000;

load lb_memsfoggps;

imuplot(imuFOG); 
imuplot(imuMTI); 
gpsplot(gps); 
insplot(attMTI, 'a');% MTI産品輸出的姿态角

% 慣性系對準{過程持續300s}
% 産品輸出yaw初始對準：-53.084344000000002
% att0結果：-53.1423
att0 = aligni0(imuFOG(400/ts:t1/ts,:), gps(1,4:6)');

% 光纖慣導純慣性解算
avp = inspure(imuFOG(t1/ts:t2/ts,:), [att0; getat(gps,t1)], 'H');

% 光纖慣導姿态角和MTI姿态角比較
avpcmpplot(avp(:,[1:3,end]), datacut(attMTI,t1,t2), 'a', 'datt');
 
avp0 = [att0; getat(gps,t1)];

ins = insinit(avp0, ts);

% 歐拉角初始誤差：pitch:600角分,roll:600角分,偏航角：1800角分
% 速度初始誤差：10m/s
% 位置初始誤差：100m
avperr = avperrset([10*60;30*60], 10, 100);

% 設定IMU噪聲，噪聲含義如下：
% 陀螺儀偏置：1000deg/h (設定初始零偏，設定P矩陣)
% 加速度計偏置：10000ug[10mg] (設定初始零偏，設定P矩陣)
% 陀螺儀高斯噪聲： 0.1deg/sqrt(Hz)
% 加速度計高斯噪聲：100ug/sqrt(Hz)
imuerr = imuerrset(1000, 10000, 0.1, 100);

% 限制P矩陣(最小值)
% 歐拉角初始誤差：pitch:0.5角分,roll:0.5角分,偏航角：2角分
% 速度初始誤差：0.01m/s
% 位置初始誤差：0.01m
% 陀螺零偏：1deg/h,1deg/h,1deg/h
% 加速度計零偏：100ug,100ug,100ug
% 杆臂(空間不同步誤差)：0.01m
% 時間不同步誤差：0.01s
Pmin = [avperrset([0.5,2],0.01,0.01); gabias(1, [100,100]); [0.01;0.01;0.01]; 0.01].^2;
% Rmin:觀測噪聲的最小值
% 速度初始誤差：0.1m/s
% 位置初始誤差：0.1m
Rmin = vperrset(0.1, 0.1).^2;

% rep3(1):lever協方差
% 0.1:dT協方差
% poserrset(10):GPS速度觀測噪聲：1m/s 1m/s 1m/s GPS位置觀測噪聲：10m 10m 10m
[avp1, xkpk1, zkrk1, sk] = sinsgps(imuMTI(t1/ts:t2/ts,:), gps, ins, avperr, imuerr, rep3(1), 0.1, vperrset(1,10), Pmin, Rmin, 'avped');

avpcmpplot(avp, avp1, 'avp', 'mu');% mems_sins/gnss組合導航和FOG資料比較姿态
avpcmpplot(gps, avp1, 'vp'); % mems_sins/gnss組合導航和gnss資料比較速度和位置
           

因為原始資料中有光纖慣導的加速度計資料和陀螺儀資料，是以代碼中用光纖慣導解算出來的姿态角和方位角作為真值，因為GNSS大部分工作在RTK模式，是以速度和位置是精度比較高的，是以作為速度和位置的真值。

光纖慣導的初始對準嚴老師用了慣性系對準，具體方法可以參考嚴老師的博士後報告。通過和光纖慣導輸出的初始方位角進行比較，差别大約為0.06deg。主函數接下來主要是設定卡爾曼濾波參數PQR，sinsgps函數是sins/gnss組合導航的核心函數。最後的avpcmpplot函數是通過和真值之間的對比，用于可視化組合導航的結果。

sinsgps核心函數

sinsgps函數的注釋如下：

function [avp, xkpk, zkrk, ins, kf] = sinsgps(imu, gps, ins, davp, imuerr, lever, dT, rk, Pmin, Rmin, fbstr)
% 19-state SINS/GPS integrated navigation Kalman filter.
% The 19-state includes:
%       phi(3), dvn(3), dpos(3), eb(3), db(3), lever(3), dT(1)
%
% Example1:
% [avp1, xkpk, zkrk, ins1, kf] = sinsgps(imu, gps, 300);
%
% Example2:
% ins = insinit([yaw;pos], ts);
% avperr = avperrset([60;300], 1, 100);
% imuerr = imuerrset(0.03, 100, 0.001, 1);
% Pmin = [avperrset([0.1,1],0.001,0.01); gabias(0.1, [10,30]); [0.01;0.01;0.01]; 0.0001].^2;
% Rmin = vperrset(0.001, 0.01).^2;
% [avp1, xkpk, zkrk, ins1, kf] = sinsgps(imu, gps, ins, avperr, imuerr, rep3(1), 0.01, vperrset(0.1,10), Pmin, Rmin, 'avp');
%
% See also  kfinit, kfupdate

% Copyright(c) 2009-2021, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 09/10/2013, 06/02/2021
% nn = 2
% ts = 0.01
% nts = 0.02
    [nn, ts, nts] = nnts(2, diff(imu(1:2,end)));
    if size(gps,2)<=5, gpspos_only=1; pos0=gps(1,1:3)'; else, gpspos_only=0; pos0=gps(1,4:6)'; end 
    if ~exist('rk', 'var')
        % rk：GPS觀測噪聲(标準差)
        if gpspos_only==1, rk=poserrset([10,30]);
        else, rk=vperrset([0.1;0.3],[10,30]); end
    end
    if ~exist('dT', 'var'), dT = 0.01; end
    if ~exist('lever', 'var'), lever = rep3(1); end
    if ~exist('imuerr', 'var'), imuerr = imuerrset(0.01, 100, 0.001, 1); end
    if ~exist('davp', 'var'), davp = avperrset([10;300], 1, [10;30]); end
    if ~exist('ins', 'var'), ins=100; end
    if ~isstruct(ins) % T=ins align time
        [~, res0] = aligni0(imu(1:fix(ins/ts),:), pos0); % 慣性系對準
        ins = insinit([res0.attk(1,1:3)'; 0;0;0; pos0], ts); ins.nts=nts;
    end
    psinstypedef(196-gpspos_only*3);
    kf = [];
    % kf.Qt：預測噪聲
    % 狀态變量： 19狀态：歐拉角,速度,位置,陀螺零偏,加速度計零偏,杆臂,時間不同步誤差
    % imuerr.sqg:一階馬爾科夫噪聲，這裡是0
    % imuerr.sqa:一階馬爾科夫噪聲，這裡是0
    % imuerr.web:陀螺儀高斯噪聲
    % imuerr.wdb:加速度計高斯噪聲
    % 預測噪聲中隻考慮了陀螺儀高斯噪聲和加速度計高斯噪聲
    kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(3,1); imuerr.sqg; imuerr.sqa; zeros(3,1); 0])^2;
    % kf.Rk：觀測噪聲[GPS速度噪聲,GPS位置噪聲]
    kf.Rk = diag(rk)^2;
    % kf.Pxk：協方差矩陣設定
    % davp：歐拉角初始誤差：pitch:600角分,roll:600角分,偏航角：1800角分
    % davp：速度初始誤差：10m/s
    % davp：位置初始誤差：10m
    % imuerr.eb：陀螺儀零偏
    % imuerr.db：加速度計零偏
    % rep3(lever)：杆臂
    % dT：時間不同步誤差
    kf.Pxk = diag([davp; imuerr.eb; imuerr.db; rep3(lever); dT]*1.0)^2;
    % kf.Hk：觀測轉換矩陣
    % kfinit0：初始化KF結構體
    kf.Hk = zeros(length(rk),19); % 初始化
    % kfinit0：初始化KF結構體
    kf = kfinit0(kf, nts);
    % Pmax在kfinit0中指派
    if exist('Pmin', 'var')
        if length(Pmin)==1, kf.pconstrain=0;
        else, kf.Pmin = Pmin; kf.pconstrain = 1; end
    end
    
    % 自适應濾波：主要是對觀測自适應
    % Rmin和Rmax預設值在kfinit0中設定
    kf.adaptive = 1;
    if exist('Rmin', 'var')
        if length(Rmin)==1, kf.adaptive=0; end
        if kf.adaptive==1, kf.Rmin = diag(Rmin); end
    end
    % fbstr:回報量
    if exist('fbstr', 'var'), kf.fbstr=fbstr; end
    kf.xtau = [ [1;1;1]; [1;1;1]; [1;1;1]; [1;1;1]; [1;1;1]; [1;1;1]; 1];
    imugpssyn(imu(:,7), gps(:,end)); % imu,gps軟體同步
    len = length(imu); [avp, xkpk, zkrk] = prealloc(fix(len/nn), 16, 2*kf.n+1, 2*kf.m+1); % 預配置設定空間
    timebar(nn, len, '19-state SINS/GPS simulation.'); ki = 1; kiz = 1;
    for k=1:nn:len-nn+1
        k1 = k+nn-1; 
        wvm = imu(k:k1,1:6); t = imu(k1,end);
        ins = insupdate(ins, wvm); % imu前向預測過程
        % kf.Phikk_1：計算狀态轉移矩陣
        kf.Phikk_1 = kffk(ins);
        kf = kfupdate(kf);
        % 由imugpssyn的同步機制可以知道INS資料和GPS資料的時間同步誤差最大為：慣導資料周期：T_sins
        % 找到了兩個慣導資料之間的GPS資料，否則：kgps=0
        [kgps, dt] = imugpssyn(k, k1, 'F');
        if kgps>0 % 如果有GPS資料
            % sins資料和gps資料轉到一個統一的坐标系{這裡是GPS天線所在的坐标系}
            ins = inslever(ins); % 杆臂補償
            % ins.vnL:杆臂補償後的慣導速度（導航系）
            % ins.tDelay一直等于0
            % dtpos一直等于0
            % fprintf("ins.tDelay = %.6f\n",ins.tDelay);
            dtpos=+vn2dpos(ins.eth,ins.vnL,ins.tDelay); 
            if gpspos_only==1
                zk = ins.posL+dtpos-gps(kgps,1:3)';
                kf.Hk = [zeros(3,6), eye(3), zeros(3,6), -ins.MpvCnb,-ins.Mpvvn];
            else
                zk = [ins.vnL+ins.tDelay*ins.an;ins.posL+dtpos]-gps(kgps,1:6)';
                % kf.Hk：觀測矩陣
                % eye(6)：速度和位置
                % -ins.CW:杆臂引起的速度誤差
                % -ins.MpvCnb:杆臂誤差引起的位置誤差
                % -ins.an：時間不同步誤差引起的速度誤差
                % -ins.Mpvvn：時間不同步誤差引起的位置誤差
                kf.Hk = [zeros(6,3), eye(6), zeros(6,6), [-ins.CW,-ins.an;-ins.MpvCnb,-ins.Mpvvn]];
            end
            kf = kfupdate(kf, zk, 'M');
            zkrk(kiz,:) = [zk; diag(kf.Rk); t];  kiz = kiz+1;
        end
        [kf, ins] = kffeedback(kf, ins, nts);
        avp(ki,:) = [ins.avp; ins.eb; ins.db; t]';
        xkpk(ki,:) = [kf.xk; diag(kf.Pxk); t]'; ki = ki+1;
        timebar;
    end
    avp(ki:end,:) = []; xkpk(ki:end,:) = [];  zkrk(kiz:end,:) = [];
    insplot(avp);
    kfplot(xkpk);
    % zkrk：
    % 1-6:觀測到的速度，位置誤差
    % 7-12:觀測到的速度，位置方差
    % 13:時間戳
    rvpplot(zkrk);
           

IMU參數辨識

MTI-G-710手冊标稱參數如下：

使用艾倫方差辨識IMU噪聲參數可以參考以下連結：

艾倫方差辨識IMU噪聲參數

下圖是MTI-G-710 Z軸陀螺儀艾倫方差：

容易得到：

零偏不穩定性：

B = 1 8 ∘ / ( 2 / 3 ) / h = 2 7 ∘ / h B=18^{\circ} /(2 / 3) / h=27^{\circ} / h B=18∘/(2/3)/h=27∘/h

可以看到比資料手冊中标稱的稍微大點。

角度随機遊走：

N = σ A R W ⋅ τ 60 = σ A R W 1 3600 h = 3 5 ∘ h 60 = 0.5 8 ∘ h N=\sigma_{A R W} \cdot \frac{\sqrt{\tau}}{60}=\sigma_{A R W} \sqrt{\frac{1}{3600} h}=\frac{\frac{35^{\circ}}{\sqrt{h}}}{60}=\frac{0.58^{\circ}}{\sqrt{h}} N=σARW​⋅60τ

​​=σARW​36001​h

​=60h

​35∘​​=h

​0.58∘​

下圖是MTI-G-710 Z軸加速度計艾倫方差：

角度随機遊走：

52 μ g H z 52 \frac{\mu g}{\sqrt{H z}} 52Hz

​μg​

零偏不穩定性：

17 u g ( 10 s ) 17 u g(10 s) 17ug(10s)

結果分析

1.原始資料分析

GPS資料大部分工作在RTK固定解模式下，少部分工作在單點定位模式或者浮點解模式下，如下所示：

GPS不同模式下的速度和位置誤差是不同的，通過分析觀測誤差，即可定量計算觀測誤差，進而設定不同的觀測噪聲，實作自适應卡爾曼濾波。

2.陀螺儀零偏估計

陀螺儀零偏估計結果如下所示：

可以看到各個軸的零偏差别比較大，零偏估計結果較為穩定，可以看到Y軸和Z軸估計出來的零偏比較大，需要注意的是由于組合導航中沒有扣除零偏常量，是以這個零偏中包含零偏重複項和零偏穩定性兩部分。通過對零偏常量的計算分析也可以證明這一點。

3.加速度計零偏估計

從上圖可以看出，在大約1000s左右時刻，加速度計快速收斂，這個時間，車輛轉了180deg，轉彎可以提高加速度計零偏的可觀測度。

4.自适應卡爾曼濾波

kfupdate函數中使用了自适應卡爾曼濾波，主要是自适應觀測噪聲，主要思想是通過分析觀測速度差和位置差資料，調整觀測噪聲。并且對觀測噪聲做了限幅操作。下面是觀測速度差和觀測速度R噪聲的圖表。可以看到當觀測速度差比較大時，R也随之增大，進而達到自适應的目的。

下面是完整的速度，位置觀測誤差以及對應的觀測噪聲R。

需要注意的是當觀測噪聲比較大時，濾波器的收斂速度會降低，具體可以參考嚴老師的部落格，連結如下所示：

量測噪聲自适應與收斂之間的沖突

5.組合導航下的方位角分析

下圖是組合導航估計出來的姿态角和方位角

下面是和光纖慣導解算出來的姿态角，方位角比較的誤差，如下所示：

可以看到整個過程中有3次轉彎，3次轉彎過程中方位角逐漸收斂到真值，說明轉彎可以增加方位角的可觀測度。

6. 調參

6.1 P矩陣

加速度計零偏嚴老師設定的為10mg，如圖是加速度計零偏對應的P矩陣的收斂情況：

可以看到收斂很快。

我将加速度計零偏改小到1mg，可以明顯看到收斂速度變慢，如下圖所示：