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