function??[attqbn]?=?align_kalman（timeimugyracclenpos）
%kalman濾波方法初始對準
global?glv
%[RmRng]=earth（pos（1））;
[RmRng]=earth（pos）;
%?gn=[0;0;-g];
%att?=?coarse_align（acc?gyr?pos（1）?gn）;?%?粗對準
att?=?Allign_coarse（accgyrglv.time_coarse*glv.tgpos（1））;?????%?粗對準?????????????pos（1）
qbn0?=?att2quat（att）?;????????????????????????????%?姿態轉四元數，b系到n系
wnie?=?glv.wie*[0;?cos（pos（1））;?sin（pos（1））];?????????????????????????????????????%pos（1）2
%?濾波器參數參見《捷聯慣導系統靜基座初始對準精度分析及仿真》
Qt?=?diag（[glv.eb;?0;0]）^2;?????????????%?系統噪聲方差矩陣?（陀螺漂移）陀螺儀零偏
Rk?=?diag（[glv.db（1）;glv.db（2）]）^2;????????%?觀測噪聲方差矩陣（加速度計漂移）加速度計零偏
xk?=?zeros（51）;?????????????????????%?狀態向量：姿態角誤差、北向陀螺漂移、天向陀螺漂移
Pk?=?diag（[[1;1;5]*glv.deg;?[0.5;0.5]*glv.dph]）^2;?????%協方差矩陣deg角度dph度每小時
Phi?=?[?0?wnie（3）?-wnie（2）???0?0???????????????%連續狀態方程的狀態轉移矩陣
??????-wnie（3）?0?0????-1?0?
??????wnie（2）?0?0?????0?-1?
??????zeros（25）?];?????????????????
Hk?=?[?0?g??0?0?0
??????-g?0??0?0?0?];?????????????%?觀測矩陣
??qbn=qbn0‘;
for?i?=?（glv.time_coarse*glv.t）:len-1
????Ts?=?timeimu（i+1）?-?timeimu（i）;
????Phikk_1?=?eye（5）+Phi*Ts;????????????%?狀態轉移矩陣離散化
????wbe?=?gyr（i:）‘;
????fbe?=?acc（i:）‘;
????qbn?=?qmul（qbn?rv2q（（wbe-qmulv（qconj（qbn）wnie））*Ts?））;??%?姿態更新（循環）單子樣，???????????????qmul四元數相乘，qconj共軛四元數
????fn?=?qmulv（qbn?fbe）;?????????????%比力轉化為導航坐標系?????????????????????????????????????????qumlv四元數乘向量，rv2q等效旋轉矢量轉換為四元數，q2rv四元數轉換為等效矩陣
????zk?=?fn（1:2）;??%?以水平比力為觀測量?
????[xk?Pk]?=?kalman（Phikk_1?Qt*Ts?xk?Pk?Hk?Rk?zk）;???%?開環卡爾曼濾波
end
bn?=?qdelphi（qbnxk（1:3））;????????????????????????%姿態更新四元數微分方程
att?=?q2att（qbn）;??????????????????????????????????%四元數轉換為姿態角

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

?????文件???????1882??2014-12-03?19:57??嚴恭敏捷聯慣導程序\align_kalman.asv

?????文件???????1909??2014-12-04?18:42??嚴恭敏捷聯慣導程序\align_kalman.m

?????文件????????538??2014-04-18?09:46??嚴恭敏捷聯慣導程序\Allign_coarse.asv

?????文件????????536??2014-04-18?09:50??嚴恭敏捷聯慣導程序\Allign_coarse.m

?????文件????????428??2013-10-10?03:13??嚴恭敏捷聯慣導程序\att2quat.asv

?????文件????????408??2012-07-30?17:30??嚴恭敏捷聯慣導程序\att2quat.m

?????文件????????154??2012-07-30?17:21??嚴恭敏捷聯慣導程序\Cnb2atti.m

?????文件???????1182??2014-04-18?16:22??嚴恭敏捷聯慣導程序\cnscl.asv

?????文件???????1196??2014-04-18?16:24??嚴恭敏捷聯慣導程序\cnscl.m

?????文件????????394??2014-04-09?23:02??嚴恭敏捷聯慣導程序\earth.asv

?????文件????????395??2014-12-03?19:56??嚴恭敏捷聯慣導程序\earth.m

?????文件??????????0??2013-10-21?23:19??嚴恭敏捷聯慣導程序\glv.eb

?????文件???????1575??2014-04-18?23:42??嚴恭敏捷聯慣導程序\glvs.asv

?????文件???????1575??2014-04-18?23:43??嚴恭敏捷聯慣導程序\glvs.m

?????文件?????605701??2014-04-18?10:13??嚴恭敏捷聯慣導程序\imu.mat

?????文件????????604??2014-04-15?14:13??嚴恭敏捷聯慣導程序\kalman.asv

?????文件????????604??2014-04-15?14:17??嚴恭敏捷聯慣導程序\kalman.m

?????文件????????403??2013-10-10?03:17??嚴恭敏捷聯慣導程序\normQ.asv

?????文件????????393??2012-03-27?20:56??嚴恭敏捷聯慣導程序\normQ.m

?????文件????????248??2012-08-01?16:26??嚴恭敏捷聯慣導程序\position.m

?????文件????1695012??2015-03-03?21:28??嚴恭敏捷聯慣導程序\pp；.mat

?????文件????????100??2013-09-12?17:33??嚴恭敏捷聯慣導程序\q2att.m

?????文件????????455??2012-07-30?10:43??嚴恭敏捷聯慣導程序\q2cbn.m

?????文件?????????75??2014-04-15?09:53??嚴恭敏捷聯慣導程序\qconj.asv

?????文件?????????75??2014-04-15?09:56??嚴恭敏捷聯慣導程序\qconj.m

?????文件?????????65??2012-07-27?16:50??嚴恭敏捷聯慣導程序\qdelphi.m

?????文件????????342??2013-11-02?20:34??嚴恭敏捷聯慣導程序\qmul.asv

?????文件????????342??2013-11-02?20:39??嚴恭敏捷聯慣導程序\qmul.m

?????文件????????145??2014-04-15?14:33??嚴恭敏捷聯慣導程序\qmulv.asv

?????文件????????147??2014-04-15?14:33??嚴恭敏捷聯慣導程序\qmulv.m

............此處省略17個文件信息