ComplementFilter 互补滤波

互补滤波，应用在INS和磁罗盘融合姿态解算，详细说明

function [qeInt]=ComplementFilter（qAibbMbWibbKiKpTeInt）      %dt update rate of accelerometer and magnetometer T update rate of gyroscope

% Kp=2.0f;   % proportional gain governs rate of convergence to accelerometer/magnetometer
% Ki =0.005f;     % integral gain governs rate of convergence of gyroscope biases
% halfT= 0.5f;      % half the sample period

      % normalise the measurements Aibb：accelerometer Mb: Magnetometer Wibb: gyroscope    
        Aibb=Aibb/norm（Aibb）;
        Mb=Mb/norm（Mb）;        
        % compute reference direction of flux这里用四元数表示的方向余弦矩阵为保持四元数的单位化 则用1减去其余两项。
        T11=1-2*q（3）^2-2*q（4）^2;
        T12=2*（q（2）*q（3）+q（1）*q（4））;
        T13=2*（q（2）*q（4）-q（1）*q（3））;
        T21=2*（q（2）*q（3）-q（1）*q（4））;
        T22=1-2*q（2）^2-2*q（4）^2;
      

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        2086  2015-08-11 14:20  ComplementFilter.m