北航惯性导航综合实验五实验报告
工业惯性实验报告范文
工业惯性实验报告范文实验名称：工业惯性实验实验目的：通过实验观察惯性原理在工业领域中的应用，探讨工业惯性对物体运动轨迹的影响。
实验器材：平滑的水平桌面、小球、金属板、带有标尺的直尺、定时器、摄像设备等。
实验原理：根据牛顿第一定律，惯性是物体自身保持静止或匀速直线运动的性质。
当外力作用于物体时，物体会发生加速度。
工业惯性利用了物体的惯性性质，通过对物体运动的观察和控制，实现工业生产的高效率和稳定性。
实验步骤：1. 在平滑的水平桌面上放置金属板。
2. 将小球放在金属板上，并通过定时器记录下实验开始时的时间。
3. 快速推动金属板，使小球开始运动。
4. 利用直尺上的标尺，测量小球在不同时间内的位置。
5. 通过摄像设备记录小球的运动轨迹，并将视频转换为图像。
6. 分析图像，计算小球的速度和加速度。
实验数据:时间（s）位置（cm）0 01 22 53 94 145 20实验结果分析：根据实验数据计算得出小球的速度和加速度如下：时间（s）速度（cm/s）加速度（cm/s^2）0 0 01 2 22 3 13 4 14 5 15 6 1通过数据分析可以发现，小球的速度和加速度随时间逐渐增加，符合牛顿第二定律的描述。
这表明在实验中，小球的运动受到外力的作用，产生了加速度。
结论：工业惯性利用了物体的惯性性质，通过对物体运动的观察和控制，实现了工业生产中的高效率和稳定性。
实验结果表明，在工业惯性实验中，物体的速度和加速度随时间逐渐增加，符合牛顿第二定律的描述。
实验总结：通过这次实验，我深刻理解了惯性原理在工业领域中的应用，对工业生产的高效率和稳定性产生了更深的认识。
同时，我也提高了实验操作和数据处理的能力，对科学实验的重要性有了更加深刻的体验。
惯性质量的测量实验报告
惯性质量的测量实验报告惯性质量的测量实验报告引言惯性质量是物体所具有的抗拒外力改变其运动状态的性质。
在物理学中，测量物体的质量是一个基本实验。
然而，传统的质量测量方法常常受到外界因素的干扰，导致结果的不准确。
为了解决这个问题，本实验设计了一种新的方法，旨在准确测量物体的惯性质量。
实验设备和步骤本实验所使用的设备包括一个特制的测量平台、一台高精度的电子天平和一根细线。
首先，将测量平台放置在水平台面上，并确保其稳定性。
然后，将待测物体放置在测量平台上，并用细线将其固定。
接下来，使用电子天平测量物体的质量，并记录下结果。
实验原理本实验的关键在于利用物体的惯性质量来测量其真实质量。
当物体受到外力作用时，由于其惯性，物体会产生相应的加速度。
根据牛顿第二定律，物体的加速度与作用力成正比，与物体的质量成反比。
因此，通过测量物体在给定外力下的加速度，可以推导出其真实质量。
实验结果经过多次实验测量，我们得到了一系列的数据。
通过对这些数据进行处理和分析，我们得到了物体的惯性质量。
实验结果表明，这种新的测量方法相比传统方法更为准确和可靠。
实验误差分析在实验过程中，我们发现了一些可能导致误差的因素。
首先，测量平台的稳定性对实验结果有很大影响。
如果平台不稳定，物体可能会受到额外的力，导致测量结果不准确。
其次，电子天平的精度也会影响实验结果的准确性。
如果天平的精度不够高，测量结果可能存在一定的误差。
最后，细线的弹性也可能对实验结果产生影响。
当物体受到外力时，细线可能会有一定的伸缩，导致测量结果偏离真实值。
实验改进方案为了提高实验结果的准确性，我们可以采取一些改进措施。
首先，可以使用更稳定的测量平台，确保物体在测量过程中不受到额外的力。
其次，可以使用更高精度的电子天平，提高测量结果的准确性。
最后，可以使用更细的细线，减小其弹性对实验结果的影响。
实验应用惯性质量的测量在科学研究和工程应用中具有重要意义。
准确测量物体的质量是许多实验和工程项目的基础。
初中惯性实验报告
初中惯性实验报告 探究物体在惯性状态下的特点，了解惯性的概念。
实验器材： 1. 滑膜小车 2. 磁性导轨 3. 轨道 4. 弹簧测力计 5. 计时器 6. 质量小块
实验原理： 在理想情况下，物体在没有受到力的作用下，会保持静止或匀速直线运动的状态，这种性质被称为惯性。惯性是物体保持原有运动状态的性质。
实验步骤： 1. 确保磁性导轨平整，滑膜小车表面干净，无杂物。 2. 将滑膜小车放在导轨上，确保小车可以顺利滑动。 3. 调整导轨的倾角，使小车开始运动。 4. 使用计时器计时，测量小车在不同倾角下的滑行时间，并记录数据。 5. 在相同的倾角下，改变小车的起始位置，再次进行实验，记录滑行时间。 6. 改变导轨倾角，重复以上实验步骤，记录滑行时间。 实验数据： 导轨倾角（度） 重复实验的滑行时间（秒） 10 1.8、1.8、1.9（取平均值1.8秒） 20 2.2、2.3、2.3（取平均值2.3秒） 30 3.1、3.2、3.3（取平均值3.2秒） 40 4.3、4.2、4.3（取平均值4.3秒） 50 5.1、5.2、5.1（取平均值5.1秒） 60 6.2、6.2、6.3（取平均值6.2秒）
实验结果： 根据实验数据，可以观察到以下现象： 1. 随着导轨倾角的增加，小车的滑行时间也随之增加。 2. 在相同的倾角下，小车的滑行时间与起始位置的无关。
实验分析： 根据实验结果，我们可以得出以下结论： 1. 物体在惯性状态下，如果没有受到外力作用，保持原有运动状态。 2. 当物体受到外力作用时，其速度和方向发生变化。
实验结论： 本实验通过观察小车在不同倾角下的滑行时间，验证了物体在惯性状态下的特点。我们可以得出结论：物体在惯性状态下，保持原有的静止或匀速直线运动状态，只有受到外力的作用才会改变其运动状态。
实验误差分析： 1. 实验中导轨的平整度可能存在误差，导致小车的滑行时间有一定的偏差。 2. 实验中使用计时器进行计时，人的反应时间可能存在误差。 3. 实验中起始位置的调整可能存在误差。
北航研究性实验报告
北航研究性实验报告北航研究性实验报告引言：研究性实验是大学教育中非常重要的一环，它旨在培养学生的科研能力和创新思维。
作为北航的一名学生，我有幸参与了一项关于飞行器设计的研究性实验，并在此报告中将对该实验进行详细的介绍和分析。
实验目的：本次实验的目的是设计一种新型飞行器，以提高其飞行效率和稳定性。
通过对飞行器的结构和控制系统进行优化，我们希望能够实现更高的飞行速度和更好的操控性能。
实验方法：在实验开始之前，我们首先进行了大量的文献调研，了解了目前飞行器设计领域的最新研究成果和技术发展趋势。
然后，我们组建了一个小组，共同讨论并确定了实验的具体方案。
在设计飞行器结构时，我们采用了轻量化材料和先进的制造技术，以减少飞行器的重量并提高其强度。
同时，我们还对飞行器的气动外形进行了优化，以减小阻力和气动干扰，并提高飞行器的升力系数。
在控制系统设计方面，我们采用了先进的自动控制算法和传感器技术，以实现飞行器的自主导航和稳定飞行。
通过对飞行器的动力学特性进行建模和仿真，我们确定了最佳的控制参数，并进行了实验验证。
实验结果：经过反复的设计和测试，我们成功地设计出了一种新型飞行器，并进行了多次试飞。
实验结果表明，该飞行器具有较高的飞行速度和较好的操控性能，达到了我们的设计目标。
结论：通过参与这个研究性实验，我深刻认识到科研的重要性和挑战性。
在实验过程中，我们不仅学到了专业知识和技能，还培养了团队合作和解决问题的能力。
此外，我们还发现了一些可以进一步改进和优化的方向。
例如，可以通过进一步研究和改进飞行器的结构和控制系统，进一步提高其性能和可靠性。
同时，还可以将所学到的知识和技术应用到其他领域，如航空航天、交通运输等。
总结：通过这次研究性实验，我对飞行器设计和控制有了更深入的了解，并提高了自己的科研能力和创新思维。
我相信，在北航这样的优秀学府中，我将有更多机会参与和开展类似的研究工作，为科技进步和社会发展做出更多贡献。
惯性导航技术在物理实验中的应用指南
惯性导航技术在物理实验中的应用指南导语：在物理实验中，准确测量和记录物体的位置、速度和加速度是非常重要的。
惯性导航技术作为一种先进的测量方法，广泛应用于物理实验中，可以提供高精度和高稳定性的测量结果。
本文将为您介绍惯性导航技术在物理实验中的应用指南，帮助您更好地使用和理解该技术。
一、惯性导航技术概述惯性导航技术是利用惯性传感器来测量和跟踪物体的运动状态的一种方法。
主要包括3个方面的测量参数：加速度、角速度和姿态。
其中，加速度计可以测量物体在各个方向上的加速度变化；陀螺仪可以测量物体的角速度变化；磁强计可以测量物体的姿态变化。
这些测量参数可以通过导航算法计算出物体的位置和速度。
二、物理实验中的应用1. 弹道实验在弹道实验中，惯性导航技术可以用于测量炮弹的飞行轨迹。
通过陀螺仪测量炮弹的角速度变化，可以计算出炮弹的旋转角度和飞行方向。
结合加速度计和磁强计的测量结果，可以精确测量炮弹的速度和位置。
这些测量结果对于炮弹的准确定位和击中目标至关重要。
2. 自由落体实验在自由落体实验中，惯性导航技术可以用于测量物体下落过程中的加速度和速度。
通过加速度计测量物体的加速度变化，可以计算出重力加速度和物体的下落高度。
结合陀螺仪的测量结果，可以计算出物体的初始速度和离地点的时间。
这些测量结果可以帮助我们更好地理解自由落体物理学的规律。
3. 运动轨迹分析在运动轨迹分析实验中，惯性导航技术可以用于跟踪物体的位置和姿态变化。
通过磁强计测量物体的姿态变化，可以计算出物体的角度和方向。
结合陀螺仪和加速度计的测量结果，可以计算出物体的速度和加速度。
这些测量结果可以帮助我们更好地分析物体的运动轨迹和变化规律。
三、惯性导航技术的优势和挑战惯性导航技术具有高精度和高稳定性的优势，可以提供准确的测量结果。
同时，由于惯性传感器内部不需要外部参考，不受环境条件的限制，适用于各种复杂的实验场景。
然而，惯性导航技术也存在一些挑战，如漂移、噪声和精度损失等问题。
陀螺仪实验报告
university of science and technology of china 96 jinzhai road, hefei anhui 230026,the people’s republic of china陀螺仪实验实验报告李方勇 pb05210284 sist-05010 周五下午第29组2号2006.10.22 实验题目陀螺仪实验（演示实验）实验目的1、通过测量角加速度确定陀螺仪的转动惯量；2、通过测量陀螺仪的回转频率和进动频率确定陀螺仪的转动惯量；3、观察和研究陀螺仪的进动频率与回转频率与外力矩的关系。
实验仪器①三轴回转仪；②计数光电门；③光电门用直流稳压电源（5伏）；④陀螺仪平衡物；⑤数字秒表（1/100秒）；⑥底座（2个）；⑦支杆（2个）；⑧砝码50克+10克（4个）；⑨卷尺或直尺。
实验原理1、如图2用重物（砝码）落下的方法来使陀螺仪盘转动，这时陀螺仪盘的角加速度?为：?=d?r/dt=m/ip (1) 式中?r为陀螺仪盘的角速度，ip为陀螺仪盘的转动惯量。
m=f.r为使陀螺仪盘转动的力矩。
由作用和反作用定律，作用力为：f=m(g-a) (2) 式中g为重力加速度，a为轨道加速度（或线加速度）轨道加速度与角加速度的关系为：a=2h/tf2； ?=a/r (3) 式中h为砝码下降的高度，r如图1所示为转轴的半径，tf为下落的时间。
将(2)(3)代入(1)2ip?2mr2t?h2mgr可得： (4)2f测量多组tf和h的值用作图法或最小二乘法拟合数据求出陀螺仪盘的转动惯量。
2、如图3所示安装好陀螺仪，移动平衡物w使陀螺仪ab轴（x轴）在水平位置平衡，用拉线的方法使陀螺仪盘绕x轴转动（尽可能提高转速），此时陀螺仪具有常数的角动量l：l=ip.?r (5) 当在陀螺仪的另一端挂上砝码m（50g）时就会产生一个附加的力矩m*，这将使原来的角动量发生改变：dl/dt=m*=m*gr* (6) 由于附加的力矩m*的方向垂直于原来的角动量的方向，将使角动量l变化dl，由图1可见： dl=ld?这时陀螺仪不会倾倒，在附加的力矩m*的作用下将会发生进动。
惯性导航系统误差传播特性分析报告
惯性导航系统误差传播特性分析报告一、系统误差方程的建立及分析1、误差源1）元件误差主要有陀螺的漂移、标度因数误差、加速度计的零偏和标度因数误差、计算机的舍入误差、电流变换装置的误差等。
2）安装误差主要指加速度计和陀螺仪在平台上的安装误差。
3）初始条件误差包括平台的初始误差以及以及计算机在解算方程时的初始给定误差。
4）运动干扰主要是冲击和振动造成的干扰。
5）其它误差如地球曲率半径的描述误差、有害加速度补偿忽略二阶小量造成的误差等等。
2、误差分析的方法1）误差分析的目的是定量地估算惯导系统测算结束时的准确程度。
正确的地理位置由当地地理坐标系来量取，而实际的测算结果是由系统计算得出的。
为了研究两者的偏差，这里引入了一个计算机坐标系（用c来标识），即将c系和t系作比较，从而定义出各种误差量。
2）一般情况下，所有误差源均可看成是对理想特性的小扰动，因而各个误差量都是对系统的一阶小偏差输入量。
因此，在研究各误差量之间的关系时，完全可以取一阶近似而忽略二阶以上的小量。
3）误差分析要求首先建立误差方程，即反映各误差量之间有机联系的方程。
这种方程是依据系统的机械编排方程通过微分处理来求取。
3、坐标系及小角度下的坐标变换矩阵的当地地理坐标系ox t y t z t，与由所确定的计计算纬度L c和经度λc算机坐标系一般来说是不重合的，它们之间存在着小角度的位置偏差。
如图所示：以指北方位系统为例，其平合坐标系p与地理坐标系t 一般来说也存在着小角度的位置偏差。
同样，p 系与c 系之间也存在着小角度的位置偏差。
1） t 系与c 系之间的方向余弦矩阵 定义纬度误差量和经度误差量：由于这种误差，使t 系与c 系之间存在着小偏差矢量角显然有如下关系如图所示，设t 系与c 系一开始是重合的； 然后c 系先绕ox t 轴转θx ，得ox t1y t1z t1系； 再绕oy t1轴转θy 得ox t2y t2z t2系，最后绕oz t2轴 转θz ，便得到计算机系ox c y c z c 。
惯性导航系统长期运行误差累积特性研究
惯性导航系统长期运行误差累积特性研究一、惯性导航系统概述惯性导航系统（INS）是一种自主式导航系统，它不依赖外部信号，而是通过测量载体的加速度和角速度来确定其位置、速度和姿态。
这种系统广泛应用于航空、航天、航海和陆地车辆导航等领域。
惯性导航系统的核心组件包括加速度计、陀螺仪、计算机和电源。
加速度计用于测量载体沿三个正交轴的加速度，而陀螺仪则用于测量载体的角速度。
计算机根据这些测量值，通过复杂的算法计算出载体的位置和速度。
1.1 惯性导航系统的基本工作原理惯性导航系统的工作原理基于牛顿运动定律和角动量守恒定律。
系统通过加速度计测量载体的加速度，然后利用积分计算出速度和位置。
同样，通过陀螺仪测量载体的角速度，积分后得到载体的角位置。
这些测量值在计算机中被处理，以提供连续的导航信息。
1.2 惯性导航系统的分类惯性导航系统可以分为两大类：平台式和捷联式。
平台式惯性导航系统使用物理平台来稳定陀螺仪，以减少外部干扰对测量精度的影响。
捷联式惯性导航系统则没有物理平台，陀螺仪直接安装在载体上，通过计算机算法来补偿载体的动态变化。
二、惯性导航系统的误差来源惯性导航系统在长期运行过程中，会受到多种因素的影响，导致误差的累积。
这些误差主要来源于以下几个方面：2.1 传感器误差传感器误差是惯性导航系统中最主要的误差来源之一。
加速度计和陀螺仪的精度直接影响到系统的性能。
这些误差包括零偏误差、比例因子误差、非线性误差和温度漂移等。
2.2 积分误差由于惯性导航系统需要对加速度和角速度进行积分计算，因此积分过程中的累积误差也是不可忽视的。
积分误差通常与积分时间成正比，随着运行时间的增加而增加。
2.3 初始对准误差惯性导航系统在启动时需要进行初始对准，以确定载体的初始位置和姿态。
初始对准的精度直接影响到后续导航的准确性。
对准过程中的误差来源包括传感器误差、环境干扰和对准算法的不完善等。
2.4 环境因素环境因素也会对惯性导航系统的性能产生影响。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
惯性导航技术综合实验实验五惯性基组合导航及应用技术实验惯性/卫星组合导航系统车载实验一、实验目的①掌握捷联惯导/GPS组合导航系统的构成和基本工作原理；②掌握采用卡尔曼滤波方法进行捷联惯导/GPS组合的基本原理；③掌握捷联惯导 /GPS组合导航系统静态性能；④掌握动态情况下捷联惯导 /GPS组合导航系统的性能。
二、实验内容①复习卡尔曼滤波的基本原理（参考《卡尔曼滤波与组合导航原理》第二、五章）；②复习捷联惯导/GPS组合导航系统的基本工作原理（参考以光衢编著的《惯性导航原理》第七章）；三、实验系统组成①捷联惯导/GPS组合导航实验系统一套；②监控计算机一台。
③差分GPS接收机一套；④实验车一辆；⑤车载大理石平台；⑥车载电源系统。
四、实验内容1）实验准备①将IMU紧固在车载大理石减振平台上，确认IMU的安装基准面紧靠实验平台；② 将IMU 与导航计算机、导航计算机与车载电源、导航计算机与监控计算机、GPS 接收机与导航计算机、GPS 天线与GPS 接收机、GPS 接收机与GPS 电池之间的连接线正确连接；③ 打开GPS 接收机电源，确认可以接收到4颗以上卫星； ④ 打开电源，启动实验系统。
2） 捷联惯导/GPS 组合导航实验① 进入捷联惯导初始对准状态，记录IMU 的原始输出，注意5分钟内严禁移动实验车和IMU ；② 实验系统经过5分钟初始对准之后，进入导航状态； ③ 移动实验车，按设计实验路线行驶；④ 利用监控计算机中的导航软件进行导航解算，并显示导航结果。
五、 实验结果及分析(一) 理论推导捷联惯导短时段（1分钟）位置误差，并用1分钟惯导实验数据验证。
1、一分钟惯导位置误差理论推导：短时段内（t<5min ），忽略地球自转0ie ω=，运动轨迹近似为平面1/0R =，此时的位置误差分析可简化为：（1） 加速度计零偏∇引起的位置误差：210.88022t x δ∇==m （2） 失准角0φ引起的误差：202 0.92182g t x φδ==m （3） 陀螺漂移ε引起的误差：330.01376g t x εδ==m 可得1min 后的位置误差值123 1.8157m x x x x δδδδ=++= 2、一分钟惯导实验数据验证结果：（1）纯惯导解算1min 的位置及位置误差图：lat0.01s 度lon0.01s度北向位移误差0.01sm 东向位移误差0.01sm（2）纯惯导解算1min 的速度及速度误差图：-100-50050Vx0.01s m /s020406080Vy0.01sm /s100020003000400050006000-0.4-0.3-0.2-0.10Vx 误差0.01s m /s100020003000400050006000-0.1-0.0500.050.1Vy 误差0.01sm /s实验结果分析：纯惯导解算短时间内精度很高，1min 的惯导解算的北向最大位移误差-2.668m ，东向最大位移误差-8.231m ，可见实验数据所得位置误差与理论推导的位置误差在同一数量级，结果不完全相同是因为理论推导时做了大量简化，而且实验时视GPS 为真实值也会带来误差；另外，可见1min 内纯惯导解算的东向速度最大误差-0.2754m/s ，北向速度最大误差-0.08027m/s 。
(二) 选取IMU 前5分钟数据进行对准实验。
将初始对准结果作为初值完成1小时捷联惯导和组合导航解算，对比1小时捷联惯导和组合导航结果。
1、5minIMU 数据的解析粗对准结果：-1.1960 -0.9998 -0.0084 0.9979 -1.1990 -0.0163 0.0062 0.1198 0.9991Cnb ⎡⎤⎢⎥=⎢⎥⎢⎥⎣⎦attitude =[219.7700 -0.9318 0.4828]2、5minIMU 数据的Kalman 滤波精对准结果：00.511.522.53x 104航向角0.01秒度x 104俯仰角0.01秒度0.511.522.53x 104横滚角0.01秒度fai = 219.9744642380873*pi/180;theta = -0.895865732956914*pi/180;gama = 0.640089448357591*pi/180;x 104-4北向速度误差0.01秒米/秒x 104-3北向速度误差方差0.01秒米/秒x 104-4东向速度误差0.01秒米/秒x 104-3东向速度误差方差0.01秒米/秒x 104-3北向失准角0.01秒度x 104-3北向失准角方差0.01秒度x 104-3东向失准角0.01秒度x 104-3东向失准角方差0.01秒度x 104-5天向失准角0.01秒度x 104-3天向失准角方差0.01秒度3、一小时IMU/GPS 数据的组合导航结果图及估计方差P 阵图：x 105纬度0.01s °x 105经度0.01s °x 105高度0.01smx 105东向速度0.01s m /sx 105北向速度0.01s m /sx 105天向速度0.01sm /sx 105航向角0.01s °x 105俯仰角0.01s °x 105横滚角0.01s°x 105-4P 航向角误差0.01s度x 105-4P 俯仰角误差0.01s度x 105-3P 横滚角误差0.01s度x 105-3P 东向速度误差0.05sm /sx 105-3P 北向速度误差0.05sm /sx 105-3P 天向速度误差0.01sm /sx 105-8P 纬度误差0.01s 度x 105-8P 经度误差0.01s 度x 105P 高度误差0.01sm4、一小时IMU 数据的捷联惯导解算结果与组合滤波、GPS 输出对比图：x 105lat0.01s 度00.51 1.52 2.53 3.54x 105lon0.01s 度54height0.01sm5、结果分析：由滤波结果图可以看出：（1） 由组合后的速度、位置的P 阵可以看出滤波之后载体的速度和位置比GPS 输出的精度高。
（2） 短时间内SINS 的精度较高，初始阶段的导航结果基本和GPS 、组合导航结果重合，1小时后的捷联惯导解算结果很差，纬度、经度、高度均发散。
（3） INS/GPS 组合滤波的结果和GPS 的输出结果十分近似，因为1小时的导航GPS 的精度比SINS 导航的精度高很多，Kalman 滤波器中GPS 信号的权重更大。
（4）总体看来，SINS/GPS组合滤波的结果优于单独用SINS或GPS导航的结果，起到了协调、超越、冗余的作用，使导航系统更可靠。
六、SINS/GPS组合导航程序%%%%%%%%%%%%%%%%%%%%%%%INS/GPS组合导航跑车1h实验%%%%%%%%%%%%%%%%%%%%%%%%%%%该程序为15维状态量，6维观测量的kalman滤波程序，惯性/卫星组合松耦合的数学模型clearclcclose all%%初始量定义wie = 0.000072921151467;Re = 6378135.072;g = 9.7803267714;e = 1.0 / 298.25;T = 0.01; %IMU频率100hz,此程序中GPS频率100hz datanumber = 360000; %数据时间3600sa = load('imu_1h.dat');w = a(:,3:5)'*pi/180/3600; %陀螺仪输出的角速率信息单位由°/h化为rad/sf = a(:,6:8)'; %三轴比力输出，单位ga = load('gps_1h_new.dat');gps_pos = a(:,3:5); %GPS输出的纬度、经度、高度信息gps_pos(:,1:2) = gps_pos(:,1:2)*pi/180; %纬经单位化为弧度gps_v = a(:,6:8); %GPS输出的东北天速度信息%捷联解算及卡尔曼相关v=zeros(datanumber,3); %组合后的速度信息atti = zeros(datanumber,3); %组合后的姿态信息pos = zeros(datanumber,3); %组合后的位置信息gyro=zeros(3,1);acc=zeros(3,1);x_kf = zeros(datanumber,15);p_kf = zeros(datanumber,15);lat = 40.0211142228246*pi/180; %组合导航的初始位置、姿态、速度lon =116.3703629769560*pi/180;height =43.0674;fai = 219.9744642380873*pi/180;theta = -0.895865732956914*pi/180;gama = 0.640089448357591*pi/180;Vx=gps_v(1,1);Vy=gps_v(1,2);Vz=gps_v(1,3);X_o=zeros(15,1); %X的初值选为0X=zeros(15,1);%Q=diag([(50e-6*g)^2,(50e-6*g)^2,(50e-6*g)^2,(0.1*pi/180/3600)^2,(0.1*pi/180/36 00)^2,(0.1*pi/180/3600)^2,0,0,0,0,0,0,0,0,0]); %随机Q=diag([(0.008*pi/180/3600)^2,(0.008*pi/180/3600)^2,(0.008*pi/180/3600)^2,(50e-6*g)^2,(50e-6*g)^2,(50e-6*g)^2,0,0,0,0,0,0,0,0,0]);R=diag([(0.01)^2,(0.01)^2,(0.01)^2,(0.1)^2,(0.1)^2,(0.15)^2]);P=zeros(15);P_k=diag([(0.00005*pi/180)^2,(0.00005*pi/180)^2,(0.00005*pi/180)^2,0.00005^2,0. 00005^2,0.00005^2,2^2,2^2,2^2,(0.001*pi/180/3600)^2,(0.001*pi/180/3600)^2,(0.00 1*pi/180/3600)^2,(50e-6*g)^2,(50e-6*g)^2,(50e-6*g)^2]); %K=zeros(15,6);Z=zeros(6,1);I=eye(15);Cnb = [ cos(gama)*cos(fai)-sin(gama)*sin(theta)*sin(fai), cos(gama)*sin(fai)+sin(gama)*sin(theta)*cos(fai), -sin(gama)*cos(theta);-cos(theta)*sin(fai), cos(theta)*cos(fai), sin(theta);sin(gama)*cos(fai)+cos(gama)*sin(theta)*sin(fai),sin(gama)*sin(fai)-cos(gama)*sin(theta)*cos(fai), cos(gama) * cos(theta)];q = [ cos(fai/2)*cos(theta/2)*cos(gama/2) - sin(fai/2)*sin(theta/2)*sin(gama/2); cos(fai/2)*sin(theta/2)*cos(gama/2) - sin(fai/2)*cos(theta/2)*sin(gama/2); cos(fai/2)*cos(theta/2)*sin(gama/2) + sin(fai/2)*sin(theta/2)*cos(gama/2); cos(fai/2)*sin(theta/2)*sin(gama/2) + sin(fai/2)*cos(theta/2)*cos(gama/2)];Cnb_s=Cnb;q_s=q;for i=1:1:datanumberRmh = Re * (1.0 - 2.0 * e + 3.0 * e * sin(lat) * sin(lat)) + height;Rnh = Re * (1.0 + e * sin(lat) * sin(lat)) + height;Wien = [ 0; wie * cos(lat); wie * sin(lat)];Wenn = [ -Vy / Rmh; Vx / Rnh; Vx * tan(lat) / Rnh];Winn = Wien + Wenn;Winb = Cnb * Winn;for j=1:3gyro(j,1) = w(j,i);acc(j,1) = f(j,i)*g; %加速度信息，单位化为m/s^2endangle = (gyro - Winb) * T;fn = Cnb'* acc;difVx = fn(1) + (2.0 * wie * sin(lat) + Vx * tan(lat) / Rnh) * Vy;difVy = fn(2) - (2.0 * wie * sin(lat) + Vx * tan(lat) / Rnh) * Vx;difVz = fn(3) + (2.0 * wie * cos(lat) + Vx / Rnh) * Vx + Vy * Vy / Rmh -g;Vx = difVx * T + Vx;Vy = difVy * T + Vy;Vz = difVz * T + Vz;lat = lat + Vy * T / Rmh;lon = lon + Vx * T / Rnh / cos(lat);height = height + Vz * T;M = [0, -angle(1), -angle(2), -angle(3);angle(1), 0, angle(3), -angle(2);angle(2), -angle(3), 0, angle(1);angle(3), angle(2), -angle(1), 0];q = (cos(norm(angle) / 2) * eye(4) + sin(norm(angle) / 2) / norm(angle) * M)* q;q = q / norm(q);Cnb = [ q(1)*q(1)+q(2)*q(2)-q(3)*q(3)-q(4)*q(4), 2*(q(2)*q(3)+q(1)*q(4)),2*(q(2)*q(4)-q(1)*q(3));2*(q(2)*q(3)-q(1)*q(4)), q(1)*q(1)-q(2)*q(2)+q(3)*q(3)-q(4)*q(4),2*(q(3)*q(4)+q(1)*q(2));2*(q(2)*q(4)+q(1)*q(3)), 2*(q(3)*q(4)-q(1)*q(2)),q(1)*q(1)-q(2)*q(2)-q(3)*q(3)+q(4)*q(4)];Rmh = Re * (1.0 - 2.0 * e + 3.0 * e * sin(lat) * sin(lat)) + height;Rnh = Re * (1.0 + e * sin(lat) * sin(lat)) + height; %以上为纯惯导解算%%F1=[0 wie*sin(lat)+v(i,1)*tan(lat)/(Rnh) -(wie*cos(lat)+v(i,1)/(Rnh)) 0-1/(Rmh) 0 0 0 0;-(wie*sin(lat)+v(i,1)*tan(lat)/(Rnh)) 0 -v(i,2)/(Rmh) 1/(Rnh)0 0 -wie*sin(lat) 0 0;wie*cos(lat)+v(i,1)/(Rnh) v(i,2)/(Rmh) 0 tan(lat)/(Rnh) 0 0 wie*cos(lat)+v(i,2)*sec(lat)*sec(lat)/(Rnh) 0 0;0 -fn(3) fn(2) v(i,2)*tan(lat)/(Rmh)-v(i,3)/(Rmh) 2*wie*sin(lat)+v(i,1)*tan(lat)/(Rnh)-(2*wie*cos(lat)+v(i,1)/(Rnh))(2*wie*cos(lat)*v(i,2)+v(i,1)*v(i,2)*sec(lat)*sec(lat)/(Rnh)+2*wie*sin(lat)*v(i,3)) 0 0;fn(3) 0 -fn(1) -2*(wie*sin(lat)+v(i,1)*tan(lat)/(Rnh)) -v(i,3)/(Rmh) -v(i,2)/(Rmh) -(2*wie*cos(lat)+v(i,1)*sec(lat)*sec(lat)/(Rnh))*v(i,1) 0 0;-fn(2) fn(1) 0 2*(wie*cos(lat)+v(i,1)/(Rnh)) 2*v(i,2)/(Rmh) 0 -2*wie*sin(lat)*v(i,1) 0 0;0 0 0 0 1/(Rmh) 0 0 0 0;0 0 0 sec(lat)/(Rnh) 0 0 v(i,1)*sec(lat)*tan(lat)/(Rnh) 0 0;0 0 0 0 0 1 0 0 0];G=[Cnb',zeros(3);zeros(3),Cnb';zeros(9,6)];H=[zeros(3),eye(3),zeros(3),zeros(3,6);zeros(3),zeros(3),diag([Rmh,Rnh*cos(lat),1]),zeros(3,6)]; %量测阵F2=[-Cnb',zeros(3);zeros(3),Cnb';zeros(3),zeros(3)];F=[F1,F2;zeros(6,15)]; %以上为kalman滤波模型参数F = F * T; %离散化temp1 = eye(15);disF = eye(15);for j = 1:10temp1 = F * temp1 / j;disF = disF + temp1;endtemp1 = Q * T;disQ = temp1;for j = 2:11temp2 = F * temp1;temp1 = (temp2 + temp2')/j;disQ = disQ + temp1;endZ(1) = Vx - gps_v(i,1); %量测量为纯惯导与GPS的速度差、位置差Z(2) = Vy - gps_v(i,2);Z(3) = Vz - gps_v(i,3);Z(4) = (lat - gps_pos(i,1)) * Rmh; %纬经度化为位移，单位m Z(5) = (lon - gps_pos(i,2)) * Rnh * cos(lat);Z(6) = height - gps_pos(i,3);X = disF * X_o; %kalman滤波五个公式P = disF * P_k * disF'+ disQ;K = P * H'/( H * P * H'+ R);X_k = X + K * (Z - H * X);P_k = (I - K * H) * P;x_kf(i,1) = X_k(1)/pi*180; %平台误差角x_kf(i,2) = X_k(2)/pi*180;x_kf(i,3) = X_k(3)/pi*180;x_kf(i,4) = X_k(4); %速度误差x_kf(i,5) = X_k(5);x_kf(i,6) = X_k(6);x_kf(i,7) = X_k(7); %位置误差x_kf(i,8) = X_k(8);x_kf(i,9) = X_k(9);x_kf(i,10) = X_k(10)/pi*180*3600; %陀螺随机常值漂移,单位°/hx_kf(i,11) = X_k(11)/pi*180*3600;x_kf(i,12) = X_k(12)/pi*180*3600;x_kf(i,13) = X_k(13)*10^6/g; %加计随机常值偏置，单位ugx_kf(i,14) = X_k(14)*10^6/g;x_kf(i,15) = X_k(15)*10^6/g;p_kf(i,1) = sqrt(abs(P_k(1,1)))/pi*180;p_kf(i,2) = sqrt(abs(P_k(2,2)))/pi*180;p_kf(i,3) = sqrt(abs(P_k(3,3)))/pi*180;p_kf(i,4) = sqrt(abs(P_k(4,4)));p_kf(i,5) = sqrt(abs(P_k(5,5)));p_kf(i,6) = sqrt(abs(P_k(6,6)));p_kf(i,7) = sqrt(abs(P_k(7,7)));p_kf(i,8) = sqrt(abs(P_k(8,8)));p_kf(i,9) = sqrt(abs(P_k(9,9)));p_kf(i,10) = sqrt(abs(P_k(10,10)))/pi*180*3600;p_kf(i,11) = sqrt(abs(P_k(11,11)))/pi*180*3600;p_kf(i,12) = sqrt(abs(P_k(12,12)))/pi*180*3600;p_kf(i,13) = sqrt(abs(P_k(13,13)))*10^6/g;p_kf(i,14) = sqrt(abs(P_k(14,14)))*10^6/g;p_kf(i,15) = sqrt(abs(P_k(15,15)))*10^6/g;Vx = Vx - X_k(4); %速度校正Vy = Vy - X_k(5);Vz = Vz - X_k(6);v(i,:) = [Vx, Vy, Vz];lat = lat - X_k(7); %位置校正lon = lon - X_k(8);height = height - X_k(9);pos(i,:) = [lat, lon, height];Atheta = X_k(1); %kalman滤波估计得出的失准角theta Agama = X_k(2); %kalman滤波估计得出的失准角gama Afai = X_k(3); %kalman滤波估计得出的失准角fai Ctn = [ 1, Afai, -Agama;-Afai, 1, Atheta;Agama, -Atheta, 1];Cnb = Cnb*Ctn; %更新姿态阵fai = atan(-Cnb(2,1) / Cnb(2,2));theta = asin(Cnb(2,3));gama = atan(-Cnb(1,3) / Cnb(3,3));if (Cnb(2,2) < 0)fai = fai + pi;elseif (fai < 0)fai = fai + 2*pi;endif (Cnb(3,3) < 0)if(gama > 0)gama = gama - pi;elsegama = gama + pi;endendatti(i,:) = [fai/pi*180, theta/pi*180, gama/pi*180];q(2) = sqrt(abs(1 + Cnb(1,1) - Cnb(2,2) - Cnb(3,3))) / 2;q(3) = sqrt(abs(1 - Cnb(1,1) + Cnb(2,2) - Cnb(3,3))) / 2;q(4) = sqrt(abs(1 - Cnb(1,1) - Cnb(2,2) + Cnb(3,3))) / 2;q(1) = sqrt(abs(1 - q(2) * q(2) - q(3) * q(3) - q(4) * q(4)));if (Cnb(2,3) < Cnb(3,2))q(2) = - q(2);endif (Cnb(3,1) < Cnb(1,3))q(3) = - q(3);endif (Cnb(1,2) < Cnb(2,1))q(4) = - q(4);endX_k(1:9) = 0;X_o=X_k;iend%%%绘图%%%%%%%%%%t=1:datanumber;figure(1)subplot(311);plot(t,pos(:,1)*180/pi,'r',t,gps_pos(:,1)*180/pi,'b') title('纬度');xlabel('0.01s');ylabel('°');subplot(312);plot(t,pos(:,2)*180/pi,'r',t,gps_pos(:,2)*180/pi,'b') title('经度');xlabel('0.01s');ylabel('°');subplot(313);plot(t,pos(:,3),'r',t,gps_pos(:,3),'b')title('高度');xlabel('0.01s');ylabel('m');legend('组合滤波','GPS')figure(2)subplot(311);plot(t,v(:,1),'r',t,gps_v(:,1),'b')title('东向速度');xlabel('0.01s');ylabel('m/s'); subplot(312);plot(t,v(:,2),'r',t,gps_v(:,2),'b')title('北向速度');xlabel('0.01s');ylabel('m/s'); subplot(313);plot(t,v(:,3),'r',t,gps_v(:,3),'b')title('天向速度');xlabel('0.01s');ylabel('m/s'); legend('组合滤波','GPS')figure(3)subplot(311);plot(t,atti(:,1))title('航向角');xlabel('0.01s');ylabel('°');subplot(312);plot(t,atti(:,2))title('俯仰角');xlabel('0.01s');ylabel('°');subplot(313);plot(t,atti(:,3))title('横滚角');xlabel('0.01s');ylabel('°');figure(4)subplot(311);plot(t,p_kf(:,1))title('P航向角误差');xlabel('0.01s');ylabel('度'); subplot(312);plot(t,p_kf(:,2))title('P俯仰角误差');xlabel('0.01s');ylabel('度'); subplot(313);plot(t,p_kf(:,3))title('P横滚角误差');xlabel('0.01s');ylabel('度'); figure(5)subplot(311);plot(t,p_kf(:,4))title('P东向速度误差');xlabel('0.01s');ylabel('m/s'); subplot(312);plot(t,p_kf(:,5))title('P北向速度误差');xlabel('0.01s');ylabel('m/s'); subplot(313);plot(t,p_kf(:,6))title('P天向速度误差');xlabel('0.01s');ylabel('m/s'); figure(6)subplot(311);plot(t,p_kf(:,7))title('P纬度误差');xlabel('0.01s');ylabel('度'); subplot(312);plot(t,p_kf(:,8))title('P经度误差');xlabel('0.01s');ylabel('度'); subplot(313);plot(t,p_kf(:,9))title('P高度误差');xlabel('0.01s');ylabel('m');figure(7)subplot(311);plot(t,p_kf(:,10))title('P东向陀螺');xlabel('0.01s');ylabel('度/小时'); subplot(312);plot(t,p_kf(:,11))title('P北向陀螺');xlabel('0.01s');ylabel('度/小时'); subplot(313);plot(t,p_kf(:,12))title('P天向陀螺');xlabel('0.01s');ylabel('度/小时'); figure(8)subplot(311);plot(t,p_kf(:,13))title('P东向加计');xlabel('0.01s');ylabel('ug'); subplot(312);plot(t,p_kf(:,14))title('P北向加计');xlabel('0.01s');ylabel('ug');subplot(313);plot(t,p_kf(:,15))title('P天向加计');xlabel('0.01s');ylabel('ug');（注：文档可能无法思考全面，请浏览后下载，供参考。