捷联惯导算法

导航坐标系：东-北-天 载体坐标系：右-前-上 姿态角范围：-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组合卡尔曼滤波演示 以上演示程序都是在静态下仿真的，运动有轨迹参数时原理类似。 可适当改变一些参数，看看其它仿真效果。