|
|
西北工業(yè)大學(xué)嚴(yán)恭敏老師的慣導(dǎo)程序，非常權(quán)威，實(shí)用，是研究慣導(dǎo)的最佳參考程序，可以直接運(yùn)行。
捷聯(lián)慣性導(dǎo)航仿真程序
作者：西北工業(yè)大學(xué) --- 嚴(yán)恭敏
導(dǎo)航坐標(biāo)系：東-北-天
載體坐標(biāo)系：右-前-上
姿態(tài)角范圍：-PI/2<=pitch<=PI/2, -PI<roll<=PI, -PI<yaw<=PI
注意程序中方位北偏西為正
詳細(xì)資料參考作者論文：《捷聯(lián)慣導(dǎo)算法及車載組合導(dǎo)航系統(tǒng)研究》
或《車載自主定位定向系統(tǒng)研究》
或《捷聯(lián)慣導(dǎo)系統(tǒng)動(dòng)基座初始對(duì)準(zhǔn)及其它相關(guān)問題研究》
子程序包括：
glvs 全局變量賦初值
a2caw 歐拉角微分方程系數(shù)矩陣，a2cwa是其逆陣
askew 由向量求反對(duì)稱陣
a2cnb 姿態(tài)角轉(zhuǎn)化為姿態(tài)矩陣
a2qnb 姿態(tài)角轉(zhuǎn)化為姿態(tài)四元數(shù)
rv2q 旋轉(zhuǎn)矢量轉(zhuǎn)化為變換四元數(shù)
qconj 四元數(shù)求共軛
qmul 兩四元數(shù)相乘
qmulv 向量通過變換四元數(shù)進(jìn)行坐標(biāo)變換
q2cnb 姿態(tài)四元數(shù)轉(zhuǎn)化為姿態(tài)矩陣
q2att 姿態(tài)四元數(shù)轉(zhuǎn)化為姿態(tài)角
qaddphi 真實(shí)姿態(tài)四元數(shù)加平臺(tái)誤差角得計(jì)算四元數(shù)
qdelphi 由計(jì)算四元數(shù)減平臺(tái)誤差角得真實(shí)姿態(tài)四元數(shù)
qq2phi 從真實(shí)姿態(tài)四元數(shù)和計(jì)算四元數(shù)中提取平臺(tái)誤差角
q2rv 變換四元數(shù)轉(zhuǎn)化為旋轉(zhuǎn)矢量
m2att 姿態(tài)矩陣轉(zhuǎn)化為姿態(tài)角
m2qnb 姿態(tài)矩陣轉(zhuǎn)化為姿態(tài)四元數(shù)
earth 計(jì)算有關(guān)地球參數(shù)函數(shù)
cnscl 圓錐和劃船誤差補(bǔ)償
sins 捷聯(lián)慣導(dǎo)算法
getf 求卡爾曼濾波系統(tǒng)矩陣Ft
kalman 離散卡爾曼濾波
kfdis 連續(xù)卡爾曼濾波狀態(tài)方程離散化
演示程序包括：
test_cone_gen 產(chǎn)生圓錐運(yùn)動(dòng)角增量
test_cone_error 圓錐誤差仿真
test_align_kalman kalman濾波初始對(duì)準(zhǔn)仿真
test_align_compass 羅經(jīng)法初始對(duì)準(zhǔn)仿真
test_sins 捷聯(lián)慣導(dǎo)解算仿真
test_SINS_GPS SINS/GPS組合卡爾曼濾波演示- function [qnb, vn, pos] = sins(qnb_1, vn_1, pos_1, wm, vm, ts)
- tss = ts*size(wm,2);
- [phim,dvbm] = cnscl(wm,vm);
- [wnie,wnen,rmh,rnh,gn] = earth(pos_1,vn_1);
- wnin = wnie+wnen;
- vn = vn_1 + qmulv(rv2q(-wnin*(1.0/2*tss)),qmulv(qnb_1,dvbm)) ...
- + (gn-cross(wnie+wnin,vn_1))*tss;
- vn1_1 = (vn+vn_1)/2;
- pos = pos_1 + tss*[vn1_1(2)/rmh;vn1_1(1)/(rnh*cos(pos_1(1)));vn1_1(3)];
- qnb = qmul(qnb_1, rv2q(phim - qmulv(qconj(qnb_1),wnin*tss)));
以上演示程序都是在靜態(tài)下仿真的，運(yùn)動(dòng)有軌跡參數(shù)時(shí)原理類似。
可適當(dāng)改變一些參數(shù)，看看其它仿真效果。
全部資料51hei下載地址：
naviga090205.zip
(22.99 KB, 下載次數(shù): 193)
2018-6-20 21:47 上傳
點(diǎn)擊文件名下載附件
西北工業(yè)大學(xué)嚴(yán)恭敏老師的慣導(dǎo)程序，非常實(shí)用
|
評(píng)分
-
查看全部評(píng)分
|
[復(fù)制鏈接]
