捷联惯导链路总结：初始对准、位置标定、姿态更新与GPS/INS组合

从过年前忙到现在总算把捷联惯导这套系统从头到尾完整跑通了一遍。从初始对准、位置标定到姿态更新算法再到GPS/INS组合导航每个环节都踩了不少坑也积累了不少经验。这篇东西把整个链路做个总结当成给自己留的技术笔记也分享给正在做惯导的同行。先说下这篇总结的适用范围。我用的是消费级和工业级的MEMS惯性器件不是光纤陀螺或激光陀螺那套高精度体系。如果你的器件量级在陀螺零偏稳定性几度每小时到几十度每小时、加速度计零偏几毫克这个范围那这篇文章几乎可以全覆盖。如果你用的是高精度光纤级器件对准、滤波的思路依然相通只是参数数量级和某些算法取舍上会有区别。项目标题: 捷联惯导总结--初始对准位置标定INS姿态更新GPS/INS组合后面我就按照从系统上电到稳定输出导航结果的顺序把每个环节的坑和经验都写清楚。1. 一条完整链路从陀螺加表到组合导航输出很多人刚接触捷联惯导时最容易犯的错误是上来就盯姿态更新算法觉得那才是核心。实际上捷联惯导系统是一个链路任何一个环节出问题后续全都白搭。我梳理一下整个数据流大家先有个整体概念。1.1 惯导系统上电后的完整工作序列一套典型的捷联惯导系统从上电到输出稳定的导航结果大致经历以下流程器件上电预热采集一段静态数据用于初始对准。初始对准阶段确定载体姿态的初始值通常分粗对准和精对准两步。位置标定阶段确定载体的初始经纬度和高度这个值从哪里来、精度如何决定了惯导后续的定位漂移水平。姿态更新阶段系统进入纯惯性导航模式后利用陀螺输出的角速度实时更新姿态利用加速度计比力减去重力后积分得速度再积分得位置。组合导航阶段引入外部观测最常见的是GPS用卡尔曼滤波估计并修正惯导系统的误差状态抑制纯惯导发散。这个流程每一步都是承上启下的关系。初始对准角度偏了后面的姿态更新全偏组合滤波里状态估计也救不回来因为姿态误差会耦合进速度、位置的状态方程里。位置标定给错了纯惯导模式的定位直接多了个常数偏置卡尔曼滤波的估计也会被误导。所以这篇文章不按算法难度排顺序按工程实施顺序来讲。1.2 坐标系约定是整条链路的语法基础做惯导最容易乱的就是坐标系。我在实际写代码前花在统一坐标系定义上的时间不比写算法本身少。捷联惯导用到的主要坐标系就这么几个载体坐标系b系x轴通常是载体前方y轴向右z轴向下前右下或者x前、y左、z上前左上也有各家用各家的关键是自洽。导航坐标系n系我这里用东北天ENU即x指向东y指向北z指向天。这在国内工程界最常见对应的姿态角定义航向角、俯仰角、横滚角也是按照北偏东为0度顺时针增加。地心惯性坐标系i系陀螺测量的角速度就是相对于i系的这是物理基准。地球坐标系e系做GPS/INS组合时GPS位置和速度通常基于WGS-84椭球和ecef坐标系需要把它转到ENU导航系里。这几套坐标系之间的转换关系是捷联惯导的基础语法。语法不统一后面的姿态矩阵C_b^n、位置更新公式全对不上。建议代码实现时从一开始就建立坐标系转换库用矩阵表示姿态而非欧拉角避免万向节锁死的坑。2. 初始对准系统第一次睁眼的关节时刻捷联惯导是被动式导航系统它只能测量角速度和加速度的变化不能直接告诉你当前朝向哪里。初始对准就是解决我是谁、我面朝哪的问题。初始对准做得够不够快、够不够准直接决定了整个系统的指标上限。2.1 粗对准的解析方法利用重力与地球自转角速度静基座粗对准的基本思想在导航系下重力矢量指向天向-g方向和z轴重合地球自转角速度矢量在东北天下的投影是一个已知量。而在载体系下加速度计实测的比力和陀螺实测的角速度可以让我们得到这两个矢量在b系中的分量。这样就有了两对矢量通过双矢量定姿就能求出姿态矩阵C_b^n。具体的解析方法假定导航系为东北天系。地球自转角速度在n系中的分解ω_n^ie [0, ωie * cos(L), ωie * sin(L)]这里L是当地纬度ωie约为7.292115 × 10^-5 rad/s。在静基座条件下加速度计测量比力f^b近似等于-g^n在b系的投影陀螺测量角速度ω_ib^b近似等于ω_ie^b。由于C_b^n是单位正交矩阵我们可以构造两个已知矢量分别在n系和b系的分量g^n [0, 0, -g]g^b f^b取反ω_ie^n [0, ωiecos(L), ωiesin(L)]ω_ie^b 陀螺实测然后构建矩阵利用C_b^n把n系矢量转到b系g^b C_n^b * g^nω_ie^b C_n^b * ω_ie^n用这两个矢量对构造双矢量定姿问题直接求解C_n^b。一个常用做法是构造两个中间矢量v1 gv2 g × ωv3 g × (g × ω)分别求在n系和b系中的值然后组装矩阵解算出C_b^n。这个方法的精度受限于陀螺测量地球自转角速度的能力。理想情况下纬度45度处的水平分量约为5.16 × 10^-5 rad/s折合约2.96°/h。如果你的陀螺零偏稳定性在10°/h以上地球自转角速度信号基本被零偏淹没了对准后的航向角会有很大误差。这正是MEMS陀螺静基座粗对准精度差的原因——不是算法不行是器件量级达不到。2.2 精对准从粗对准结果出发估计失准角粗对准得到粗略姿态后还需要精对准。精对准的本质是用滤波器经典罗经对准或卡尔曼滤波对失准角进行估计和修正。静基座精对准常用的方法是基于小失准角误差模型的卡尔曼滤波。状态量通常是水平失准角α、β和方位失准角γ以及陀螺零偏和加速度计零偏。利用静基座下速度误差为零作为观测量——实际中加速度计测到的比力积分出来的速度在静基座下应该始终为零任何因为失准角产生的虚假速度变化都可以反推失准角。这里有个对初学者很困惑的概念为什么静基座下速度误差可以观测出方位失准角γ从系统可观测性角度看水平失准角的可观性很强速度误差几分钟内就能把α、β估计出来但γ的可观测性很差因为γ主要通过地球自转角速度水平分量耦合进系统收敛速度取决于陀螺能否分辨出地球自转角速度。所以MEMS惯导方位对准通常需要几分钟甚至更长且最终精度受限于器件零偏。工程上有个非常实用的加速方法如果GPS信号有效可以在静基座对准后立刻切入组合导航模式用GPS的速度观测辅助方位角收敛。这个方法在车载项目中特别管用车辆只要开动起来方位角误差迅速被GPS速度方向纠正。2.3 对准时最容易踩的工程坑初始对准这个环节理论上讲起来半小时工程上调试能折腾一周。我列几个实际踩过的坑静止检测判据不能只看加速度计标准差。有一次车辆停在路边但发动机怠速振动很大。加速度计标准差超阈值系统误判为运动状态对准程序不启动。后来我把陀螺模值也加入判据且要求连续判断一段时间比如连续5秒都满足条件才认为静止。MEMS陀螺在振动环境下角速度方差很大双条件判断更可靠。对准时长和精度权衡。精对准时间内如果载体被推动或晃动直接导致对准失败或精度恶化。应对方案是检测到非静止立即重置对准而不是继续累积。这个逻辑在代码里必须写清楚不然会带着错误姿态进入导航。初始不水平的情况。如果载体停在坡道上粗对准的解析公式依然成立因为不需要水平假设。但精对准的失准角模型是小角度线性化的坡度大于5度时线性误差会明显。这种情况下先用一段时间静止解算得到初始俯仰和横滚再做小失准角滤波。表不同器件精度下的静基座对准效果预期器件等级陀螺零偏稳定性水平对准精度方位对准精度建议对准时长战术级0.01°/h优于0.01°0.01° ~ 0.03°3~5分钟工业级1°/h0.01° ~ 0.05°0.1° ~ 0.5°3~10分钟消费级MEMS5~30°/h0.05° ~ 0.2°1° ~ 5°以上1~5分钟3. 位置标定经纬度初始值看似不起眼实则决定成败位置标定经常被大家忽略因为看着太简单了——给个初始经纬度就行。但它的影响范围远比想象中大。3.1 初始经纬度对纯惯导积累误差的影响捷联惯导的位置解算是从初始经纬度开始积分的。如果初始经度差了1毫角秒约为31米那么纯惯导模式下的位置输出就会带有一个固定约31米的偏移。这里的数学关系是位置初始位置速度积分。初始位置是积分常数一旦错了无论怎么积分都救不回来。在组合导航模式中初始位置误差会作为状态量在量测更新中被估计出来但需要一定收敛时间。收敛期间载体的导航输出带有明显偏差。如果初始位置误差过大比如几公里卡尔曼滤波容易收敛到错误的局部极小值甚至滤波器发散。所以工程实践中初始位置有三种获取方式GPS定位精度最好单点定位下几米量级。人工输入/地图匹配精度随输入源而定通常几十到几百米。未知初始位置这种情况适合用松组合让滤波器自己收敛出位置误差但要在运动后处理。我在车载终端方案里的做法是设备启动后先等GPS有效定位才进入INS导航模式。如果GPS信号不可用则沿用上次关机时的位置同时标记为低置信度初始位置让滤波器的位置状态初始方差设置得很大靠后续量测把位置拉回来。3.2 重力模型与垂线偏差位置标定的隐藏细节位置标定不仅是经纬度还包括重力加速度的准确赋值。在惯导解算中比力方程需要用到重力模型g f(纬度高度)最常用的公式是WGS-84椭球下的正常重力公式把纬度和高度映射为重力加速度。这个值在纬度0度赤道处约为9.7803 m/s²两极约9.8322 m/s²。看似只有0.5%的变化但在比力方程中重力项是加速度计测量的主要抵消项。如果重力给错1e-4 m/s²约等于10毫伽等效于一个持续的水平加速度偏差短时间内就会产生明显的速度漂移。一个常被忽略的工程细节是杆臂效应在初始标定中的影响。如果GPS天线不在IMU质心位置安装杆臂矢量会直接影响GNSS位置观测与INS位置之间的换算以及在初始时刻的位置初值。我在系统中为此单独维护一个IMU到GPS天线的杆臂参数并在每次量测更新前把GPS观测值转到IMU位置。3.3 传感器级标定位置精度的根基位置标定另一层含义是标定惯性器件本身的误差。你拿着一个陀螺和一个加速度计去测得到的是包含零偏、标度因数误差、安装误差等的原始量。不把这些误差标出来后面所有算法都是空中楼阁。消费级MEMS惯导最常用的标定方案加速度计标定六位置法。把IMU的六个面分别朝上和朝下采集各轴静态数据利用重力矢量已知这一约束可以求出标度因数、零偏和安装误差。注意每个位置要静止足够时间我一般采集30秒取平均且一旦固定数据采集时间系统不可有振动干扰。陀螺标定多位置法或速率法。高精度场景用转台给已知角速率标定标度因数和零偏。没有转台时可以做多位置静态下的零偏标定和Allan方差分析获得零偏稳定性、随机游走系数等指标。网上很多代码里直接拿器件手册的参数当零偏标度用这在小规模演示里可以放进实际产品里基本不行。器件温度漂移带来的零偏变化往往比手册给的典型值大好几倍。所以量产系统里还有一个环节是温补把零偏随温度变化的曲线标出来做补偿。4. INS姿态更新四元数算法的核心与边界姿态更新是捷联惯导算法的心脏。陀螺输出的是角增量或角速度姿态更新负责把它转化为载体姿态的连续变化。姿态表示方式有很多但工程实现中四元数是绝对的主流。4.1 为什么姿态表示选四元数而不是欧拉角先说说各选项的比较表示方式优点缺点欧拉角直观符合人类理解存在万向节锁死三角函数计算复杂方向余弦矩阵无奇异矩阵直接用于矢量变换9个元素冗余必须正交化处理计算量大四元数无奇异计算量小便于插值物理意义相对抽象需注意规范化实际导航解算中需要高频率100Hz~1kHz更新姿态矩阵欧拉角在这个场景下非常不利——每次更新要做多次sin/cos计算且万向节锁死问题在动态载体上不是理论风险而是现实风险。四元数只有4个元素更新公式简洁约束只有一个模长为1非常便于工程实现。很多人会问既然姿态矩阵最终用于比力分解那能不能直接用方向余弦矩阵更新当然可以但你要做9个元素的微分方程积分每个周期还要做正交化和规范化计算量明显高于四元数。现代处理器虽然不在乎这点算力但在高更新率系统比如云台或飞行器飞控上四元数依然是最优雅的方案。4.2 四元数姿态更新算法的具体推导与实现四元数姿态更新基于四元数微分方程q_nb_dot 0.5 * q_nb ⊗ ω_nb^b其中ω_nb^b是载体角速度在b系的表示。这里有个工程细节陀螺测的是相对于惯性系的角速度ω_ib^b而姿态更新中需要的是载体相对于导航系的角速度ω_nb^b。两者之间差一个地球自转角速度和载体运动引起的导航系旋转ω_nb^b ω_ib^b - C_n^b * (ω_ie^n ω_en^n)其中ω_en^n是载体相对地球运动引起的导航系旋转项和东向北向速度、曲率半径有关。低速或短时间车载场景ω_en^n很小可以忽略甚至地球自转项在一些低成本方案里也被省略。但严格说中高精度系统这两项不能丢否则长航时纯惯导会累积可观的姿态误差。微分方程的离散求解我用的是毕卡逼近法。角增量ΔΘ ω * dt一阶算法写出来是这样q_new (cos(0.5*|ΔΘ|) * I sin(0.5*|ΔΘ|) / |ΔΘ| * Ω) * q_old其中Ω是角增量构成的反对称矩阵。当采样周期很短10ms|ΔΘ|非常小可以做近似展开q_new ≈ (1 - |ΔΘ|²/8) * q_old 0.5 * [ΔΘ]× * q_old在车载动态环境下一阶和二阶毕卡结果差异不大但如果你后续要做更高动态的无人机或机器人平台建议用三阶展开或等效旋转矢量法。4.3 圆锥运动补偿姿态更新里最容易被忽略的性能瓶颈对MEMS级别的器件来说圆锥误差的影响可能被噪声淹没很多人就忘了这回事。但在划船、振动、角速率急剧变化的场景下忽略圆锥补偿会导致姿态误差随时间线性累积。圆锥运动指的是载体角速度矢量在空间中以锥面形式旋转比如机体绕纵轴高频振动的同时航向也在变化。即使角度很小圆锥运动也会导致四元数二子样、三子样算法的误差放大。工程实践上使用等效旋转矢量法并使用多子样采样补偿圆锥误差是标准做法。MEMS惯导系统如果只做最简单的一子样四元数更新在强振动环境下姿态误差可能比带圆锥补偿的算法大一个数量级。我在车载振动环境下对比过一子样和二子样圆锥补偿横滚角误差从约0.3°下降到0.08°效果非常明显。具体的多子样圆锥补偿公式这里不展开全部推导就说一下原理在一个更新周期内把陀螺角增量分成多个子样通过子样叉积项估计圆锥运动引起的等效旋转矢量偏差修正姿态更新。子样数越多对高频圆锥运动的补偿越好但计算量也随之增加。实际车载场景二子样足够。4.4 姿态更新的节奏与规范化陷阱姿态更新频率要远高于载体角运动频率。经验法则如果载体最大角速度约200°/s普通车载运动更新率至少100Hz。工业级方案我推荐200Hz~500Hz这样角增量足够小逼近精度足够高。四元数规范化是另一个看起来简单但容易出问题的点。四元数随时间漂移模长会偏离1如果不定期规范化导航矩阵的正交性会被破坏比力分解结果也会产生偏差。但规范化频率不必每个周期做一次那样反而会引入额外的舍入误差。我建议每10~100个更新周期做一次规范化每次计算4个元素的平方和开方然后归一化完全够用。5. GPS/INS组合松组合的工程实现与调参心法惯导单独使用误差随时间发散GPS单独使用在信号遮挡时无法定位这两者的互补性天然决定了它们是黄金搭档。GPS/INS组合导航是捷联惯导系统实用化的关键。5.1 组合架构选择从松组合到深组合组合导航按耦合深度分为三种架构松组合Loosely Coupled用GPS输出的位置和速度作为观测量和INS输出的位置速度做差分送入卡尔曼滤波器估计误差状态。实现简单模块独立性强适合绝大多数车载和机器人场景。紧组合Tightly Coupled直接用GPS原始伪距、伪距率作为观测量不用GPS解算位置。在可见卫星数少于4颗时松组合无法定位但紧组合仍能利用部分卫星。适合城市峡谷、遮挡严重的场景。深组合Deeply CoupledGPS接收机内部和INS深度融合能在高动态和强干扰环境下保持跟踪。主要用在导弹、制导等高动态场景普通开发者基本碰不到。这篇总结里我重点讲松组合因为它最容易落地也是我实际做过的方案。5.2 15状态卡尔曼滤波的状态方程与量测方程松组合的标准做法是把误差状态作为滤波状态量误差方程是线性化的。我用的状态量是15维姿态误差3个φe, φn, φu速度误差3个δVe, δVn, δVu位置误差3个δL, δλ, δh或ECEF下的delta位置陀螺零偏3个εx, εy, εz加速度计零偏3个∇x, ∇y, ∇z状态方程由惯导误差传播方程给出离散化后x_k F_k * x_{k-1} w_k其中F矩阵里包含了姿态误差对速度误差的耦合、速度误差对位置误差的耦合、陀螺零偏对姿态误差的驱动等。这里的关键是F矩阵的构建要严格按照你采用的坐标系和运动状态推导不同文献的方程形式有些微差异不能抄错了版本。量测方程就是GPS和INS的位置速度差z [P_gps - P_ins; V_gps - V_ins] H * x vH矩阵就是把位置误差状态和速度误差状态映射到量测差上。滤波关键参数是两个噪声矩阵Q过程噪声和R量测噪声。Q矩阵描述状态方程的随机扰动强度主要反映陀螺和加速度计的噪声、随机游走特性。如果器件零偏稳定性差Q里对应的过程噪声方差要调大否则滤波器会过于信任预测而不更新导致估计滞后。R矩阵描述量测噪声GPS定位误差通常在几米到十几米。城市环境下GPS误差分布不均匀R要动态调整我采用基于GPS定位状态和卫星数目的变差R策略卫星少或HDOP大时R放大降低量测权重。5.3 组合导航调参实践中的经验和教训调卡尔曼滤波是最考验耐心的部分。我总结几个亲自踩过的坑Q矩阵设置过小导致滤波器发散或输出震荡。典型表现定位结果一会儿贴近GPS一会儿贴近INS输出跳变。排查方法把Q中陀螺零偏对应的方差逐步调大观察状态估计是否回归合理值。GPS数据跳变和粗差问题。城市峡谷中GPS位置可能突然跳几十米。如果不做粗差检测这个跳变会直接通过量测更新污染全部状态估计。我的做法是设置新息innovation门限检测如果新息的马氏距离超过阈值比如5倍卡方分位点就把该次量测的R放大100倍或直接跳过本次更新。状态的可观测性和收敛速度。静基座下组合导航位置误差可观测性差只有运动起来GPS位置才能充分约束位置误差。所以滤波器启动后不要着急用它的输出先让系统运动几分钟等滤波收敛再说。这在工程品控环节要特别注明。IMU与GPS时间同步问题。这是松组合中最隐蔽但影响巨大的问题。GPS输出的UTC时间戳和IMU采样时刻之间如果有延迟比如GPS信号处理链路延时50ms相当于量测时刻和预测时刻对不上速度误差会被系统性放大。解决方案用GPS PPS脉冲作为硬件同步基准或者用软件估计IMU和GPS间的时间差。我在实际测试中发现100ms的同步误差在车速30m/s下会造成约3m的定位偏差这在车道级导航中完全不能接受。下面给一个松组合系统中状态方程核心代码的伪代码框架# 状态转移矩阵F的构建简化为2D情景演示 # 姿态误差 phi, 速度误差 dV, 位置误差 dP, 陀螺零偏 eps, 加速度计零偏 acc_bias F zeros((15, 15)) # 姿态误差方程 F[0:3, 0:3] -cross_matrix(omega_ie omega_en) # 姿态误差的自耦合 F[0:3, 3:6] -C_b_n # 陀螺零偏对姿态误差的驱动 F[0:3, 9:12] -C_b_n # 陀螺噪声对姿态的驱动 # 速度误差方程 F[3:6, 0:3] skew_matrix(force_n) # 姿态误差引起比力投影误差 F[3:6, 3:6] -(2*omega_ie omega_en) # 科氏项和导航系旋转项 F[3:6, 6:9] gravity_gradient_matrix() # 重力模型变化 F[3:6, 12:15] C_b_n # 加速度计零偏对速度的影响 # 位置误差方程 F[6:9, 3:6] C_n_e # 速度误差到位置误差这个框架在完整的15维状态下展开具体每一项要根据你的导航系和坐标系约定来写。6. 工程实测中的几个经典故障与排查思路最后分享几个我在实测中遇到的实际故障和完整的排查链路。这些案例比算法推导更能帮助大家理解系统的耦合关系。6.1 故障车载静止时导航输出的航向缓慢漂移现象车辆静止停在停车场导航输出的航向角以每分钟零点几度的速度缓慢变化。排查链路第一步看陀螺原始数据。如果陀螺原始零偏稳定说明不是器件问题。第二步看姿态更新代码。检查是否把地球自转角速度项错误地减掉了两次或没有减。这个问题在代码初版非常常见。第三步检查对准后的姿态初值。如果静止时航向漂移很可能是初始对准时方位角就偏了而纯惯导模式下没有外部观测量来修正。结论该场景下航向漂移的本质是MEMS陀螺零偏和噪声在静止时无法被GNSS观测修正松组合模式下静止时GPS速度观测对航向的约束很弱如果GPS速度噪声大航向的可观测性低。解决思路是改用速度约束辅助车辆静止时速度为零作为虚拟观测。这个排查链路能帮你明确漂移到底是器件问题、对准问题还是组合导航算法问题。6.2 故障车辆转弯后组合导航位置出现尖刺现象车辆转弯过程中组合导航的位置输出出现一个明显尖刺转弯结束后恢复正常。排查链路检查加速度计在转弯时输出是否超量程或出现非线性。高离心力场景下MEMS加速度计可能瞬间饱和。检查姿态更新在转弯时是否正确。转弯时陀螺输出增大如果算法更新率不够姿态误差会短暂增大进而比力分解出错。检查GPS多路径效应。转弯时车体姿态变化导致GPS天线方向改变接收机可能出现周跳或多路径干扰。排查结果在我的场景里问题出在GPS原始观测上有野值而我当时只对位置输出做了低通滤波没有做新息粗差检测。加上新息门限检测后尖刺消失。这类问题再次印证组合导航系统的鲁棒性不靠单一算法要靠一层层防护——原始数据质量检查、量测粗差检测、滤波器自适应调节。6.3 加速度计零偏对姿态和位置的耦合效应加速度计零偏不会只影响速度它通过比力方程影响姿态解算进而影响位置。很多初学者以为加速度计零偏只引入一个积分误差实际上在姿态环里它就搅拌进去了。假设水平加速度计零偏为1mg它在静基座下会被误认为倾斜角等效失准角约为0.057°。这个失准角会让重力矢量的一部分被投影到水平方向产生持续的水平加速度最终导致速度和位置误差随时间加速增长。位置误差中来自加速度计零偏的项是随时间平方增长的而来自陀螺零偏的项是随时间立方增长的。所以在长航时纯惯导中陀螺零偏的影响远大于加速度计零偏。这个耦合关系的工程启示做传感器选型时如果系统要求短时间几分钟姿态精度加速度计零偏是主要矛盾如果要求长时间半小时以上位置精度陀螺零偏是主要矛盾。不要只盯着单轴指标表要看整个误差链路。7. 我做这套系统后的一些体会写到这里核心内容基本都覆盖了。最后说几个真实的操作体会。第一个体会是代码架构比算法本身重要。捷联惯导的算法其实高度成熟推来推去就那么几个公式但工程落地的难点在于坐标系统一、时间同步、异常处理、数据记录这些脏活。我建议从一开始就设计一个干净的传感器数据接口层让IMU和GPS数据都以统一的时间戳进入融合模块后续调试会轻松很多。第二个体会是必须有可复现的调试工具链。没有离线数据回放工具你很难定位问题。我的做法是每帧数据都记录原始IMU、原始GPS、中间层解算结果和最终导航输出的完整日志配合python脚本做离线分析和可视化。很多bug都是靠事后回放定位的不是现场调试。第三个体会是MEMS惯导的标定和对准是3分算法、7分工程。你花一周调出来的滤波器可能不如认真做一次六位置标定带来的精度提升大。先保证输入数据干净再去追求高阶补偿算法这个顺序不能反。这套系统从搭框架到调试稳定前后花了我将近两个月。中间好几次想放弃特别是看到漂亮的曲线图背后还藏着几米的误差时。现在回头总结痛点基本都在对准和标定环节姿态更新和组合滤波反而相对顺利。希望这篇总结能帮你少走弯路。做惯导就是这样每一步都要踩实了再走下一步。