西北工業(yè)大學嚴恭敏老師的慣導程序，非常權威，實用，是研究慣導的最佳參考程序，可以直接運行。
捷聯(lián)慣性導航仿真程序
作者：西北工業(yè)大學 --- 嚴恭敏
導航坐標系：東-北-天
載體坐標系：右-前-上
姿態(tài)角范圍：-PI/2<=pitch<=PI/2, -PI<roll<=PI, -PI<yaw<=PI
注意程序中方位北偏西為正
詳細資料參考作者論文：《捷聯(lián)慣導算法及車載組合導航系統(tǒng)研究》
或《車載自主定位定向系統(tǒng)研究》
或《捷聯(lián)慣導系統(tǒng)動基座初始對準及其它相關問題研究》
子程序包括：
glvs 全局變量賦初值
a2caw 歐拉角微分方程系數(shù)矩陣，a2cwa是其逆陣
askew 由向量求反對稱陣
a2cnb 姿態(tài)角轉化為姿態(tài)矩陣
a2qnb 姿態(tài)角轉化為姿態(tài)四元數(shù)
rv2q 旋轉矢量轉化為變換四元數(shù)
qconj 四元數(shù)求共軛
qmul 兩四元數(shù)相乘
qmulv 向量通過變換四元數(shù)進行坐標變換
q2cnb 姿態(tài)四元數(shù)轉化為姿態(tài)矩陣
q2att 姿態(tài)四元數(shù)轉化為姿態(tài)角
qaddphi 真實姿態(tài)四元數(shù)加平臺誤差角得計算四元數(shù)
qdelphi 由計算四元數(shù)減平臺誤差角得真實姿態(tài)四元數(shù)
qq2phi 從真實姿態(tài)四元數(shù)和計算四元數(shù)中提取平臺誤差角
q2rv 變換四元數(shù)轉化為旋轉矢量
m2att 姿態(tài)矩陣轉化為姿態(tài)角
m2qnb 姿態(tài)矩陣轉化為姿態(tài)四元數(shù)
earth 計算有關地球參數(shù)函數(shù)
cnscl 圓錐和劃船誤差補償
sins 捷聯(lián)慣導算法
getf 求卡爾曼濾波系統(tǒng)矩陣Ft
kalman 離散卡爾曼濾波
kfdis 連續(xù)卡爾曼濾波狀態(tài)方程離散化
演示程序包括：
test_cone_gen 產生圓錐運動角增量
test_cone_error 圓錐誤差仿真
test_align_kalman kalman濾波初始對準仿真
test_align_compass 羅經法初始對準仿真
test_sins 捷聯(lián)慣導解算仿真
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)下仿真的，運動有軌跡參數(shù)時原理類似。
可適當改變一些參數(shù)，看看其它仿真效果。
0.png (9.65 KB, 下載次數(shù): 120)
下載附件
2018-6-21 06:01 上傳
全部資料51hei下載地址：
naviga090205.zip
(22.99 KB, 下載次數(shù): 193)
2018-6-20 21:47 上傳
點擊文件名下載附件
西北工業(yè)大學嚴恭敏老師的慣導程序，非常實用 下載積分: 黑幣 -5
|
[復制鏈接]
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
