資源簡介
捷聯慣導解算程序,有詳細的代碼標注和參考文獻說明,主要參考親永元的《組合導航》的書,提供給大家一起學習。

代碼片段和文件信息
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個文件信息
- 上一篇:手寫鋼筆字體
- 下一篇:RPGViewer 3.0 rpg游戲資源提取工具
評論
共有 條評論