function?[?x_aposterioriP_aposterioriRotMatrixrollpitchyaw]?=?AttltitudeEKF（?dtz?qrx_aposteriori_kP_aposteriori_k）
%AttltitudeEKF?狀態估計的拓展卡爾曼濾波方法
%輸入：
%???updateVect：指示哪些數據進行了更新
%???dt:更新周期
%???z:測量值
%???q:系統噪聲，r:測量噪聲
%???H_k:測量矩陣
%???x_aposteriori_k：上一時刻的狀態估計
%???P_aposteriori_k：上一時刻估計協方差
%輸出：
%???x_aposteriori：當前時刻的狀態估計
%???P_aposteriori：當前時刻的估計協方差
%???RotMatrix:旋轉矩陣
%???roll、pitch、yaw：歐拉角
?
%???O=[?0wz?-wy;
%???????-wz?0?wx;
%???????wy?-wx?0];
????O=[0x_aposteriori_k（3）-x_aposteriori_k（2）;
??????-x_aposteriori_k（3）0x_aposteriori_k（1）;
??????x_aposteriori_k（2）-x_aposteriori_k（1）0];
??????bCn?=?eye（33）?+O*dt;%旋轉矩陣
%更新先驗狀態矩陣
??x_apriori（1:3）?=?x_aposteriori_k（1:3）?+?x_aposteriori_k（4:6）*dt;%角速度
??x_apriori（4:6）?=?x_aposteriori_k（4:6）;%角加速度
??x_apriori（7:9）?=?bCn*x_aposteriori_k（7:9）;%加速度
??x_apriori（10:12）?=?bCn*x_aposteriori_k（10:12）;%磁場
??%更新狀態轉移矩陣
%???r_a=[?0??-az?ay;????r_m=[?0??-mz?my;
%?????????az??0??-ax;?????????mz??0??-mx;
%????????-ay?ax??0];?????????-my??mx??0];
??r_a?=?[0-x_aposteriori_k（9）x_aposteriori_k（8）;
?????????x_aposteriori_k（9）0-x_aposteriori_k（7）;
?????????-x_aposteriori_k（8）x_aposteriori_k（7）0];
??r_m?=?[0?-x_aposteriori_k（12）x_aposteriori_k（11）;
?????????x_aposteriori_k（12）0-x_aposteriori_k（10）;
?????????-x_aposteriori_k（11）x_aposteriori_k（10）0];
?A_lin?=?[eye（33）eye（33）*dtzeros（33）zeros（33）;
??????????zeros（33）eye（33）zeros（33）zeros（33）;
??????????r_a*dtzeros（33）（eye（33）+O*dt）zeros（33）;
??????????r_m*dtzeros（33）zeros（33）（eye（33）+O*dt）];
??%預測誤差協方差矩陣
??Q=[eye（33）*q（1）zeros（33）zeros（33）zeros（33）;
?????zeros（33）eye（33）*q（2）zeros（33）zeros（33）;
?????zeros（33）zeros（33）eye（33）*q（3）zeros（33）;
?????zeros（33）zeros（33）zeros（33）eye（33）*q（4）];
?P_apriori?=?A_lin*P_aposteriori_k*A_lin‘+A_lin*Q*A_lin‘;
?

?%卡爾曼增益
?R=[eye（33）*r（1）zeros（33）zeros（33）;
?????zeros（33）eye（33）*r（2）zeros（33）;
?????zeros（33）zeros（33）eye（33）*r（3）];
?H_k=[eye（33）zeros（33）zeros（33）zeros（33）;
??????zeros（33）zeros（33）eye（33）zeros（33）;
??????zeros（33）zeros（33）zeros（33）eye（33）];
??K_k=（P_apriori*H_k‘）/（H_k*P_apriori*H_k‘+R）;
??%狀態估計矩陣
??x_aposteriori=x_apriori‘+K_k*（z?-?H_k*x_apriori‘）;
??%估計誤差協方差
?%?P_aposteriori=（eye（1212）-K_k*H_k）*P_apriori;%計算方式簡單但是容易失去正定性，PX4中使用這種方式
??P_aposteriori=（eye（1212）-K_k*H_k）*P_apriori*（eye（1212）-K_k*H_k）‘+K_k*R*K_k‘;
?

???R=eye（33）*r（1）;
???H_k=[eye（33）zeros（33）zeros（33）zeros（33）];
???K_k=（P_apriori*H_k‘）/（H_k*P_apriori*H_k‘+R）;
???x_aposteriori=x_apriori‘+K_k*（z（1:3）?-?H_k*x_apriori‘）;
???P_aposteriori=（eye（1212）-K_k*H_k）*P_apriori*（eye（1212）-K_k*H_k）‘+K_k*R*K_k‘;
?
?
?????R=[eye（33）*r（1）zeros（33）;
????????zeros（33）eye（33）*r（2）];
H_k=[eye（33）zeros（33）zeros（33）zeros（33）;
????zeros（33）zeros（33）eye（33）zeros（33）];
????K_k=（P_apriori*H_k‘）/（H_k*P_apriori*H_k‘+R）;
???x_aposteriori=x_apriori‘+K_k*（z（1:

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件???????4031??2018-10-29?15:31??PX4EKF??MATLAB代碼參考\AttltitudeEKF.m

?????目錄??????????0??2019-01-09?15:28??PX4EKF??MATLAB代碼參考

-----------?---------??----------?-----??----

?????????????????4031????????????????????2