imumdl.m

自己编写的鲁棒滤波算法的应用

function [WibbM,FbM]=IMUMdl(Wibb,Kw,Wbas,Wrnd,Fb,Ka,Abas,Arnd)
%********************************************************************
%*********************加速度计和陀螺误差模型*************************
% Kw   = [0,0,0]' -  陀螺刻度因子误差
% Wbas = [0,0,0]' -  陀螺零偏             单位(度/小时)
% Wrnd = [0,0,0]' -  陀螺测量随机噪声     单位(度/小时)^2
% Ka   = [1,1,1]' -  加速度计刻度因子误差 
% Abas = [0,0,0]' -  加速度计零偏         单位(ug)
% Arnd = [0,0,0]' -  加速度计测量随机噪声 单位(ug)^2
%*********************************************************************
[m,ArryLth]=size(Wibb);                                 % 测量数据维数
%**********************************************************************
WibbM = Kw * ones( 1,ArryLth ).*Wibb;                   % 陀螺仪测量值
WibbM = WibbM + Wbas*ones(1,ArryLth)*pi/180/3600;       % 零偏
WibbM = WibbM + Wrnd*(randn(3,ArryLth)-0.5)*pi/180/3600; % 噪声
%WibbM = WibbM + Wrnd*randn(3,ArryLth)*pi/180/3600;
%***********************************************************************
FbM   = Ka * ones(1,ArryLth).*Fb;                       % 加速度测量值
FbM   = FbM + Abas*ones(1,ArryLth)*9.8*1e-6;            % 零偏
FbM   = FbM + Arnd*(randn(3,ArryLth)-0.5)*9.8*1e-6;      % 噪声
%FbM   = FbM + Arnd*randn(3,ArryLth)*9.8*1e-6;
%***********************************************************************