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

感谢严老师的无私奉献 

---

imuadderr——imu数据添加误差

function imu = imuadderr(imu, imuerr, ts)
% SIMU outputs adding errors simulation, denoted as:
%    imu = K*imu + drift error.
%
% Prototype: imu = imuadderr(imu, imuerr, ts)
% Inputs:    imu - raw SIMU data
%         imuerr - SIMU error struture array
%             ts - SIMU sample interval
% Output:    imu - output SIMU data added errors
%
% See also  imuerrset, imuclbt, imudeldrift, imuasyn, imugsensi, avperrset, trjsimu, insupdate.

% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 11/09/2013, 06/03/2014, 21/07/2015, 17/08/2016, 24/02/2017, 24/07/2020

    if nargin<3,  ts = imu(2,7)-imu(1,7);  end——'计算采样间隔'
    
    if isfield(imuerr, 'rx')——'imu误差中包含内杠杆臂'
        wb = imu(:,1:3)/ts;
        dotwf = imudot(imu, 5.0);
        fL = [ (-dotwf(:,2).^2-dotwf(:,3).^2+0)*imuerr.rx(1) + (dotwf(:,1).*dotwf(:,2)-wb(:,3))*imuerr.rx(2) + (dotwf(:,1).*dotwf(:,3)+wb(:,2))*imuerr.rx(3), ... 
               (dotwf(:,1).*dotwf(:,2)+wb(:,3))*imuerr.ry(1) + (-dotwf(:,1).^2-dotwf(:,3).^2+0)*imuerr.ry(2) + (dotwf(:,2).*dotwf(:,3)-wb(:,1))*imuerr.ry(3), ... 
               (dotwf(:,1).*dotwf(:,3)-wb(:,2))*imuerr.rz(1) + (dotwf(:,2).*dotwf(:,3)+wb(:,1))*imuerr.rz(2) + (-dotwf(:,1).^2-dotwf(:,2).^2+0)*imuerr.rz(3)  ]; 
        imu(:,4:6) = imu(:,4:6) + fL*ts;——'内杠杆臂只影响加速度计采样，不影响陀螺采样'
    end
    
    if isfield(imuerr, 'dtGA')——'imu误差中包含陀螺和加速度计采样时刻未对准误差'
        acc = [ [imu(1,4:6),imu(1,7)-ts]; imu(:,4:7); [repmat(imu(end,4:6),10,1),imu(end,7)+(1:10)’*ts] ];——'对加速度计数据添头添尾，避免进行外推'
        acc(:,1:3) = cumsum(acc(:,1:3),1);——'对加速度计三轴数据分别进行累加'
        t = [imu(1,end)-ts; imu(:,end)] + imuerr.dtGA;——'进行时延补偿，与陀螺采样时刻对齐'
        for k=1:3
            acc1 = interp1(acc(:,end), acc(:,k), t, 'linear');——'将加速度计数据内插到陀螺的采样时刻'
            imu(:,3+k) = diff(acc1,1);——'时延补偿后的加速度计每个时刻的采样数据'
        end
    end
    
    [m, n] = size(imu); sts = sqrt(ts);
    drift = [ ts*imuerr.eb(1) + sts*imuerr.web(1)*randn(m,1), ...
              ts*imuerr.eb(2) + sts*imuerr.web(2)*randn(m,1), ...
              ts*imuerr.eb(3) + sts*imuerr.web(3)*randn(m,1), ...
              ts*imuerr.db(1) + sts*imuerr.wdb(1)*randn(m,1), ...
              ts*imuerr.db(2) + sts*imuerr.wdb(2)*randn(m,1), ...
              ts*imuerr.db(3) + sts*imuerr.wdb(3)*randn(m,1) ];——'陀螺和加速度计的常值零偏和随机游走，注意单位转化'

'-----一般情况下仿真的imu误差主要由常值零偏和随机游走构成-----'
          
    if min(abs(imuerr.sqg))>0——'陀螺相关漂移'
        mvg = markov1(imuerr.sqg.*sqrt(imuerr.taug/2), imuerr.taug, ts, m);——'一阶马尔可夫过程'
        drift(:,1:3) = drift(:,1:3) + mvg*ts;
    end
    if min(abs(imuerr.sqa))>0——'加速度计相关漂移'
        mva = markov1(imuerr.sqa.*sqrt(imuerr.taua/2), imuerr.taua, ts, m);——'一阶马尔可夫过程'
        drift(:,4:6) = drift(:,4:6) + mva*ts;
    end
    
    if min(abs(imuerr.KA2))>0——'不懂，还望网友赐教'
        imu(:,4:6) = [ imu(:,4)+imuerr.KA2(1)/ts*imu(:,4).^2, ...
                       imu(:,5)+imuerr.KA2(2)/ts*imu(:,5).^2, ...
                       imu(:,6)+imuerr.KA2(3)/ts*imu(:,6).^2 ];
    end
    
    if isfield(imuerr, 'dKg')——'陀螺和加速度计的标定刻度误差矩阵，应该还有dKa'
        Kg = eye(3)+imuerr.dKg; Ka = eye(3)+imuerr.dKa;——'dKg和dKa的对角线元素为尺度因子误差，加上单位阵形成尺度因子'
        imu(:,1:6) = [imu(:,1:3)*Kg', imu(:,4:6)*Ka'];
    end
    imu(:,1:6) = imu(:,1:6) + drift;