下载文档原格式
捷联惯性导航系统的解算方法捷联惯性导航系统（Inertial Navigation System，简称INS）是一种利用陀螺仪和加速度计等惯性测量单元测量物体的加速度和角速度，然后通过对这些测量值的积分计算出物体的速度和位置的导航系统。
INS广泛应用于航空航天、无人驾驶车辆和船舶等领域，具有高精度和自主性等特点。
INS的解算方法一般分为初始对准、运动状态估计和航位推算三个主要过程。
初始对准是指在启动导航系统时，通过利用外部辅助传感器（如GPS）或静态校准等方法将惯性传感器的输出与真实姿态和位置进行初次校准。
在初始对准过程中，需要获取传感器的初始偏差和初始姿态，一般采用标定或矩阵运算等方法进行。
运动状态估计是指根据惯性传感器的测量值，使用滤波算法对物体的加速度和角速度进行实时估计。
常用的滤波算法包括卡尔曼滤波、扩展卡尔曼滤波和粒子滤波等。
其中，卡尔曼滤波是一种最优估计算法，通过对观测值和状态进行线性组合，得到对真实状态的最佳估计。
扩展卡尔曼滤波则是基于卡尔曼滤波的非线性扩展，可以应用于非线性INS系统。
粒子滤波是一种利用蒙特卡洛采样技术进行状态估计的方法，适用于非高斯分布的状态估计问题。
航位推算是指根据运动状态估计的结果，对物体的速度和位置进行推算。
INS最基本的航位推算方法是利用加速度值对速度进行积分，然后再对速度进行积分得到位置。
但是，在实际应用中，由于传感器本身存在噪声和漂移等误差，导致航位推算过程会出现积分漂移现象。
为了解决这个问题，通常采用辅助传感器（如GPS）和地图等数据对INS的输出进行校正和修正。
当前，还有一些先进的INS解算方法被提出，如基于深度学习的INS 解算方法。
这些方法利用神经网络等深度学习模型，结合原始传感器数据进行端到端的学习和预测，以实现更高精度的位置和姿态估计。
综上所述，捷联惯性导航系统的解算方法主要包括初始对准、运动状态估计和航位推算三个过程。
其中，运动状态估计过程利用滤波算法对传感器的测量值进行处理，得到物体的加速度和角速度的估计。
车载捷联惯导系统基本原理一、捷联惯导系统基本原理捷联惯导系统基本原理如图2-1所示：图中陀螺和加速度计直接与载体系b固联，用来测量载体的角运动信息和线运动信息。
导航解算的本质是根据初值进行积分的过程，通过求解姿态微分方程完成对姿态和航向角的积分，通过求解比力微分方程完成对速度的积分，通过求解位置微分方程实现对位置的积分。
捷联惯导的姿态矩阵C n 相当于“数学平台”，取代了平台惯导中的实体平台，而ωˆ相当于对数学平台“施矩”的指令角速率。
二、捷联惯导微分方程（一）姿态微分方程在捷联惯导系统中，导航坐标系n 和载体坐标系b 之间的角位置关系通常用姿态矩阵、四元数和欧拉角表示，相应也存在姿态矩阵微分方程、四元数微分方程和欧拉角微分方程三种形式。
姿态矩阵微分方程的表达式为：在欧拉角微分方程式(2.2-7)中，当俯仰角θ趋于90º时，cosθ趋于0，tanθ趋于无穷，方程存在奇异性，所以这种方法不能在全姿态范围内正常工作；姿态矩阵微分方程式(2.2-1)可全姿态工作，但姿态矩阵更新相当于求解包含9个未知量的线性微分方程组，计算量大；四元数微分方程式(2.2-6)同样可以全姿态工作，且更新算法只需求解4个未知量的线性微分方程组，计算量小，算法简单，是较实用的工程算法。
（二）速度微分方程速度微分方程即比力方程，是惯性导航解算的基本关系式：三、捷联惯性导航算法捷联惯导解算的目的是根据惯性器件输出求解载体姿态、速度和位置等导航信息，实际上就是求解三个微分方程的过程，相应存在姿态更新算法、速度更新算法和位置更新算法。
（一）姿态更新算法求解微分方程式(2.2-6)可得四元数姿态更新算法为：在车辆行驶过程中，一般不存在高频大机动环境，并且车载导航系统往往不工作在纯惯性导航方式，而是利用里程仪或零速条件进行组合导航，所以算法误差的影响有限，常用的5ms采样周期和二子样优化算法即可满足要求。
四、捷联惯导误差模型传感器误差、初值误差和算法误差是SINS的主要误差源，其中器件误差和初值误差又是影响导航结果的主要因素。
捷联惯导与组合导航系统高精度初始对准技术研究捷联惯导与组合导航系统高精度初始对准技术研究引言捷联惯导与组合导航系统是一种集捷联惯导和其他导航传感器（如GPS、气压计、陀螺仪等）的优势于一体的导航系统，具有在惯导滞后情况下实现导航信息快速、准确更新的优势。
为了确保导航精度和可靠性，捷联惯导与组合导航系统的初始对准是不可或缺的关键技术之一。
本文将重点探讨捷联惯导与组合导航系统高精度初始对准技术的研究。
一、捷联惯导与组合导航系统概述捷联惯导与组合导航系统是一种通过融合多种导航传感器测量数据来计算导航解的导航系统。
其中，捷联惯导通过惯性导航算法利用加速度计和陀螺仪提供的姿态、速度和位移信息进行导航计算，而组合导航则通过融合GPS和其它传感器的信息来修正惯导的误差，提供更准确的导航结果。
二、初始对准技术的研究现状初始对准技术在捷联惯导与组合导航系统中起到了决定性的作用，对其精度和可靠性具有重大影响。
目前，针对初始对准技术的研究主要集中在以下几个方面：1. 惯性传感器标定：惯导系统的精度和准确性直接依赖于惯性传感器的性能。
因此，对于惯导系统而言，惯性传感器的标定至关重要。
传感器标定主要涉及惯性传感器的误差估计、参数校准和标定方法等。
2. 导航状态估计算法：捷联惯导与组合导航系统的核心是导航状态估计算法。
目前常用的算法包括扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）以及粒子滤波（PF）等。
这些算法通过融合多种传感器的信息，实现对导航状态的准确估计。
3. 高精度传感器融合：为了提高初始对准的精度和可靠性，可以考虑使用更高精度的传感器，如高精度的加速度计和陀螺仪。
此外，对于GPS系统而言，使用双频技术和高精度的差分GPS技术可以进一步提高导航精度。
三、捷联惯导与组合导航系统高精度初始对准技术研究在捷联惯导与组合导航系统高精度初始对准技术的研究中，可以采用以下方法来提高初始对准的精度和可靠性：1. 多目标标定方法：采用多目标标定方法来标定捷联惯导系统中的惯性传感器。
第１５卷第ｌ期２００７年２月中国惯性技术学报ＪｏｕｍａｌｏｆＣｈｉｎｅｓｅＩｎｅｒｔｉａｌＴｃｃｈｎｏｌｏｇｙＶｂｌ．１５Ｎｏ．１Ｆｅｂ．２００７文章编号：１００５－６７３４（２００７）０１一００２４－０４车载捷联惯导系统定位测姿算法研究陈允芳１，叶泽田２，钟若飞３（１．山东科技大学地球信息科学与工程学院，青岛２６６５１０；２．中国测绘科学研究院，北京１０００３９；３．首都师范大学，北京１０００３７）摘要：ＧＰｓ／ＩＮｓ组合精确测定平台的位置和姿态是移动测图系统中的重要模块。
对陀螺仪和加速度计所测角速度和比力进行两次积分得载体姿态、速度和位置即ｓＩＮｓ力学机械编排。
目前该过程大多在地理坐标系进行。
这里详细推导了地球坐标系中完整的解算过程，以四元数姿态矩阵更新及重力计算为核心，由ＩＭｕ原始观测值解算出了载体位置、速度和姿态等参数，可快速高效与ＧＰｓ输出的位置速度信息进行组合滤波处理，可据此编程进行工程应用数据处理。
关键词：捷联惯导系统；姿态矩阵；坐标转换；力学编排；四元数中图分类号：ｕ６６６．１文献标识码：ＡＰｏｓｉｔｉｏｎｉｎｇａｎｄｏｒｉｅｎｔａｔｉｏｎｃｏｍｐｕｔａｔｉｏｎｏｎＶｅｈｉｃｌｅ－ｂｏｒｎｅＳＩＮＳａｎｄｄｉｓｃｕｓｓｏｆｃａｌｃｕｌａｔｉｏｎｅｒｒｏｒｃＨＥＮＹｕｎ．‰９１，ＹＥｚｅ－ｔｉａｎ２，ｚＨＯＮＧＲｕｏ．ｆｅｉ３（１．Ｇｅｏ·ｉｎｆｏ衄ａｔｉｏｎＳｃｉｅｎｃｅ＆ＥｎｇｉｎｅｅｒｉｎｇＣｏｌｌｅｇｅ，ＳｈａｎｄｏｎｇＵｎｉｖｅｒＳｉ哆ｏｆＳｃｉｅｎｃｅａＩｌｄＴｂｃｈｎｏｌｏｇｙ，Ｑｉｎｇｄａｏ２６６５１０，Ｃｈｉｎａ；２．ＳｕｒｖｅｙｉｎｇａＴｌｄＭａｐｐｉｎｇＳｃｉｅｎｃｅＲｅｓｅａｒｃｈＩｎＳｔｉｔｕｔｅｏｆＣｈｉｎａ，Ｂｅｉｊｉｎｇ１０００３９，Ｃｈｉｎａ；３．Ｃ印ｉｔａｌＮｏｍｌａｌＵｎｉｖｅｒＳｉ劬Ｂｅｉｊｉｎｇ１０００３７，Ｃｈｉｎａ）Ａｂｓｔｒａｃｔ：ＧＰＳａｎｄＩＮＳｉｎｔｅｇｒａｔｅｄｔｏａｃｃｕｒａｔｅｌｙｄｅｔｅｍｉｎｉｎｇｐｏｓｉｔｉｏｎａＩｌｄａｔｔｉｔｕｄｅｏｆｎａｔＩ‘ｏｏｆｉｓＶｉｔａｌｍｏｄｕｌｅｉｎｍｏｂｉｌｅｍａｐｐｉｎｇＳｙｓｔｅｍ．Ｓｐｅｃｉｎｃｆｏｒｃｃ行ｏｍｓｐｅｅｄｏｍｅｔｅｒ蚰ｄ舭ｇｌｅｒａｔｅ矗ｏｍ留ｒｏａｒｅｉｎｔｅ铲ａｔｅｄｔｗｉｃｅｒｅｓｐｅｃｔｉＶｅｌｙｔｏａｃｈｉｅｖｅａｎ沁ｄｅ，ｖｅｌｏｃ时ａＩｌｄｐｏｓｉｔｉｏｎｎ锄ｅｌｙＳＩＮＳｍｅｃｈａＩｌｉｚａｔｉｏｎ．Ｃｕｒｒｅｎｔｌｙｔｈｉｓｔｏｏｋｐｌａｃｅｄｉｎｇｅｏｇｒ印ｈｉｃｃｏｏｒｄｉｎａｔｅ，ｗｈｉＩｅｈｅｒｅｄｅｍｏｎｓｔｒａｔｅｄｉｎｄｅｔａｉｌｍｅｗｈｏｌｅｍｅｃｈａＪｌｉｚａｔｉｏｎｉｎｅａｎｌｌ－ｃｅｎｔｃｌｒｃｄｅａｒｔｈ－ｆｉｘｅｄｃｏｏｒｄｉｎａｔｅ，ｍｏｓｔｌｙｑｕａｔｅｍｉｏｎａ钍ｉｔｕｄｅｍａｔｒｉｘｕｐｄａｔｉｎｇ锄ｄｇｒａｖｉｔ）ｒｃａＩｃｕｌａｔｉｏｎ．Ｕｌｔｉｍａｔｅｌｙｖｅｈｉｃｌｅｎａｖｉｇａｔｉｏｎｐａｒ锄ｅｔｅｒｓｓｕｃｈａＳａｔｔｉｔｕｄｅ，ｖｅＩｏｃｉｔｙ锄ｄｐｏｓｉｔｉｏｎｗｅｒｃｇａｈｅｄ丘ｏｍＩＭＵｏｒｉｇｉｎ“０ｂｓｅｒｖａｔｉｏｎｓ．Ｍａｔｈｅｍａｔｉｃｓｐｌａｔｆｏ眦ｉｓｆｏｍｌｃｄｉｎＳｒＮＳｔｏｃａｒｒｙｏｕｔｓｕⅣｅｙｉｎｇａＪｌｄｃａｌｃｕｌａｔｉｎｇｐｒｅｃｉｓｅｌｙｔｈｅｎａｖｉｇａｔｉｏｎｍｏＶｅｍｅｎｔｐａｒ锄ｃｔｅｒＳ．Ｔｈｅｒｅｓｕｌｔｓａｒｃｐｒｏｎｅｔｏｉｎｔｅ黟ａｔｅｗｉｔｌｌｓｉｍｉｌａｒｐａｍｍｅｔｅｒｓ疔ｏｍＧＰＳｔｏｆｉｌｔｅｒｐｒｏｃｅｓｓｉｎｇ．Ｐｒｏ可锄ｍｉｎｇｈｅｒｃｂｙｃ锄ｐｍｃｅｓｓｄａｔａｉｎｅｎｇｉｎｅｅｒｉｎｇａｐｐｌｉｃａｔｉｏｎＫｅｙｗｏｒｄｓ：ＳＩＮＳ；ａｔｔｉｔｕｄｅｍａｔｒｉｘ；ｃｏｏｒｄｉｎａｔｅｔｒａｎｓｆｏｍａｔｉｏｎ；ｍｅｃｈａｎｉｚａｔｉｏｎ；ｑｕａｔｅｍｉｏｎ随着惯性技术与卫星导航定位技术的发展，由ＧＰＳ／ＩＮｓ不同程度组合而成的定位定姿传感器已成为移动测图系统中确定载体轨迹和平台姿态的重要工具，其中ＧＰｓ多用于定位而ＩＮＳ则用于测姿。
捷联惯导系统极区导航算法优化设计及误差特性分析张海峰;张礼伟;王兴岭;李琳;仲岩【摘要】采用格网坐标系下的力学编排方案能够有效解决常规惯导系统力学编排方案在极区航向误差急剧发散且无法实现定位定向的难题.格网坐标系力学编排方案可以直接获得格网航向,以及地心地固坐标系下的位置坐标,且输出航向精度及定位精度不随纬度的增高而变差.通过深入研究格网坐标系力学编排方案的误差传播规律,详细分析了高纬度下格网航向保持高精度输出的数学机理.针对格网坐标系力学编排方案在极点附近存在计算奇异值的问题,提出了一种通过格网坐标系和地球坐标系间的位置方向余弦矩阵更新解算替代由地心地固位置坐标求解经纬度三角函数值的优化算法,实现了真正意义上的格网坐标系力学编排方案在极区的“无死角”导航能力.仿真分析了载体沿经线穿越极点运动时的算法性能,并与固定指北力学编排方案进行了比较,结果表明,相比于传统导航方案,格网系下输出的航向误差不随纬度升高而发散,导航精度与低纬度区域导航能力相当.【期刊名称】《中国惯性技术学报》【年(卷),期】2015(023)006【总页数】6页(P701-706)【关键词】极区导航;格网坐标系;误差特性;算法优化【作者】张海峰;张礼伟;王兴岭;李琳;仲岩【作者单位】天津航海仪器研究所,天津300131;天津航海仪器研究所,天津300131;天津航海仪器研究所,天津300131;天津航海仪器研究所,天津300131;天津航海仪器研究所,天津300131【正文语种】中文【中图分类】U666.1随着航空、航海事业的蓬勃发展以及国际政治经济的不断变化，对海军作战舰艇的全球作战能力提出了更高的要求。
由于极区地理经线快速收敛，导致传统地理导航坐标系失效。
虽然自由和游移方位惯导可在极区完成姿态方向余弦矩阵和位置方向余弦矩阵的解算，但从矩阵中提取航向信息和经度信息时存在奇异值。
采用格网线取代传统的地理经纬线对地表重新划分是解决该问题的有效手段之一。
捷联惯导算法与组合导航原理讲义严恭敏，翁浚编著西北工业大学2016-9前言近年来，惯性技术不论在军事上、工业上，还是在民用上，特别是消费电子产品领域,都获得了广泛的应用,大到潜艇、舰船、高铁、客机、导弹和人造卫星，小到医疗器械、电动独轮车、小型四旋翼无人机、空中鼠标和手机，都有惯性技术存在甚至大显身手的身影。
相应地，惯性技术的研究和开发也获得前所未有的蓬勃发展，越来越多的高校学生、爱好者和工程技术人员加入到惯性技术的研发队伍中来。
惯性技术涉及面广，涵盖元器件技术、测试设备和测试方法、系统集成技术和应用开发技术等方面，囿于篇幅和作者知识面限制，本书主要讨论捷联惯导系统算法方面的有关问题，包括姿态算法基本理论、捷联惯导更新算法与误差分析、组合导航卡尔曼滤波原理、捷联惯导系统的初始对准技术、组合导航系统建模以及算法仿真等内容。
希望读者参阅之后能够对捷联惯导算法有个系统而深入的理解，并能快速而有效地将基本算法应用于解决实际问题。
本书在编写和定稿过程中得到以下同行的热心支持，指出了不少错误之处或提出了许多宝贵的修改建议,深表谢意:西北工业大学自动化学院：梅春波、赵彦明、刘洋、沈彦超、肖迅、牟夏、郑江涛、刘士明、金竹、冯理成、赵雪华；航天科工第九总体设计部：王亚军；辽宁工程技术大学：丁伟；北京腾盛科技有限公司：刘兴华；东南大学：童金武；中国农业大学:包建华；南京航空航天大学：赵宣懿；武汉大学：董翠军;网友：Zoro；山东科技大学:王云鹏。
书中缺点和错误在所难免，望读者不吝批评指正.作者2016年9月目录第1章概述 (6)1.1捷联惯导算法简介 (6)1.2 Kalman滤波与组合导航原理简介 (7)第2章捷联惯导姿态解算基础 (10)2。
1反对称阵及其矩阵指数函数 (10)2。
1。
1 反对称阵 (10)2。
1.2 反对称阵的矩阵指数函数 (12)2。
2方向余弦阵与等效旋转矢量 (13)2.2.1 方向余弦阵 (13)2。
捷联惯导算法心得1、四个概念：“地理”坐标系、“机体”坐标系、他们之间换算公式、换算公式用的系数。
地理坐标系：东、北、天，以下简称地理。
在这个坐标系里有重力永远是（0,0,1g），地磁永远是（0,1,x）（地磁的垂直不关心）两个三维向量。
机体坐标系：以下简称机体，上面有陀螺、加计、电子罗盘传感器，三个三维向量。
换算公式：以下简称公式，公式就是描述机体姿态的表达方法，一般都是用以地理为基准，从地理换算到机体的公式，有四元数、欧拉角、方向余弦矩阵。
换算公式的系数：以下简称系数，四元数的q0123、欧拉角的ROLL/PITCH/YAW、余弦矩阵的9个数。
系数就是描述机体姿态的表达方法的具体数值。
姿态，其实就是公式+系数的组合，一般经常用人容易理解的公式“欧拉角”表示，系数就是横滚xx度俯仰xx度航向xx度。
2、五个数据源：重力、地磁、陀螺、加计、电子罗盘，前两个来自地理，后三个来自机体。
3、陀螺向量：基于机体，也在机体上积分，因为地理上无参考数据源，所以很独立，直接在公式的老系数上积分，得到新系数。
狭义上的捷联惯导算法，就是指这个陀螺积分公式，也分为欧拉角、方向余弦矩阵、四元数，他们的积分算法有增量法、数值积分法（X阶龙格-库塔）等等4、加计向量、重力向量：加计基于机体，重力基于地理，重力向量（0,0,1g）用公式换算到机体，与机体的加计向量算出误差。
理论上应该没有误差，这误差逆向思维一下，其实就是换算公式的系数误差。
所以这误差可用于纠正公式的系数（横滚、俯仰），也就是姿态。
5、电子罗盘向量、地磁向量：同上，只不过要砍掉地理上的垂直向量，因为无用。
只留下地理水平面上的向量。
误差可以用来纠正公式的系数（航向）。
6、就这样，系数不停地被陀螺积分更新，也不停地被误差修正，它和公式所代表的姿态也在不断更新。
如果积分和修正用四元数算法（因为运算量较少、无奇点误差），最后用欧拉角输出控制PID（因为角度比较直观），那就需要有个四元数系数到欧拉角系数的转换。
常用的三种公式，它们之间都有转换算法。
再搞个直白一点的例子：机体好似一条船，地理就是那地图，姿态就是航向（船头在地图上的方位），重力和地磁是地图上的灯塔，陀螺/积分公式是舵手，加计和电子罗盘是瞭望手。
舵手负责估计和把稳航向，他相信自己，本来船向北开的，就一定会一直往北开，觉得转了90度弯，那就会往东开。
当然如果舵手很牛逼，也许能估计很准确，维持很长时间。
不过只信任舵手，肯定会迷路，所以一般都有地图和瞭望手来观察误差。
瞭望手根据地图灯塔方位和船的当前航向，算出灯塔理论上应该在船的X方位。
然而看到实际灯塔在船的Y方位，那肯定船的当前航向有偏差了，偏差就是ERR=X-Y。
舵手收到瞭望手给的ERR报告，觉得可靠，那就听个90%*ERR，觉得天气不好、地图误差大，那就听个10%*ERR，根据这个来纠正估算航向。
------------------------------------------------------来点干货，注意以下的欧拉角都是这样的顺序：先航向-再俯仰-然后横滚公式截图来自：袁信、郑锷的《捷联式惯性导航原理》，邓正隆的《惯性技术》。
--------------------------------------------------根据加计计算初始欧拉角这个无论欧拉角算法还是四元数算法还是方向余弦矩阵都需要，因为加计和电子罗盘给出欧拉角的描述方式比较方便。
imu.euler.x=atan2(imu.accel.y,imu.accel.z);imu.euler.y=-asin(imu.accel.x/ACCEL_1G);ACCEL_1G为9.81米/秒^2，accel.xyz的都为这个单位，算出来的euler.xyz单位是弧度航向imu.euler.z可以用电子罗盘计算--------------------------------------------------欧拉角微分方程如果用欧拉角算法，那么这个公式就够了，不需要来回转换。
矩阵上到下三个角度（希腊字母）是roll pitch和yaw，公式最左边的上面带点的三个是本次更新后的角度，不带点的是上个更新周期算出来的角度。
Wx,y,z是roll pitch和yaw方向的三个陀螺在这个周期转动过的角度，单位为弧度，计算为间隔时间T*陀螺角速度,比如0.02秒*0.01弧度/秒=0.0002弧度. --------------------------------------------------以下是四元数--------------------------------------------------四元数初始化q0-3为四元数四个值,用最上面公式根据加计计算出来的欧拉角来初始化--------------------------------------------------四元数微分方程四元数更新算法，一阶龙库法，同样4个量（入、P1-3）也为四元数的四个值,即上面的q0-3。
Wx,y,z是三个陀螺的这个周期的角速度，比如欧拉角微分方程中的0.01弧度/秒，T为更新周期,比如上面的0.02秒。
再来一张，另外一本书上的，仔细看和上面是一样的delta角度，就是上面的角速度*周期，单位为弧度--------------------------------------------------四元数微分方程更新后的规范化每个周期更新完四元数，需要对四元数做规范化处理。
因为四元数本来就定义为四维单位向量。
求q0-3的平方和，再开根号算出的向量长度length。
然后每个q0-3除这个length。
--------------------------------------------------四元数转欧拉角公式把四元数转成了方向余弦矩阵中的几个元素，再用这几个元素转成了欧拉角先从四元数q0-3转成方向余弦矩阵：再从方向余弦矩阵转成欧拉角代码：//更新方向余弦矩阵t11=q.q0*q.q0+q.q1*q.q1-q.q2*q.q2-q.q3*q.q3;t12=2.0*(q.q1*q.q2+q.q0*q.q3);t13=2.0*(q.q1*q.q3-q.q0*q.q2);t21=2.0*(q.q1*q.q2-q.q0*q.q3);t22=q.q0*q.q0-q.q1*q.q1+q.q2*q.q2-q.q3*q.q3;t23=2.0*(q.q2*q.q3+q.q0*q.q1);t31=2.0*(q.q1*q.q3+q.q0*q.q2);t32=2.0*(q.q2*q.q3-q.q0*q.q1);t33=q.q0*q.q0-q.q1*q.q1-q.q2*q.q2+q.q3*q.q3;//求出欧拉角imu.euler.roll=atan2(t23,t33);imu.euler.pitch=-asin(t13);imu.euler.yaw=atan2(t12,t11);if(imu.euler.yaw<0){imu.euler.yaw+=ToRad(360);}----------------------------------------------------以下代码摘自网上，很巧妙，附上注释，有四元数微分，有加计耦合。
没电子罗盘，其实耦合原理也一样。
view plaincopy to clipboardprint?1.//================================================================================================= ====2.//IMU.c3.//S.O.H.Madgwick4.//25th September20105.//================================================================================================= ====6.//Description:7.//8.//Quaternion implementation of the'DCM filter'[Mayhony et al].9.//10.//User must define'halfT'as the(sample period/2),and the filter gains'Kp'and'Ki'.11.//12.//Global variables'q0','q1','q2','q3'are the quaternion elements representing the estimated13.//orientation.See my report for an overview of the use of quaternions in this application.14.//15.//User must call'IMUupdate()'every sample period and parse calibrated gyroscope('gx','gy','gz')16.//and accelerometer('ax','ay','ay')data.Gyroscope units are radians/second,accelerometer17.//units are irrelevant as the vector is normalised.18.//19.//================================================================================================= ====20.21.//----------------------------------------------------------------------------------------------------22.//Header files23.24.#include"IMU.h"25.#include<math.h>26.27.//----------------------------------------------------------------------------------------------------28.//Definitions29.30.#define Kp2.0f//proportional gain governs rate of convergenceto accelerometer/magnetometer31.#define Ki0.005f//integral gain governs rate of convergence of gyroscope biases32.#define halfT0.5f//half the sample period33.34.//---------------------------------------------------------------------------------------------------35.//Variable definitions36.37.float q0=1,q1=0,q2=0,q3=0;//quaternion elements representing the estimated orientation38.float exInt=0,eyInt=0,ezInt=0;//scaled integral error39.40.//================================================================================================= ===41.//Function42.//================================================================================================= ===43.44.void IMUupdate(float gx,float gy,float gz,float ax,float ay,float az){45.float norm;46.float vx,vy,vz;47.float ex,ey,ez;48.49.//normalise the measurements50.norm=sqrt(ax*ax+ay*ay+az*az);51.ax=ax/norm;52.ay=ay/norm;53.az=az/norm;54.把加计的三维向量转成单位向量。
