西北工业大学严恭敏老师的惯导程序，非常权威，实用，是研究惯导的最佳参考程序，可以直接运行。
捷联惯性导航仿真程序
作者：西北工业大学 --- 严恭敏
导航坐标系：东-北-天
载体坐标系：右-前-上
姿态角范围：-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, 下载次数: 169)
|
QQ好友和群
QQ空间
腾讯微博
腾讯朋友
收藏
淘帖
顶
踩
