MEMS惯导在晃动环境下做自对准，这个需求我太熟悉了。不管是船载设备、移动测绘车，还是机械臂末端的姿态参考系统，你都会撞上同一个尴尬局面：理论书上都写“静基座对准”，可实际现场根本没有绝对的“静”——发动机在震、甲板在晃、人在走动。标题里这三个词，MEMS、惯导、自对准，看起来都是老生常谈，但把“晃动”这个条件加进去之后，整套处理思路都要变。这篇文章我就围绕这个具体工况，把从建模到滤波参数整定、再到实测评估的完整过程拆开讲清楚，给正在调对准算法的朋友一份可以直接落地的参考。
1. 项目概述与需求拆解
1.1 这个项目要解决的核心问题
自对准，本质上是在系统上电之后、正式导航之前，确定载体坐标系相对导航坐标系的初始姿态矩阵。对MEMS惯导来说，这个姿态矩阵的水平部分通常靠加速度计观测重力方向来估计，方位部分传统上靠陀螺敏感地球自转角速度的投影来估计。但这个流程有一个隐性前提：载体必须近似静止。一旦载体晃动，加速度计输出里混入了动态干扰加速度，陀螺输出里混入了角振动，直接用解析法算姿态，结果会剧烈跳变甚至完全错误。
我见过不少团队评估MEMS惯导时，一开始都用三轴转台或者大理石平台做静态对准，性能指标很好，一到车上、船上实测就原形毕露。原因不是传感器变差了，而是对准算法没有针对晃动环境做处理。本文要讲的，就是如何让自对准在晃动条件下依然能收敛到可用精度，说白了就是两个问题：晃动怎么抑制，对准怎么估计。
1.2 为什么晃动环境会打破传统自对准的前提
先看水平对准。解析法里俯仰角和横滚角的计算公式是：根据重力加速度在载体坐标系三个轴上的投影分量反算姿态角。比如俯仰角可以用 θ = atan2(-f_x, sqrt(f_y² + f_z²)) 这类式子求出来。这个式子成立的前提是，加速度计测到的比力只包含重力分量。一旦载体晃动，比如船体在波浪中做低频起伏，加速度计会额外测到运动加速度；再比如车载安装位置靠近发动机，高频振动会直接叠加在输出上。这些干扰反映到姿态角上，就是水平姿态的抖动幅度会远超MEMS器件本身的噪声水平。
再看方位对准。传统罗经对准的物理基础是利用陀螺敏感地球自转角速度在水平面的投影分量。地球自转角速率约15度/小时，在北纬45度处其水平分量大概在10.6度/小时，换算成每秒约0.0026度/秒。而消费级MEMS陀螺的零偏不稳定性通常在10度/小时到100度/小时量级，即使是战术级MEMS也只能做到1度/小时左右。也就是说，地球自转信号本身就淹没在陀螺零偏的“底噪”里，静态下靠MEMS对准方位就已经很吃力，晃动一叠加角振动，信噪比进一步恶化，用传统罗经方案基本行不通。
1.3 技术选型：为什么最终走了“滤波估计”这条路
既然解析法在晃动下不可靠，那就得换思路。工程上有几种可选方案：
- 对原始数据做低通滤波后再走解析对准。这个方案实现简单，能滤掉一部分高频晃动，但对付低频大幅摆动几乎没有效果。船在波浪里的横摇周期经常是5到10秒，这个频段的运动无论如何滤波都会残留，本质上是治标不治本。
- 外部辅助传感器直接给定初始姿态。比如用磁力计辅助给方位角，用倾角传感器辅助给水平姿态。这种方式工程上可行，但对系统集成度有要求，而且磁力计本身容易受载体磁环境干扰，属于把一个问题换成另一个问题。
- 间接法卡尔曼滤波，把姿态误差作为状态量在线估计。这种方式的好处是模型里可以显式考虑加速度计零偏、陀螺零偏，滤波过程天然具备对时变扰动的抑制能力，而且对准结束后误差状态可以无缝衔接给导航解算。缺点是算法复杂度高，需要对滤波模型有深入理解。
最终选择的是第三种方案，理由很实际：姿态解算模块和导航滤波器共用同一套误差模型，从对准到导航不需要切换算法架构，后续扩展组合导航也方便。而且用滤波做自对准，核心工作从“算一个姿态角”变成了“调好一个滤波器”，这个转变对晃动工况特别有利，因为滤波器天然有“用一段时间的数据来平滑估计”的特性。
提示：如果载体晃动幅度非常大（比如姿态变化超过正负30度），建议在对准之前先做一个粗略的姿态初始化，把滤波器的初始失准角控制在小角度范围内，否则线性化误差会严重影响收敛。
2. 系统建模与核心原理
2.1 坐标系定义和变量说明
滤波模型的第一步不是写方程，而是把坐标系和变量约定清楚。很多工程悲剧都是从坐标系定义混乱开始的。我这里统一用导航坐标系n系（当地水平坐标系，x北y东z地，或者x东北天y北西z上，你需要统一）和载体坐标系b系（x前y右z下）。姿态矩阵 C_b^n 表示从载体坐标系到导航坐标系的变换。
需要估计的误差状态包括三个平台失准角（即姿态误差角在n系下的投影分量）φ、陀螺零偏ε、加速度计零偏∇。如果只做对准不涉及速度估计，速度误差δv也经常被引入状态向量，因为它可以通过“零速观测”把加速度计零偏的可观测性建立起来。
状态向量可以写为：
x = [φ_x, φ_y, φ_z, δv_x, δv_y, δv_z, ε_x, ε_y, ε_z, ∇_x, ∇_y, ∇_z]^T
这里 φ 是平台失准角，δv 是速度误差，ε 是陀螺零偏（角速率量纲），∇ 是加速度计零偏（比力量纲）。如果你只想做纯姿态对准，也可以省略速度误差，只保留9维状态，但实际效果往往不如引入速度误差好，后面我会说明原因。
2.2 误差状态方程的建立
在这个模型里，姿态误差方程的线性化形式为：
φ̇ = -ω_in^n × φ + δω_ie^n + δω_en^n - C_b^n ε
在静基座（或晃动基座）对准场景中，载体相对地球没有显著运动速度，所以 δω_en^n 项可以近似忽略，δω_ie^n 表示地球自转角速度的估计误差，一般在地理纬度准确已知时近似为零。这样方程简化为：
φ̇ = -ω_in^n × φ - C_b^n ε
这个式子的物理含义很重要：失准角的变化率一方面受当前的失准角和地球自转角速度（n系下垂直于地球自转轴的分量）耦合影响，另一方面受陀螺零偏驱动。陀螺零偏越大，失准角会持续累积，这正是为什么滤波要把陀螺零偏作为状态估计出来的原因。
速度误差方程的简化形式：
δv̇ = - (C_b^n f^b) × φ + C_b^n ∇
这个方程说明两件事：第一，失准角会影响比力投影，进而造成速度误差累积；第二，加速度计零偏直接耦合进速度误差。之所以引入速度误差状态并用“零速观测”来更新，本质上是利用速度误差作为桥梁，把加速度计零偏和失准角同时激励起来。
2.3 观测方程与可观测性分析
滤波器的观测用得很直接：载体在晃动基座上并没有整体平移运动（或者虽有平移但我们可以通过运动检测判断出“准静态”时段），所以真实速度为零。观测方程为：
z = δv + v_noise
这个观测量的妙处在于，它间接地把水平失准角和加速度计零偏的线性组合观测出来了。从可观测性理论角度看，静基座对准时系统的可观测矩阵秩为5，也就是说15维（或12维）状态里只有5个线性组合是可观的——三个水平失准角中的两个、加速度计零偏的两个水平分量、方位失准角是不可观的。这个结论解释了为什么纯静基座MEMS对准无法获得高精度方位：不是滤波算法不行，是物理上就不可观测。
但要注意，“晃动”本身反而会提升可观测性。当载体在晃动时，加速度计测量到的比力方向在变化，这就相当于给系统增加了一个激励输入，使得原本不可观的方位失准角在某种条件下变得部分可观。我在实测中确实观察到，在持续小幅角晃动的条件下，方位角的收敛速度和稳定性明显优于完全静止工况，就是这个原理。
2.4 水平失准角与方位失准角的收敛特性差异
水平失准角的收敛速度通常很快。原理上可以这样理解：初始水平失准角φ_y会在水平加速度计上引起一个重力投影误差 g·φ_y，这个误差通过速度观测和加速度计零偏估计很快就能被分离出来。实测中，在晃动不太剧烈的情况下，俯仰和横滚失准角在20到60秒内就能收敛到0.1度以内。
方位失准角则完全是另一回事。前面已经说过，MEMS陀螺无法可靠敏感地球自转，方位角的可观性主要来源于载体晃动引起的比力方向变化。如果晃动是绕天向轴的小幅摇摆，它能给方位角提供一定的激励，但收敛速度和精度高度依赖晃动幅度和频率。如果晃动是纯平动或者绕水平轴的摆动，对方位角的可观测性提升就非常有限。
所以实操里要有一个正确预期：水平姿态可以在晃动下较好收敛，方位角能不能对准、能对准到什么程度，很大程度取决于“你所在环境的激励条件”。这也解释了为什么很多产品在Motion Rate模式下，整机晃动几下方位角反而会跳变，因为激励被算法误解为真实转动了。
提示：测试方位角收敛效果时，不要只盯着滤波器输出的曲线，要同时记录安装位置的振动频谱。如果振动能量集中在100Hz以上，说明是高频振动，低通滤波能处理；如果晃动能量集中在0.5到2Hz的横摇纵摇，这才是真正影响MEMS对准的“元凶”。
3. 滤波器设计与参数整定
3.1 状态向量与初始协方差配置
滤波模型确定后，第二个关键步骤是配置初始协方差矩阵。这个矩阵反映的是对自己初始估计不确定度的信任程度。失准角的初始不确定度可以根据你的粗略初始化方式来确定：如果上电后用加速度计静态解析求了水平角，水平失准角的初始标准差可以给0.1到0.5度；方位角如果没有任何参考（没有磁力计也没有外部航向），建议给到5到10度甚至更大，别给太乐观。
陀螺零偏的初始标准差可以按MEMS器件手册里的零偏重复性来给，比如消费级可以给0.1度/秒，战术级给0.01度/秒。加速度计零偏的初始标准差同理，参考零偏重复性指标，常见0.005g到0.02g之间。
初始协方差设得太小会“骄傲”，导致滤波增益一直很低，后续量测很难修正初始误差；设得太大则前几十个历元增益过高，输出波动大，但最终也能收敛。我习惯上初始协方差宁可给大一些，让滤波前段快速收敛，代价是输出曲线前几十秒偏“躁”，通过事后平滑可以处理。
3.2 过程噪声Q矩阵的工程标定
过程噪声Q是滤波器里最需要花心思调的参数。它反映的是你对状态方程模型本身的不信任程度，也就是陀螺白噪声、加速度计白噪声以及随机游走等因素对状态演化的影响强度。
姿态误差对应的Q块主要由陀螺角度随机游走（ARW）决定。如果陀螺的噪声密度是0.01度/s/√Hz，带宽200Hz，等效ARW约为0.14度/√h。速度误差对应的Q块由加速度计速度随机游走决定，同时也要把晃动带来的等效振动干扰算进去。
这里有一个工程技巧：把“晃动”当作额外过程噪声来吸收。具体做法是，在晃动环境下适当放大速度误差对应的Q值，比如把等效加速度噪声密度从静止状态下的0.005g增加到0.02g甚至更高。这样滤波器就会被“劝退”——它知道速度观测量里含有大量不能靠状态估计解释的噪声，就不会强行去跟振动，姿态输出反而更稳。很多人滤波器发散，就是Q给得比实际噪声小了一个量级，滤波器以为自己很准，结果疯狂跟踪振动。
陀螺零偏和加速度计零偏对应的是随机游走率，一个经验范围是：陀螺零偏随机游走近似为陀螺零偏稳定性的千分之一到百分之一（按秒记），加速度计同理。这个值不要给零，否则状态一旦收敛就永远改不过来，也无法应对温度漂移导致的零偏缓变。
3.3 量测噪声R矩阵的选取与自适应
量测噪声R在零速观测里其实很微妙。如果R设得小，说明你非常相信“速度为零”这个观测，滤波器会强约束速度误差趋近为零，这有助于快速收敛水平姿态，但同时也会让速度观测里的晃动分量通过状态耦合污染姿态估计。如果R设得大，滤波收敛变慢，但鲁棒性好。
我的经验是，R不要用固定值，最好做成可变的：用加速度计输出的高频分量作为晃动强度的指标，晃动大时把R调大，晃动小时把R调小。这个技术称为自适应量测噪声，实现起来并不复杂，可以简单用一个滑动窗口计算比力模值相对重力模值的偏差：
ε_a = std(|f| - g) 在最近2到3秒内的标准差
然后令 R = R_0 + k·ε_a²，其中 k 是需要标定的比例系数。这样在平稳时段滤波快速收敛，在晃动时段滤波稳定不蹦。项目实测中，这个自适应规则让我在晃动台实验里的横滚角均方根误差降低了约40%，是提升最明显的一次改动。
3.4 对准时间与收敛判据
什么时候算“对准完成”？很多工程人员会凭经验拍个固定时间，比如“对准60秒”，但实际晃动条件下收敛速度是时变的，固定时间容易造成两个问题：一是晃动剧烈时60秒根本不够，强行进入导航会带着残余姿态误差；二是晃动轻微时可能30秒就够了，固定等待浪费了使用者的时间。
更合理的做法是一个基于协方差的收敛判据。卡尔曼滤波输出的协方差矩阵P对角元素代表了状态估计的不确定度，可以直接用姿态失准角对应的子块来判断：当水平失准角的标准差连续20秒低于预设阈值（比如0.2度），且方位失准角标准差低于预设阈值（比如2度），就判定对准完成。这个判据不需要额外开发，P矩阵本身就是现成的，写几十行代码就能实现。
提示：收敛判据里的标准差阈值要和你最终应用的导航精度需求挂钩，不必追求越低越好。如果后端回灌的初始姿态误差会被组合导航滤波器二次估计，那么对准精度要求可以适当放宽，重点放在“不残留发散性误差”上。
4. 实操流程与代码实现要点
4.1 传感器数据采集与预处理
数据采集这一步看似简单，却决定了对准效果的上限。我踩过最大的坑是采样延时和时钟抖动。MEMS惯导模块一般通过串口或SPI输出数据，在嵌入式系统里如果软件架构设计不好，中断响应不确定，数据时间戳会出现毫秒级抖动。对于静态对准，这个抖动影响不大，但在晃动环境下，时间错位等效于在角速率信号上叠加了一个和角速率变化率成正比的误差，晃动越剧烈误差越大。
正确的做法是：在数据采集线程里用硬件定时器或IMU内部采样时钟给每一帧数据打时间戳，而不是数据到达时刻才打。如果传感器支持数据就绪中断，务必用中断信号同步采样时刻。另一个容易被很多人忽略的点是，尽量避免在操作IMU读取时做高负载操作（比如日志写入SD卡），否则数据流会出现间隙，卡尔曼滤波的时间更新步长不一致，模型精度会受影响。
预处理方面的建议：
- 消除常值零偏：上电后静置2到3秒取平均，作为初始零偏扣除。不要用出厂标定值替代，MEMS零偏随温度变化很大，以现场实测为准。
- 低通滤波：如果晃动主要是低频运动，可以用截止频率10到20Hz的一阶IIR低通滤波，滤掉高频振动，改善信噪比。如果晃动本身能量就在1到5Hz，低通滤波作用有限，主要靠滤波模型吸收。
- 重力模值检测：在准静态时段可以用加速度计模值判断初始是否水平，但晃动时要避免直接用模值判断“静止”，否则误判会让解析初始化给出错误姿态。建议用分段方差检测替换简单阈值。
4.2 运动状态检测：区分准静态和晃动
在晃动环境下，一个很实用的中间模块是运动状态检测。它输出的不是模棱两可的“静/动”状态，而是一个连续的运动强度指标，用来调节滤波器的R矩阵和对准完成判据。
实现思路是这样的：维护一个长度为N（比如100个采样点）的滑动窗口，计算窗口内加速度计模值的标准差 σ_a 和陀螺仪角速率模值的均值 ω_mean。
计算方式：
σ_a = std(|f|_g - g) 归一化到重力单位
ω_mean = mean(|ω|) 单位rad/s
当 σ_a 小于0.005g 且 ω_mean 小于0.5度/秒时，认为是准静态；当 σ_a 超过0.05g 或 ω_mean 超过5度/秒时，认为是明显晃动；中间状态用线性插值折算成一个0到1之间的运动强度系数 s。然后把 R 矩阵的基准调整量设成 R_s = R_0 / (1 + 10s²)，也就是说晃动越厉害，量测越不被信任。
这里也顺带解决了一个问题：实际系统往往混合了“静-晃-静”多个阶段，单纯靠滤波算法自适应很难快速响应状态突变，而运动检测几乎是零延迟的。把它和自适应R结合，就有了一种“前馈+反馈”的路子。
4.3 卡尔曼滤波主循环的关键步骤
直接给一段可运行的伪代码，方便你理解主循环结构。这一段我按离散卡尔曼滤波器实现，时间更新与量测更新分开写。
import numpy as np # 状态: [phi(3), dv(3), eps(3), grad(3)] n = 12 # 初始协方差，单位需统一（弧度, m/s, rad/s, g） P = np.diag([...]) # 根据你的初始不确定性填 # 状态转移矩阵在时间更新里由动力学计算， # 这里简化为零阶保持形式（小步长近似） def time_update(F, G, Q, dt): global P, x # F: 状态转移矩阵, Q: 离散过程噪声矩阵 x = F @ x P = F @ P @ F.T + G @ Q @ G.T def meas_update(H, R_dyn, z): global P, x # H: 观测矩阵, R_dyn: 自适应量测噪声 S = H @ P @ H.T + R_dyn K = P @ H.T @ np.linalg.inv(S) y = z - H @ x # 零速观测残差 x = x + K @ y P = (np.eye(n) - K @ H) @ P # 主循环 for each sample t: # 计算运动强度 s s = compute_motion_intensity() # 计算状态转移矩阵 F（用当前姿态、陀螺输出线性化） F, G = build_dynamics(gyro, accel, dt) # 过程噪声 Q：固定部分 + 与晃动强度相关的部分 Q = base_Q + s * added_Q time_update(F, G, Q, dt) # 零速观测，R_dyn 随 s 调整 z = -v_acc # 通常是导航系积分速度的负值 R_dyn = R_0 / (1 + 10 * s**2) meas_update(H_zero_speed, R_dyn, z)有几个细节要注意：一是状态转移矩阵 F 里的姿态误差耦合项需要用到陀螺输出的角增量，步长 dt 必须用真实的时间间隔；二是观测矩阵 H 在零速观测里是选择 δv 对应三个元素的0/1矩阵，注意维度匹配；三是滤波循环里不要天天做矩阵求逆（虽然小维度无所谓），到了嵌入式环境要提前做 Cholesky 分解预计算，避免主循环耗时波动。
4.4 对准结果的评估与验证方法
做了滤波对准，怎么判断效果？不能只看自己滤波器输出的曲线，因为滤波器存在“自证清白”的问题——你估计的姿态可能平滑稳定，但真实的误差你并不知道。我常用的验证方法有四种，按可靠性排序：
- 高精度参考对比。在转台或者带高精度光纤陀螺的基准板上做同步对比，这个最客观，但也不是每个团队都有条件。
- 重复性评估。在相同位置做多次上电对准，统计对准完成后姿态角的标准差。这个指标反映了算法和传感器的综合重复性，如果标准差超过程序中协方差给出的不确定度，说明滤波过于自信。
- 对准后导航一致性。对准完成后静置或简单运动几分钟，看导航解算出的位置漂移方向是否与初始姿态误差吻合。一个明显的偏航初始误差会导致位置漂移的横向分量，通过这个可以反推初始方位精度。
- 冗余传感器交叉验证。如果系统里还有磁力计或GNSS双天线测向，可以在对准结束后对比方位角，差量对应MEMS对准的方位误差。这个方法在现场最实用。
晃动条件下的验证尤其要注意：评估时选择一段晃动较弱的时间窗口读数值，不要用剧烈晃动阶段的瞬时输出，那时姿态角的抖动是物理晃动本身，不代表对准误差。
提示：不要用“对准结束后姿态曲线是否平稳”来评估对准精度。滤波输出平稳只能说明滤波器收敛到了某个值，不代表这个值就是真值。一定要有独立参考或者至少做重复性统计。
5. 常见问题排查与实操心得
5.1 对准发散：Q/R失配的典型表现
遇到最频繁的问题就是滤波器发散，表现形式是姿态角在某一次量测更新后突然跳变到离谱值，或者协方差P对角元素持续增大不收敛。排查顺序我总结为“由内向外”四步：
- 检查时间步长。主循环里dt是否正常？如果日志记录出现时间戳跳变，dt可能是0.01秒瞬间变成1秒，状态转移发散那是一瞬间的事。
- 检查状态转移矩阵线性化。姿态误差方程里的叉乘项方向是否写反？n系和b系之间的转换规律是否统一了？我见过很多人“东北天”和“北东地”混合使用，却在公式里不做转换，结果姿态误差互相耦合，表现为特定方向发散。
- 检查Q和R的量纲统一。状态里有角度（弧度）、速度（m/s）、角速率（rad/s）、比力（m/s²或g），P的初值、Q、R都必须使用同一套单位。很多发散就是单位混乱导致的，比如把g写成了9.8却没有统一到米制，结果误差放大了10倍。
- 检查晃动量级与R的匹配。如果现场晃动远大于R的设定，滤波器会强行把振动当速度误差来修正，每次修正都会给姿态注入噪声，表现就是姿态角抖动幅度异常放大。这时优先把自适应R调大，而不是继续压缩量测噪声。
实操中遇到姿态输出剧烈跳变，90%的情况不是算法数学有问题，而是以上四类工程细节出了问题。先加日志可视化中间量，再动手改，比盲目调参高效得多。
5.2 方位收敛受限：MEMS自对准的物理边界
必须坦白说，MEMS惯导在纯惯性条件下对方位角的对准能力有物理上限。地球自转信号太小，MEMS陀螺的漂移太大，即使滤波模型再精致，在无外部信息时方位角估计的本质是一个“可观测性很弱”的状态。你可能会看到某些产品号称“静态对准航向精度0.5度”，那基本是加了磁力计辅助或者用了更高精度的光纤陀螺，对纯MEMS方案不具备参考意义。
我实测过某款战术级MEMS模块，零偏不稳定性约1度/小时，在完全静止条件下纯惯性对准半小时，方位角依旧有2到3度的跳变；而在持续小幅晃动（约正负10度、频率0.2Hz）条件下，方位角虽能收敛到某个值，但重复性在1到2度水平。这个数字基本代表了当前MEMS纯惯性对准的天花板。如果你的项目要求更高的方位精度，建议尽早引入辅助手段：磁力计做初始航向参考、GNSS双天线测向、或者视觉/激光匹配定位在运动后修正。不要把希望全押在算法上。
另外提醒一点，很多MEMS模块内置的姿态解算库在对准时会用“等待罗经效应”的方式慢慢稳定航向，那个本质是磁力计辅助加长时间平均。如果你在无磁环境里测试（比如某些建筑内部），这个航向值是不可靠的。现场测试前确认好磁环境。
5.3 杆臂振动与减振处理的实战经验
机械安装对MEMS自对准的影响经常被低估。实际项目中有一个非常典型的坑：MEMS模块安装在结构件上，结构件本身有谐振频率，比如金属支架在80Hz附近有共振峰，电机或发动机的振动经过结构传播后，谐振频率处的振动幅值会被放大数十倍。加速度计输出里混入这个频段的强振动后，即使低通滤波，也会因为滤波器的相位延迟造成与陀螺信号不一致的误差，从而影响姿态解算。
我的处理习惯分三层：
- 机械减振优先。在模块和安装面之间加一层硅胶减振垫或航天级减振器，这比任何数字滤波都有效。实测中，一个合适的减振垫可以把高频谐振幅值降低到原来的十分之一。
- 数字滤波匹配。对加速度计和陀螺使用截止频率和相位特性一致的滤波器，避免二者群延迟不一致。如果条件允许，给IMU原始输出做相同的低通处理，再进入姿态解算和对准滤波。
- 激励检测。在运动检测模块里对特定频段（比如你安装支架的谐振频率）做带通检测，如果该频段能量持续过高，提示后端增大R或暂缓对准判据的通过。
杆臂效应的处理也要注意。如果MEMS安装位置距离载体晃动中心有一段距离，在晃动时加速度计会测到向心加速度和切向加速度，这个分量与运动角速率强耦合。滤波模型里如果不加杆臂补偿，水平对准就会残留一个与晃动强度相关的误差。一个实用的补偿式是：
f_compensated = f_measured - (ω × (ω × r)) - (α × r)
其中 r 是加速度计相对载体参考点的杆臂向量，α 是角加速度，需要从陀螺输出差分得到。虽然杆臂补偿不总是必须的，但只要安装位置偏离参考点超过去十几厘米，在剧烈晃动时建议补上。
5.4 晃动台实验的实测记录
分享一组项目实测数据，便于你有个量级概念。测试环境是一个六自由度晃动台，模拟船体在中等海况下的横摇和纵摇：横摇正负15度、周期6秒，纵摇正负5度、周期8秒，另有小幅艏摇正负3度、周期12秒。MEMS模块为战术级，零偏不稳定性约2度/小时，采样率200Hz。
测试流程：晃动台起振后，模块保持静止安装，让算法在晃动下自对准，记录120秒数据，然后参考真值由高精度光纤惯导同步提供。重复了10次。
水平姿态结果：俯仰角对准误差在收敛后约0.05到0.1度，横滚角误差约0.1到0.2度，收敛时间约30到40秒。对比完全静止条件下精度（约0.03度），晃动引入的额外误差可以接受。
方位角结果：重复性标准差约1.5度，均值偏差约0.8度。这个结果比静态对准反倒略好，原因正如2.3节所述，晃动提供的持续激励增强了方位角的可观测性，但代价是收敛时间被拉长到约100秒以上。
从这个实验看到的实际现象是，晃动对水平对准的破坏可以通过合理设置滤波器参数控制在可接受范围，而方位角对准在晃动条件下反而能获得额外可观测性，前提是你忍受得了“慢”和“不完全可重复”这两个特点。
还有一个让很多人意外的小发现：在对准算法里把初始速度误差状态 δv 直接置为高置信零，整体对准速度会变慢。因为这样相当于“告诉”滤波器速度绝对可信，滤波器不敢把速度误差用来激励零偏估计，结果加速度计零偏收敛就拖延了。工程上我给初始速度误差一个不小的不确定度（标准差0.05到0.1m/s，即使实际静止），反而能在保证精度的同时让对准更快完成。这属于模型与实现的“微调权衡”，但实际收益很实在。
提示：做晃动台实验时，一定要记录晃动台的真实运动参数和参考姿态真值，因为后期排查滤波器问题时，你首先需要回答的问题是“那是滤波器的问题，还是测试环境的问题”。
关于这个题目能聊的实操细节其实还有很多，比如温度漂移补偿、多IMU阵列融合、以及用反向平滑进一步压缩对准时间等等，但核心模型和工程处理方法，就是以上这些。按这套建模思路去搭滤波器，再根据现场晃动强度去整定Q和R，基本能覆盖大多数MEMS晃动自对准的实际需求。如果你在具体调参时遇到什么奇怪的现场表现，记住一个原则：先把物理环境影响量化出来，再回头修改模型和参数，永远比盲调有效。
