无人机垂直通道卡尔曼滤波器：从状态空间模型到六传感器融合的数值实例

摘要

本文以某无人机飞控（FCS）惯性导航状态估计器中的垂直通道（高度）滤波器为对象，系统阐述其卡尔曼滤波（EKF）的数学原理与具体实现。文章从连续时间运动方程出发，经前向欧拉离散化得到系统矩阵 $A$ 与输入矩阵 $B$；随后给出预测（Predict）与修正（Correct）两步的完整方程，并以一组 6 路传感器（GNSS 速度/高度、声呐、气压计、VIO 速度/位置）的真实结构为例，代入具体数值逐步计算出新息、新息协方差、卡尔曼增益与修正后的状态与协方差，最后讨论了信任权重、偏置可观测性与协方差收缩等工程要点。

---

1 背景与系统架构

在典型 INS/GNSS 组合导航中，位置估计按"东–北–高"三轴组织。该模型的实现并非三个独立滤波器，而是：

• 水平滤波器（Horizontal Filter）：将东、北两轴合并为一个 6 状态 EKF，状态为 $[E_{pos},N_{pos},E_{vel},N_{vel},E_{bias},N_{bias}{]}^{\top }$；
• 垂直滤波器（Vertical Filter）：单独处理"高"轴，为一个 3 状态 EKF。

本文聚焦垂直滤波器，其状态定义为

$$x=\left[\begin{array}{c}z\\ v\\ b\end{array}\right]$$

其中 $z$ 为高度，$v$ 为垂直速度，$b$ 为垂直加速度计偏置。垂直通道之所以独立成滤波器，是因为其观测源（气压计、声呐、GNSS 高度、VIO 位置）与水平通道不同，且需单独处理重力与垂向偏置。

---

2 垂直通道连续状态空间模型

由垂直方向运动学及 IMU 偏置建模，连续时间方程为

$$\begin{array}{rl}\dot{z}& =v\\ \dot{v}& =u-b\\ \dot{b}& =0\end{array}$$

其中 $u$ 为加速度计提供的垂向比力（特定力）读数。写成标准连续状态空间形式 $\dot{x}=Fx+Gu$：

$$F=\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& -1\\ 0& 0& 0\end{array}\right],G=\left[\begin{array}{c}0\\ 1\\ 0\end{array}\right]$$

物理含义：$F(1,2)=1$ 表示高度由速度驱动；$F(2,3)=-1$ 表示速度变化率受偏置负向影响（偏置从加速度中扣除）；$F(3,3)=0$ 表示偏置作为"随机常数"本身不漂移；$G(2,1)=1$ 表示控制输入 $u$ 直接进入速度通道。

---

3 离散化与系统矩阵 $A$、$B$

该模型采用**前向欧拉（Forward Euler）**离散化，采样周期 $dt=0.0025\text{s}$（400 Hz，由模型中常量块直接给出）：

$$A=I+dt\cdot F=\left[\begin{array}{ccc}1& dt& 0\\ 0& 1& -dt\\ 0& 0& 1\end{array}\right],B=dt\cdot G=\left[\begin{array}{c}0\\ dt\\ 0\end{array}\right]$$

代入 $dt=0.0025$ 得

$$A=\left[\begin{array}{ccc}1& 0.0025& 0\\ 0& 1& -0.0025\\ 0& 0& 1\end{array}\right],B=\left[\begin{array}{c}0\\ 0.0025\\ 0\end{array}\right]$$

说明：此处使用前向欧拉，故不含 $\frac{1}{2}d{t}^2$ 项。这意味着加速度对高度的贡献是间接的（仅通过 $z+=dt\cdot v$），而非直接二次积分。这是该模型的一个刻意简化。

---

4 卡尔曼滤波：预测与修正

4.1 预测步（时间更新）

$$\begin{array}{rl}{x}_{pred}& =Ax+Bu\\ P_{pred}& =AP{A}^{\top }+Q\end{array}$$

其中 $Q$ 为过程噪声协方差。关键项 $AP{A}^{\top }$ 表示"状态不确定性随运动方程传播"，并在传播中自动生成状态间的相关性（如速度–偏置耦合）。

4.2 修正步（量测更新）

设有观测矩阵 $H$（本模型中高度类传感器行 $[1,0,0]$，速度类行 $[0,1,0]$，偏置列恒为 0）、观测向量 $z$ 与观测噪声协方差 $R$，则：

$$\begin{array}{rlrl}y& =z-H{x}_{pred}& & \text{（新息 / 残差）}\\ S& =H{P}_{pred}{H}^{\top }+R& & \text{（新息协方差）}\\ K& =P_{pred}{H}^{\top }{S}^{-1}& & \text{（卡尔曼增益）}\\ x_{next}& =x_{pred}+Ky& & \text{（修正后状态）}\\ P_{next}& =(I-KH)P_{pred}& & \text{（修正后协方差）}\end{array}$$

$S=HP{H}^{\top }+R$ 的含义：$HP{H}^{\top }$ 是"预测测量值"的协方差（由状态不确定度经 $H$ 投影到测量空间），$R$ 是传感器自身噪声；二者之和即"残差应当有多大的不确定度"。$S$ 在增益公式中充当分母——它越大（越不信任残差），增益越小，越偏向预测值。

---

5 六传感器融合数值算例

5.1 场景设定

状态 $x=[z;v;b]$，预测步输出：

$$x_{pred}=\left[\begin{array}{c}10.000\\ 0.500\\ 0.020\end{array}\right],P_{pred}=\mathrm{diag}(0.4,0.25,0.04)$$

6 路传感器（顺序与模型 Covariance Scheduling 一致）：GNSS 速度、GNSS 高度、声呐高度、气压高度、VIO 速度、VIO 位置高度。其观测矩阵与实测值、噪声方差为：

$$H=\left[\begin{array}{ccc}0& 1& 0\\ 1& 0& 0\\ 1& 0& 0\\ 1& 0& 0\\ 0& 1& 0\\ 1& 0& 0\end{array}\right],z=\left[\begin{array}{c}0.45\\ 10.02\\ 9.98\\ 10.01\\ 0.52\\ 9.99\end{array}\right],R=\mathrm{diag}(0.16,0.0225,0.0625,0.09,0.16,1.96)$$

$R$ 取值对应各传感器有效（valid）模式的典型噪声方差，其中 GNSS 高度 $0.15^2$、VIO 速度 $0.4^2$、VIO 位置 $1.4^2$ 直接取自模型的 Covariance Scheduling 常量。

5.2 逐步计算

（1）新息 $y=z-H{x}_{pred}$：

各高度传感器预测值为 $10.0$，各速度传感器预测值为 $0.5$，故

$$y=\left[\begin{array}{c}-0.05\\ 0.02\\ -0.02\\ 0.01\\ 0.02\\ -0.01\end{array}\right]$$

（2）新息协方差 $S=H{P}_{pred}{H}^{\top }+R$

高度类传感器预测测量方差 = $P_{pred}(1,1)=0.4$，速度类 = $P_{pred}(2,2)=0.25$；观测同一状态的传感器之间互相相关：

$$S=\left[\begin{array}{cccccc}0.41& 0& 0& 0& 0.25& 0\\ 0& 0.4225& 0.4& 0.4& 0& 0.4\\ 0& 0.4& 0.4625& 0.4& 0& 0.4\\ 0& 0.4& 0.4& 0.49& 0& 0.4\\ 0.25& 0& 0& 0& 0.41& 0\\ 0& 0.4& 0.4& 0.4& 0& 2.36\end{array}\right]$$

（3）卡尔曼增益 $K=P_{pred}{H}^{\top }{S}^{-1}$

$$K=\left[\begin{array}{cccccc}0& 0.596& 0.2146& 0.149& 0& 0.0068\\ 0.3788& 0& 0& 0& 0.3788& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right]$$

读取：高度状态主要由 GNSS 高度拉动（权重 0.596），VIO 位置因噪声大（1.96）几乎不被信任（0.0068）；速度状态由两路速度传感器平分（各 0.3788）；偏置行全 0——因无传感器直接观测偏置，且此处 $P_{pred}$ 为对角阵。

（4）修正状态 $x_{next}=x_{pred}+Ky$

$$Ky=\left[\begin{array}{c}0.00905\\ -0.01136\\ 0\end{array}\right],x_{next}=\left[\begin{array}{c}10.0091\\ 0.4886\\ 0.020\end{array}\right]$$

（5）修正协方差 $P_{next}=(I-KH)P_{pred}$

$$P_{next}=\mathrm{diag}(0.0134,0.0606,0.0400)$$

5.3 结果汇总

量	修正前	修正后
高度 $z$ (m)	10.000	10.0091
速度 $v$ (m/s)	0.500	0.4886
偏置 $b$ (m/s²)	0.020	0.020
高度方差	0.400	0.0134
速度方差	0.250	0.0606
偏置方差	0.040	0.040

---

6 讨论

信任权重的体现。 标量情形 $K=P/(P+R)$ 在此放大为矩阵形式：高度通道中 GNSS 高度权重 0.596、VIO 位置仅 0.0068，正对应二者噪声 $0.0225$ 与 $1.96$ 的差距——噪声越小越被信任。

多传感器使协方差显著收缩。 4 路高度传感器将高度方差从 0.4 压到 0.0134，2 路速度传感器将速度方差从 0.25 压到 0.0606，体现了融合对不确定度的抑制。

偏置的可观测性。 本算例中因 $P_{pred}$ 取对角阵，偏置增益为 0，偏置一步不变。但在真实环路中，Predict 步产生的"速度–偏置"交叉项会使偏置通过速度传感器获得极微弱修正（验证显示每拍仅约 $2\times 10^{-5}$）。这说明该结构中偏置主要依赖间接、弱耦合方式估计，是设计上值得注意的特性。

---

7 结论

垂直通道 EKF 以 $[z,v,b{]}^{\top }$ 为状态，经前向欧拉离散得到 $A$、$B$，通过"预测–修正"两步融合 6 路传感器。本文给出的完整数值算例表明：滤波器最终输出高度估计 $10.0091\text{m}$、速度 $0.4886\text{m/s}$，并将高度不确定度降低约 30 倍。该算例与 Simulink 模型中 system model、Predict、Correct、measurement model 及 Covariance Scheduling 的实现完全一致，可作为理解与验证该滤波器的参考范例。

---

附录：与 Simulink 模型实现的对应关系

文章符号	模型位置
$A,B$	vertical filter / system model ，由常量 $dt=0.0025$（Constant 27630）生成
$H$	measurement model 各子模型输出 $C$；高度类 $[1,0,0]$，速度类 $[0,1,0]$（已核对常量块）
$R$	covariance matrix generation / Covariance Scheduling（有效模式噪声方差）
预测/修正	navigationLibrary / Kalman Filter 库内 Predict、Correct 子系统
融合高度输出	`