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