PSINS工具箱函数介绍——imuerrset

MATLAB卡尔曼

于 2024-07-31 10:22:34 发布

阅读量473

点赞数 5

文章标签： matlab 开发语言

本文链接：https://blog.csdn.net/callmeup/article/details/140791029

版权

在这里插入图片描述

关于PSINS工具箱

本文所述的代码需要基于PSINS工具箱，工具箱的讲解：

PSINS初学指导：https://blog.csdn.net/callmeup/article/details/137087932

imuerrset简介

imuerrset是设置IMU误差的函数，用于输入IMU的一些误差参数（如：陀螺仪零偏、加速度计零偏、陀螺仪随机游走等值），经此函数处理后，得到"imuerr"这个结构体。

使用方法

代码使用方法如下：

imuerr = imuerrset(0.03, 100, 0.001, 5);

运行上述代码后，即可得到imuerr这个结构体，其值如下：
在这里插入图片描述
上图中：

eb表示陀螺仪常值漂移
db表示加速度计常值漂移
web表示角度随机游走
wdb表示速度随机游走
其余值一般用不到，可以看到在例程运行后，其它的值也大多为0或无意义（inf）

代码

源代码

function imuerr = imuerrset(eb, db, web, wdb, sqrtR0G, TauG, sqrtR0A, TauA, dKGii, dKAii, dKGij, dKAij, KA2, rxyz, dtGA)

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
    if length(eb)==2, eb=[eb(1);eb(1);eb(2)]; end
    if length(db)==2, db=[db(1);db(1);db(2)]; end
    if length(web)==2, web=[web(1);web(1);web(2)]; end
    if length(wdb)==2, wdb=[wdb(1);wdb(1);wdb(2)]; 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