2. 高动态运动建模

高动态导航滤波的第一步不是选滤波器，而是把飞行器“怎么动”写成状态方程。若运动模型与真实机动不匹配，再好的鲁棒估计也只能在错误的状态转移上做补偿。本节围绕三个层次展开：六自由度刚体运动如何构成状态量的骨架，机动加速度如何被建模为过程噪声或确定性输入，以及高动态如何改变状态方程的数值特性与滤波可观测性。

一、六自由度运动：状态量的物理骨架

飞行器六自由度指质心三轴平动与绕质心三轴转动。在导航滤波中，常用状态量包括位置 p = [x, y, z]、速度 v = [vx, vy, vz]、姿态四元数 q = [qw, qx, qy, qz]、角速度 ω = [ωx, ωy, ωz]，以及加速度计零偏 ba 和陀螺零偏 bg。若采用15维误差状态，可写为 δx = [δp, δv, δθ, δba, δbg]。这里先约定坐标系：本文采用前-右-下（FRD）机体系与北-东-地（NED）导航系，重力矢量在 NED 下为 [0, 0, g]，姿态旋转由机体系到导航系。

平动方程在导航系下为 ṗ = v，v̇ = C_b^n (a_m - ba) + g_n，其中 C_b^n 是姿态方向余弦矩阵，a_m 为加速度计测量比力。转动方程为 q̇ = 0.5 q ⊗ [0, ω_m - bg]，ω̇ 由力矩与转动惯量决定。若把角速度视为随机游走，则 ḃg = n_bg。这组方程决定了状态转移矩阵 F 的结构：位置-速度耦合、速度-姿态耦合、姿态-陀螺零偏耦合，以及零偏自身的随机游走项。

二、机动加速度模型：从“未知输入”到可估计状态

高动态场景中，a_m 不仅包含重力补偿后的真实加速度，还包含剧烈机动产生的过载。若把机动加速度简单当作白噪声，滤波器会在强机动段出现明显滞后。常用做法有三类：第一类是把机动加速度建模为一阶马尔可夫过程，即 ȧ = -a/τ + w_a，其中 τ 为机动时间常数，w_a 为驱动噪声；第二类是把加速度作为随机游走状态扩维，即 ȧ = w_a，适合持续过载；第三类是交互多模型（IMM），用“平稳、转弯、高机动”多个模型并行匹配。

以一类典型高动态飞行器为例，设机动时间常数 τ = 0.5 s，驱动噪声标准差 σ_a = 8 m/s²，采样周期 T = 0.01 s。离散化后一阶马尔可夫模型的状态转移系数为 α = exp(-T/τ) ≈ 0.9802，过程噪声方差近似为 Q_a ≈ σ_a² (1 - α²) ≈ 3.14 (m/s²)²。若把该加速度扩维进15维状态，状态维数变为18维，F 中速度行对加速度状态产生 T 的耦合，加速度行自身为 α。这一步直接决定了协方差传播的数值量级。

三、高动态对状态方程的影响

高动态首先改变的是状态转移矩阵的时变性。当角速度达到 200 °/s 以上、加速度过载超过 5 g 时，C_b^n 在一个采样周期内变化显著，若仍用常值 F 近似，速度与姿态的耦合项会产生不可忽略的离散化误差。工程上常采用两步策略：在 T 内用中值角速度构造等效旋转矢量，再对 F 做分段常值离散；或者直接使用二阶龙格-库塔离散，把 F 的时变影响压到 O(T³) 量级。

其次是过程噪声协方差 Q 的显著增大。高动态下，未建模的机动、气动扰动、结构振动都会进入 Q。若 Q 设得过小，滤波器会“过度自信”，在机动段出现发散；若设得过大，平稳段又会放大噪声。一个可操作的判断依据是：用新息序列 ν_k = z_k - H x̂_k|k-1 的归一化平方和 NIS = ν_k^T S_k^{-1} ν_k 做卡方检验。当 NIS 连续超过门限（例如3维量测下门限约 7.81，对应95%置信度），说明 Q 或机动模型偏小，应增大 Q_a 或切换到高机动模型。

第三是可观测性。高动态下，加速度计同时敏感重力与机动加速度，若姿态误差与加计零偏同时存在，二者在短时间窗口内难以区分。解决办法是引入GNSS/气压高度等外部量测，或在机动段利用角速度与比力的相关性增强可观测性。实际调试时，可先固定 ba 与 bg 的随机游走参数，观察姿态误差收敛曲线；若姿态误差在转弯段出现周期性波动，通常说明 Q_a 与 τ 不匹配。

四、一个可复现的建模检查清单

下面给出一份面向工程实现的建模检查表，可直接用于代码评审或仿真前自检。表中参数为示例值，实际使用时应按飞行器过载能力与传感器带宽调整。

综上，六自由度运动建模提供了状态方程的骨架，机动加速度模型决定了过程噪声与扩维结构，而高动态则通过时变 F、增大 Q 和改变可观测性三条路径影响滤波性能。实际实现时，建议先用仿真生成一段包含 5 g 过载与 180 °/s 转弯的轨迹，再对比常值 F、一阶马尔可夫扩维和 IMM 三种方案的 NIS 曲线与姿态误差，从而确定与飞行包线匹配的模型参数。