PSINS惯性器件误差模拟与分析

惯性器件误差模拟与分析

误差模拟与注入

通常惯性器件误差包含漂移的常值零偏、一阶马尔可夫过程、随机游走、比例因数误差、不正交/安装误差角、加表的比例因数二阶非线性、加表内杆臂、陀螺与加表之间时间不同步等
imuerrset：设置IMU的各种误差参数，见下图；
imuadderr：给IMU注入误差。
imuerrset代码如下：

function imuerr = imuerrset(eb, db, web, wdb, sqrtR0G, TauG, sqrtR0A, TauA, dKGii, dKAii, dKGij, dKAij, KA2, rxyz, dtGA)
% SIMU errors setting, including gyro & acc bias, noise and installation errors, etc.
%
% Prototype: imuerr = imuerrset(eb, db, web, wdb, sqrtR0G, TauG, sqrtR0A, TauA, dKGii, dKAii, dKGij, dKAij, KA2, rxyz, dtGA)
% Inputs: including information as follows
%     eb - gyro constant bias (deg/h)
%     db - acc constant bias (ug)
%     web - angular random walk (deg/sqrt(h))
%     wdb - velocity random walk (ug/sqrt(Hz))
%     sqrtR0G,TauG - gyro correlated bias, sqrtR0G in deg/h and TauG in s
%     sqrtR0A,TauA - acc correlated bias, sqrtR0A in ug and TauA in s
%     dKGii - gyro scale factor error (ppm)
%     dKAii - acc scale factor error (ppm)
%     dKGij - gyro installation error (arcsec)
%     dKAij - acc installation error (arcsec)
%     KA2 - acc quadratic coefficient (ug/g^2)
%       where, 
%               |dKGii(1) dKGij(4) dKGij(5)|         |dKAii(1) 0        0       |
%         dKg = |dKGij(1) dKGii(2) dKGij(6)| , dKa = |dKAij(1) dKAii(2) 0       |
%               |dKGij(2) dKGij(3) dKGii(3)|         |dKAij(2) dKAij(3) dKAii(3)|
%     rxyz - acc inner-lever-arm, =[rx;ry;rz] (cm)
%     dtGA - time-asynchrony between gyro & acc, dtGA=Tacc_later-Tgyro_early>0 (ms)
% Output: imuerr - SIMU error structure array
%
% Example:
%     For inertial grade SIMU, typical errors are:
%       eb=0.01dph, db=50ug, web=0.001dpsh, wdb=10ugpsHz
%       scale factor error=10ppm, askew installation error=10arcsec
%       sqrtR0G=0.001dph, taug=1000s, sqrtR0A=10ug, taug=1000s
%    then call this funcion by
%       imuerr = imuerrset(0.01,100,0.001,10, 0.001,1000,10,1000, 10,10,10,10, 10, 10, 10);
%
% See also  imuadderr, gabias, avperrset, insinit, kfinit.

% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 11/09/2013, 06/03/2014, 15/02/2015, 22/08/2015, 17/08/2016, 24/07/2020
global glv
    if nargin==1  % for specific defined case 
        switch eb
            case 1,  imuerr = imuerrset(0.01, 100, 0.001, 10);
            case 2,  imuerr = imuerrset(0.01,100,0.001,10, 0.001,300,10,300, 10,10,10,10, 10, 2, 1);
        end
        return;
    end
    o31 = zeros(3,1); o33 = zeros(3);
    imuerr = struct('eb',o31, 'db',o31, 'web',o31, 'wdb',o31,...
        'sqg',o31, 'taug',inf(3,1), 'sqa',o31, 'taua',inf(3,1), 'dKg',o33, 'dKa',o33, 'dKga',zeros(15,1),'KA2',o31); 
    %% constant bias & random walk
    if ~exist('web', 'var'), web=0; end
    if ~exist('wdb', 'var'), wdb=0; end
    imuerr.eb(1:3) = eb*glv.dph;   imuerr.web(1:3) = web*glv.dpsh;
    imuerr.db(1:3) = db*glv.ug;    imuerr.wdb(1:3) = wdb*glv.ugpsHz;
    %% correlated bias
    if exist('sqrtR0G', 'var')
        if TauG(1)==inf, imuerr.sqg(1:3) = sqrtR0G*glv.dphpsh;   % algular rate random walk !!!
        elseif TauG(1)>0, imuerr.sqg(1:3) = sqrtR0G*glv.dph.*sqrt(2./TauG); imuerr.taug(1:3) = TauG; % Markov process
        end
    end
    if exist('sqrtR0A', 'var')
        if TauA(1)==inf, imuerr.sqa(1:3) = sqrtR0A*glv.ugpsh;   % specific force random walk !!!
        elseif TauA(1)>0, imuerr.sqa(1:3) = sqrtR0A*glv.ug.*sqrt(2./TauA); imuerr.taua(1:3) = TauA; % Markov process
        end
    end
    %% scale factor error
    if exist('dKGii', 'var')
        imuerr.dKg = setdiag(imuerr.dKg, dKGii*glv.ppm);
    end
    if exist('dKAii', 'var')
        imuerr.dKa = setdiag(imuerr.dKa, dKAii*glv.ppm);
    end
    %% installation angle error
    if exist('dKGij', 'var')
        dKGij = ones(6,1).*dKGij*glv.sec;
        imuerr.dKg(2,1) = dKGij(1); imuerr.dKg(3,1) = dKGij(2); imuerr.dKg(3,2) = dKGij(3); 
        imuerr.dKg(1,2) = dKGij(4); imuerr.dKg(1,3) = dKGij(5); imuerr.dKg(2,3) = dKGij(6);
    end
    if exist('dKAij', 'var')
        dKAij = ones(3,1).*dKAij*glv.sec;
        imuerr.dKa(2,1) = dKAij(1); imuerr.dKa(3,1) = dKAij(2); imuerr.dKa(3,2) = dKAij(3); 
    end
    imuerr.dKga = [imuerr.dKg(:,1); imuerr.dKg(:,2);   imuerr.dKg(:,3);
                   imuerr.dKa(:,1); imuerr.dKa(2:3,2); imuerr.dKa(3,3)];
    %% acc 2nd scale factor error
    if exist('KA2', 'var')
        imuerr.KA2(1:3) = KA2*glv.ugpg2; 
    end
    %% acc inner-lever-arm error
    if exist('rxyz', 'var')
        if length(rxyz)==1, rxyz(1:6)=rxyz; end
        if length(rxyz)==6, rxyz(7:9)=[0;0;0]; end 
        imuerr.rx = rxyz(1:3)/100; imuerr.ry = rxyz(4:6)/100; imuerr.rz = rxyz(7:9)/100;
    end
    %% time-asynchrony between gyro & acc
    if exist('dtGA', 'var')
        imuerr.dtGA = dtGA/1000; 
    end
               

其中，常见的四个误差参数为eb, db, web, wdb，分别代表陀螺随机常值漂移、加速度计随机常值漂移、陀螺的角随机游走、加速度计的速度随机游走。

误差分析

这里的误差分析主要指陀螺静态漂移（加表零漂用时域平滑图即可imumeanplot），其它误差在标定中介绍。误差分析有时间序列、功率谱、小波分析等方法，但最好的方法还是Allan方差分析法，它比较全面，能给出除逐次启动零偏之外的大部分误差，作图直观。
avar：针对一只陀螺作Allan方差分析；
avars：同时分析多只陀螺的Allan方差。

陀螺漂移稳定性误差分析是对陀螺漂移做的最主要的误差分析

Allan方差分析法，只能分析一次漂移的稳定性；逐次启动，上次和本次漂移之间的常值误差，是分析不出来的。

其代码如下：

function [sigma, tau, Err] = avar(y0, tau0, str, isfig)
% Calculate Allan variance.
%
% Prototype: [sigma, tau, Err] = avar(y0, tau0, str, isfig)
% Inputs: y0 - data (gyro in deg/hur; acc in g)
%         tau0 - sampling interval
%         str - string for ylabel
%         isfig - figure flag
% Outputs: sigma - Allan variance
%          tau - Allan variance correlated time
%          Err - Allan variance error boundary
%
% Example: 
%     y = randn(100000,1) + 0.00001*[1:100000]';
%     [sigma, tau, Err] = avar(y, 0.1, 2);
%
% See also  avarsimu, avarfit, avar2, avars, oavar, meann, sumn.

% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 22/08/2012, 16/12/2019, 30/5/2021
    N = length(y0);
    y = y0; NL = N;
    for k = 1:log2(N)
        sigma(k,1) = sqrt(1/(2*(NL-1))*sum((y(2:NL)-y(1:NL-1)).^2)); % diff&std
        tau(k,1) = 2^(k-1)*tau0;      % correlated time
        Err(k,1) = 1/sqrt(2*(NL-1));  % error boundary
        NL = floor(NL/2);
        if NL<3
            break;
        end
        y = 1/2*(y(1:2:2*NL) + y(2:2:2*NL));  % mean & half data length
    end
    if nargin<4, isfig=1; end
    if nargin<3, str=[]; end
    if isnumeric(str), isfig=str; str=[]; end  % avar(y0, tau0, 1)
    if isfig
        figure('Color','White');
        if isempty(str), str = '\itx \rm/ ( (\circ) / h )'; end
        subplot(211), plot(tau0*(1:N)', y0); grid
        xlabel('\itt \rm/ s'); ylabel(str); title(sprintf('Mean: %.6f', mean(y0)));
        subplot(212), 
        loglog(tau, sigma, '-+', tau, [sigma.*(1+Err),sigma.*(1-Err)], 'r--'); grid
        idx = strfind(str,'/');
        if ~isempty(idx),   str = str(idx(1):end);
        else                str = [];
        end
        str = strcat('\it\sigma_A\rm( \tau ) \rm',str);
        xlabel('\it\tau \rm/ s'); ylabel(str);
    end
	if isfig==2  % re-plot subplot(211)
        my0 = mean(y0(1:fix(length(y0)/100)));
        ang = cumsum(y0-my0)*tau0/3600;
        subplot(211); [ax,h1,h2] = plotyy(tau0*(1:N)', y0, tau0*(1:N)', ang); grid on;
        xlabel('\itt \rm/ s'); 
        ylabel(ax(1), '\it\omega \rm/ ( (\circ) / h )');  ylabel(ax(2), '\Delta\it\theta \rm/ (\circ)');
        title(sprintf('Mean: %.6f ( (\\circ) / h )', my0));
    end

可以使用如下语句进行分析：

avar(A2(:,1:3),0.01,2);

A2(:,1:3)表示imu的前三列，即陀螺输出；0.01表示采样间隔