严恭敏老师PSINS工具箱解读——imuerrset

感谢严老师的无私奉献

---

imuerrset——imu的误差设置

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)——'加速度计常值零偏，单位：微g'
%     web - angular random walk (deg/sqrt(h))——'角度随机游走，单位：度/根号小时'
%     wdb - velocity random walk (ug/sqrt(Hz))——'速度随机游走，单位：微g/根号赫兹'
%     sqrtR0G - gyro correlated bias (deg/h)——'陀螺相关零偏，单位：度/小时'
%     TauG - gyro correlated time (s)——'陀螺相关时间，单位：秒'
%     sqrtR0A - acc correlated bias (deg/h)——'加速度计相关零偏，单位：度/小时'
%     TauA - acc correlated time (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)——'加速度计二次系数，单位：微g/g平方'
%       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——'全局变量，请参考glvf函数'

    if nargin==1  % for specific defined case 
        switch eb——'只输入一个参数eb，eb应为1或2，采用严老师设置的惯性级imu误差'
            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);——'设置imu误差结构体，对未输入变量设置默认值'

    % constant bias & random walk——'陀螺和加速度计的常值零偏和随机游走'
    if ~exist('web', 'var'), web=0; end——'如果未输入角度随机游走，则设置其为0'
    if ~exist('wdb', 'var'), wdb=0; end——'如果未输入速度随机游走，则设置其为0'
    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;——'转化成国际标准单位，单位转化请参考glvf函数'

'-----一般情况下imu误差只需要设置以上的陀螺常值零偏、加速度计常值零偏、角度随机游走和速度随机游走-----'

    % 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——'如果陀螺相关时间大于0，陀螺零偏建模成马尔可夫过程'，
        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——'如果加速度计相关时间大于0，加速度计零偏建模成马尔可夫过程'
        end——'sqa = sqrt(2*sqrtR0A.^2./TauA)'
    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;——'dtGA>0，加速度计采样滞后陀螺'
    end

glvf函数；陀螺漂移误差的马尔科夫＋白噪建模