1. 从“事后诸葛亮”到“全局最优”为什么需要最优平滑在捷联惯导系统SINS的实际工程应用中我们常常会遇到一个令人头疼的困境实时滤波比如卡尔曼滤波给出的导航参数其精度往往无法满足某些高要求的应用场景。比如在飞行器着陆后的航迹复现、测绘数据的后处理或者是对一段已经结束的飞行任务进行高精度分析时我们手头不仅有惯导的实时输出还有任务结束后获取的、精度更高的外部参考信息如差分GPS的精密轨迹。这时候我们不禁会想既然已经知道了“未来”更准确的信息能不能回过头去把整个任务过程中的导航数据“重算”一遍得到一个比实时滤波结果更优、更平滑的轨迹呢这个想法就是“最优平滑”算法的核心。它本质上是一种“事后处理”技术但绝非简单的数据平均或曲线拟合。最优平滑算法利用了整个时间区间从起点到终点的全部观测信息对系统在每一个历史时刻的状态进行重新估计。如果说实时卡尔曼滤波是一个“边走边看”的向导只能根据当前和过去的信息做出判断那么最优平滑就是一个“复盘大师”站在全局的终点审视整个过程的全部信息从而得出对历史状态更公允、更准确的评价。在惯性导航领域这种能力对于提升事后分析精度、校准传感器误差模型、乃至评估系统真实性能都至关重要。2. 平滑算法的家族谱系固定点、固定滞后与固定区间在深入最优平滑的具体公式之前我们有必要先理清平滑算法的几种主要类型。它们根据处理数据的方式和“延迟”程度不同适用于不同的场景。2.1 固定点平滑想象一下你只对历史上某一个特定时刻比如火箭点火的那一秒的状态特别感兴趣希望得到关于这个时刻最精确的估计。固定点平滑就是干这个的。它不断融合新的观测数据但所有计算都只为了优化对那一个固定时间点的状态估计。随着数据越来越多对这个“点”的估计会越来越准。这种方法计算量相对集中但只针对单个时刻。2.2 固定滞后平滑这种平滑方式更贴近一种“准实时”的应用。它允许一个固定的、较小的处理延迟比如滞后1秒。系统在t时刻不仅使用t时刻及之前的观测还能利用未来一小段时间到tL时刻L为固定滞后的观测来估计t时刻的状态。这相当于一个“带了未来一小段记忆”的滤波器其输出比纯实时滤波更平滑、更准确但存在固定的时间延迟。在有些对实时性要求不是极端苛刻但需要更好瞬时精度的场合如某些制导环节会考虑使用。2.3 固定区间平滑这是我们今天讨论的重点也是捷联惯导后处理中最常用的一类。顾名思义它处理的是一个固定的、完整的时间区间[t0, tN]。当所有区间内的观测数据都已采集完毕后算法开始工作目标是利用从t0到tN的全部信息重新估计出该区间内每一个离散时刻tk的状态。这完美契合了飞行数据事后分析、轨迹后处理等场景的需求。最优平滑算法如经典的Rauch-Tung-Striebel (RTS) 平滑器就是固定区间平滑的杰出代表。注意对于捷联惯导我们通常处理的是离散时间系统。因此下文的最优平滑算法也将在离散时间框架下展开。连续时间的平滑理论虽然优美但最终落地实现仍需离散化。3. RTS平滑器前向滤波与后向平滑的优雅结合Rauch-Tung-Striebel (RTS) 平滑器是固定区间平滑中最著名、应用最广泛的算法之一。它的核心思想非常直观分为两个清晰的阶段一个前向过程和一个后向过程。3.1 前向过程标准卡尔曼滤波第一阶段就是沿着时间顺序从起始时刻t0运行到结束时刻tN执行一遍标准的离散时间卡尔曼滤波。我们需要完整地保存这个前向滤波过程中的所有“中间产物”包括状态估计前值 (x_k^-)和后值 (x_k^)分别代表在k时刻融合观测前和融合观测后的状态估计。估计误差协方差阵前值 (P_k^-)和后值 (P_k^)表征对应状态估计的不确定度。状态转移矩阵 (Φ_{k-1})和过程噪声协方差阵 (Q_{k-1)由系统的动力学模型决定。这些数据需要按时间顺序存储下来它们是后向平滑的“原料”。前向滤波为我们提供了基于“过去和当前”信息的最佳估计轨迹。3.2 后向过程信息反向传播与融合第二阶段从最终时刻tN开始逆着时间方向回溯到初始时刻t0。这个后向过程并不重新处理观测数据而是巧妙地利用前向滤波保存的结果进行信息的反向传播与融合。RTS平滑器的后向递推公式是其精髓所在。假设我们已经通过前向滤波得到了所有时刻的x_k^, P_k^, x_k^-, P_k^-。初始化平滑过程的终点就是前向滤波的终点。因此平滑器在最终时刻tN的状态和协方差直接继承自前向滤波的后验值x_N^s x_N^P_N^s P_N^这里上标s代表平滑后的估计。后向递推 (对于 k N-1, N-2, ..., 0) 这是核心步骤。为了从k1时刻的平滑估计x_{k1}^s 计算出k时刻的平滑估计x_k^s RTS平滑器引入了一个关键的“平滑增益”矩阵C_kC_k P_k^ Φ_k^T (P_{k1}^-)^-1这个增益C_k衡量了前向滤波在k时刻的后验估计误差与在k1时刻的先验估计误差之间的相关性。它决定了有多少“未来的信息”需要反向修正“过去的估计”。然后平滑估计及其协方差按如下公式更新x_k^s x_k^ C_k (x_{k1}^s - x_{k1}^-)P_k^s P_k^ C_k (P_{k1}^s - P_{k1}^-) C_k^T公式解读(x_{k1}^s - x_{k1}^-)这一项是“未来信息的修正量”。x_{k1}^s是融合了全程信息后对k1时刻的更优估计x_{k1}^-是前向滤波仅用k1时刻之前信息做出的预测。两者的差值就包含了从k1时刻及之后观测中获得、但前向滤波在k时刻无法获知的信息。平滑增益C_k将这个来自未来的修正量以最优最小均方误差意义下的方式传递并应用到k时刻的估计x_k^上从而得到平滑估计x_k^s。协方差更新公式P_k^s则反映了经过平滑后状态估计不确定度的降低。理论上平滑误差协方差P_k^s一定会小于或等于前向滤波的后验误差协方差P_k^这直观地体现了融合全局信息带来的收益。通过这个从后向前的递推我们最终得到了整个时间区间[t0, tN]内每一个时刻的平滑状态估计{x_0^s, x_1^s, ..., x_N^s}及其对应的误差协方差{P_0^s, P_1^s, ..., P_N^s}。4. 在捷联惯导中实施RTS平滑模型、状态与实操考量将RTS平滑算法应用到捷联惯导系统的后处理中需要将上述理论框架与SINS的具体模型结合起来。4.1 状态向量的定义平滑算法处理的状态向量就是前向卡尔曼滤波所采用的状态向量。对于一个典型的、与GPS松组合的SINS其状态向量通常包括导航参数误差位置误差纬度、经度、高度、速度误差北、东、地、姿态误差角滚转、俯仰、航向。惯性传感器误差陀螺零偏3轴、加速度计零偏3轴。有时还会包含标度因数误差和安装误差角等更高阶的误差项。外部辅助传感器误差如与GPS组合时GPS接收机钟差、钟漂等。因此状态向量的维度通常在15维以上。平滑算法将对这些误差状态进行全局最优估计。4.2 前向滤波模型的建立这是整个平滑处理的基础也是工程上的关键。前向滤波必须采用与真实系统动力学尽可能匹配的模型。状态方程基于惯性导航误差方程ψ角方程或Φ角方程等建立描述导航误差和传感器误差的传播规律。状态转移矩阵Φ需要根据当前导航参数位置、速度和惯性器件误差模型准确计算。过程噪声协方差阵 Q它主要建模那些未被状态方程描述的扰动比如陀螺和加速度计的随机游走角随机游走和速度随机游走。Q阵的设置至关重要过小会导致滤波器对模型过于自信而发散过大会使平滑效果打折扣。通常根据惯性器件的 Allan 方差分析结果来标定。量测方程根据组合模式位置、速度、姿态组合建立。量测噪声协方差阵R需要根据辅助传感器如GPS的实际精度来合理设置。一个准确、稳定的前向滤波是高质量平滑结果的前提。如果前向滤波本身由于模型不准或噪声设置不当而性能不佳那么平滑也只是在“优化”一个有问题的基准效果有限。4.3 数据存储与后向递推实现在计算机上实现时需要注意内存与存储对于长时间任务例如数小时飞行前向滤波的所有中间结果x_k^, P_k^, x_k^-, P_k^-,Φ_k 可能还有C_k都需要保存。P阵是方阵维度是状态维度的平方。对于一个18维状态每个P阵就有324个元素。这会对存储空间提出要求。在实际编程中可以考虑只存储必要的矩阵如P_k^和P_{k1}^-的逆或直接存储计算好的C_k或者使用文件缓存策略。数值稳定性在后向递推中需要计算(P_{k1}^-)^-1即对前向滤波的先验协方差矩阵求逆。必须确保P_{k1}^-是正定且良态的否则求逆会失败或引入巨大数值误差。在前向滤波中使用平方根滤波算法如SRCKF, UD分解滤波可以更好地保证协方差矩阵的数值特性为后向平滑提供更稳定的输入。时间同步确保前向滤波过程中所有数据IMU数据、GPS观测数据的时间戳严格同步并且在后向平滑时时间索引能够正确对应。5. 平滑效果评估与工程实践中的陷阱当我们运行完RTS平滑算法得到一条平滑后的轨迹后如何评估其效果在实践中又会遇到哪些坑5.1 效果评估与参考真值及前向滤波对比最直接的评估方法是与更高精度的“参考真值”进行对比。在事后处理场景中这个真值可能来自更高精度的差分GPS/精密单点定位(PPP)解。激光跟踪仪、全站仪等地面精密测量数据。经过严格事后处理的多源融合导航结果如紧组合平滑。 通过计算平滑前后位置、速度、姿态误差的统计量均值、标准差、RMS可以定量评估平滑算法带来的精度提升。另一种评估是内部对比将平滑轨迹x_k^s与前向滤波的后验轨迹x_k^进行对比。理想情况下平滑轨迹应该更平滑突变和毛刺更少。你可以绘制同一误差状态的时间序列图平滑后的曲线理应更贴近“真值”趋势且协方差P_k^s应普遍小于P_k^。5.2 常见陷阱与实操心得“垃圾进垃圾出”这是最根本的原则。如果前向滤波的模型严重失准或者观测数据质量极差GPS频繁失锁、多路径效应严重那么平滑算法无力回天。它只能优化基于现有模型和数据的估计无法创造信息。确保高质量的前向滤波输入是平滑成功的先决条件。过程噪声Q与量测噪声R的权衡调节比这两个参数决定了滤波器更相信模型预测还是更相信观测。在前向滤波中我们需要调节它们以获得良好的实时性。对于平滑由于利用了全部观测理论上可以对Q和R进行更精细的调整甚至可以采用自适应滤波技术在线估计它们。一个实用的技巧是在确保前向滤波不发散的前提下可以尝试略微减小Q让模型更“自信”这样平滑器在利用未来信息修正时效果可能会更显著。但这需要反复试验和验证。计算复杂度与实时性RTS平滑是典型的“批处理”算法需要存储所有时刻的矩阵并且后向递推涉及大量的矩阵乘法和求逆运算。对于状态维度高、数据量大的任务计算负荷不容忽视。它不能用于实时导航。在工程实现时需要优化矩阵运算代码考虑使用数值稳定的线性代数库。端点效应理论上在平滑区间的两端起点和终点平滑估计与滤波估计是相同的x_0^s x_0^,x_N^s x_N^。但在实际中由于模型和数据的局限性起点和终点附近的平滑效果可能不如中间段稳定。对于特别关心起点精度的应用如对准点需要谨慎评估。验证平滑结果的“合理性”平滑后的轨迹应该物理上更合理。例如车辆在静止阶段的速度平滑估计应该更接近于零飞行器的姿态变化应该更连续平滑不应出现高频抖动。将平滑后的导航参数位置、速度、姿态反算回去与原始的IMU数据进行比较检查其一致性也是一个有效的验证手段。在我处理过的多个无人机测绘和车载组合导航数据后处理项目中RTS平滑几乎是一个标配环节。有一次我们处理一段长达2小时、GPS信号在城市峡谷中频繁衰减的车载数据。前向滤波的轨迹在城市区域跳动明显。经过RTS平滑后不仅轨迹变得光滑连贯而且通过与事后采购的精密星历解算结果对比平面位置精度提升了约40%。这让我深刻体会到在拥有完整数据集的情况下最优平滑算法是一种成本极低仅增加计算而收益显著的精度提升手段。它就像给导航系统一次“重新思考”的机会利用全局信息去修正每一个历史瞬间的判断最终得到一条更接近真相的轨迹。