PX4位置速度估计技术详解（二）：LPE 滤波器的设计——输入噪声 R 与过程噪声 Q构造

本文是《PX4 位置速度估计技术详解》系列的第二篇。建议先阅读第一篇：引言与问题建模，了解比力方程、状态空间定义和 RK4 积分的基础内容。

---

会不会设计滤波器的过程噪声矩阵 Q 和测量噪声矩阵 R，是区分控制/导航工程从业者（科班出身）与普通爱好者（野路子）的一块试金石。

前者知道：这两个矩阵不是"调出来的魔法数字"，而是有着明确的物理含义、严格的量纲约束和可解释的工程直觉。把 Q 和 R 当作黑箱参数盲目试错，和知道每一个参数对应什么物理不确定性、该在什么场景下增大或减小，是两种完全不同的能力层级。

后者往往止步于"把卡尔曼滤波跑通"------状态方程写对了、观测方程写对了、增益算出来了，仿真也能跑。但一旦接入真实传感器，位置漂移、机动超调、收敛缓慢等问题接踵而至，却找不到系统性的调参方法。根本原因在于：没有理解 Q 和 R 的物理来源，也就无法判断问题是出在传感器层面还是模型层面。

本篇从 PX4 LPE 的源码出发，把连续时间卡尔曼滤波中这两个矩阵的设计讲清楚。

---

在第一篇中，我们建立了 LPE 的状态空间模型：

x = [ p n v n b b t z ] T ∈ R 10 \mathbf{x} = \begin{bmatrix} \mathbf{p}^n & \mathbf{v}^n & \mathbf{b}^b & t_z \end{bmatrix}^T \in \mathbb{R}^{10} x=[pnvnbbtz]T∈R10

并推导出简化后的比力方程：

v ˙ n = f n + g n = R b n f b + g n \dot{\mathbf{v}}^n = \mathbf{f}^n + \mathbf{g}^n = \mathbf{R}_b^n \mathbf{f}^b + \mathbf{g}^n v˙n=fn+gn=Rbnfb+gn

其中 f b \mathbf{f}^b fb 为机体系下的比力（加速度计测量值）， R b n \mathbf{R}_b^n Rbn 为姿态旋转矩阵， g n = [ 0 , 0 , g ] T \mathbf{g}^n = [0, 0, g]^T gn=[0,0,g]T 为导航系下的重力加速度。

本篇在此基础上，深入讨论 LPE 中连续时间卡尔曼滤波的两个核心设计问题：

1. 输入噪声 R \mathbf{R} R：加速度计测量不是完美的，其噪声如何通过系统传播？
2. 过程噪声 Q \mathbf{Q} Q：状态方程是简化的，未建模的动态如何补偿？

LPE 的协方差传播方程为：

d P d t = A P + P A T + B R B T + Q \frac{d\mathbf{P}}{dt} = \mathbf{A}\mathbf{P} + \mathbf{P}\mathbf{A}^T + \mathbf{B}\mathbf{R}\mathbf{B}^T + \mathbf{Q} dtdP=AP+PAT+BRBT+Q

右端最后两项------ B R B T \mathbf{B}\mathbf{R}\mathbf{B}^T BRBT 和 Q \mathbf{Q} Q------分别对应上述两个问题。理解它们的物理含义、参数化方法和调参逻辑，是掌握 LPE 设计精髓的关键。

---

1. 状态方程的再审视：输入究竟包含了什么？

1.1 从第一篇回顾输入构造

LPE 的 predict() 函数中，输入 u \mathbf{u} u 的构造过程如下（BlockLocalPositionEstimator.cpp）：

_R_att = matrix::Dcm<float>(matrix::Quatf(_sub_att.get().q));
Vector3f a(imu.accelerometer_m_s2);   // 机体系原始比力测量 f^b_meas
_u = _R_att * a;                       // 旋转到导航系
_u(U_az) += CONSTANTS_ONE_G;          // 补偿重力，得到导航系加速度

注意：这里 a 是机体系下的原始比力测量 （包含零偏），_u 是导航系下的加速度 （尚未扣除零偏）。零偏的扣除是通过系统矩阵 A \mathbf{A} A 中的耦合项完成的，而非在输入构造阶段。

1.2 输入的完整数学表达

设机体系下的比力测量为：

f m e a s b = f t r u e b + b + η b o d y \mathbf{f}^b_{meas} = \mathbf{f}^b_{true} + \mathbf{b} + \boldsymbol{\eta}_{body} fmeasb=ftrueb+b+ηbody

其中：

• f t r u e b \mathbf{f}^b_{true} ftrueb：真实比力
• b \mathbf{b} b：加速度计零偏（机体系，被 LPE 作为状态估计）
• η b o d y \boldsymbol{\eta}_{body} ηbody：加速度计测量噪声（机体系）

导航系下的输入 u \mathbf{u} u 为：

u = R b n f m e a s b + g n = R b n f t r u e b + g n ⏟ 真实导航系加速度 a t r u e n + R b n b + R b n η b o d y \mathbf{u} = \mathbf{R}b^n \mathbf{f}^b{meas} + \mathbf{g}^n = \underbrace{\mathbf{R}b^n \mathbf{f}^b{true} + \mathbf{g}^n}{\text{真实导航系加速度 } \mathbf{a}^n{true}} + \mathbf{R}_b^n \mathbf{b} + \mathbf{R}b^n \boldsymbol{\eta}{body} u=Rbnfmeasb+gn=真实导航系加速度 atruen Rbnftrueb+gn+Rbnb+Rbnηbody

1.3 速度导数的正确形式

LPE 的状态方程为：

x ˙ = A x + B u \dot{\mathbf{x}} = \mathbf{A}\mathbf{x} + \mathbf{B}\mathbf{u} x˙=Ax+Bu

展开速度分量：

v ˙ n = u − R b n b = ( a t r u e n + R b n b + R b n η b o d y ) − R b n b = a t r u e n + R b n η b o d y \dot{\mathbf{v}}^n = \mathbf{u} - \mathbf{R}b^n \mathbf{b} = \left( \mathbf{a}^n{true} + \mathbf{R}_b^n \mathbf{b} + \mathbf{R}b^n \boldsymbol{\eta}{body} \right) - \mathbf{R}b^n \mathbf{b} = \mathbf{a}^n{true} + \mathbf{R}b^n \boldsymbol{\eta}{body} v˙n=u−Rbnb=(atruen+Rbnb+Rbnηbody)−Rbnb=atruen+Rbnηbody

关键观察：

• 零偏 b \mathbf{b} b 在输入 u \mathbf{u} u 中被"加了一次"（通过 R b n b \mathbf{R}_b^n \mathbf{b} Rbnb），又在状态方程中被"减了一次"（通过 A \mathbf{A} A 矩阵的 − R b n -\mathbf{R}_b^n −Rbn 项），两者抵消。
• 如果零偏估计准确（ b ^ ≈ b \hat{\mathbf{b}} \approx \mathbf{b} b^≈b），则速度导数接近真实加速度 a t r u e n \mathbf{a}^n_{true} atruen。
• 如果零偏估计有误差（ b ~ = b − b ^ ≠ 0 \tilde{\mathbf{b}} = \mathbf{b} - \hat{\mathbf{b}} \neq 0 b~=b−b^=0），则残余零偏 R b n b ~ \mathbf{R}_b^n \tilde{\mathbf{b}} Rbnb~ 会以误差形式进入速度积分，导致位置漂移------这正是第一篇中讨论的双重积分发散问题的根源。

1.4 系统矩阵与输入矩阵的完整形式

回顾第一篇，系统矩阵为：

A = [ 0 I 0 0 0 0 − R b n 0 0 0 0 0 0 0 0 0 ] \mathbf{A} = \begin{bmatrix} \mathbf{0} & \mathbf{I} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & -\mathbf{R}_b^n & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & 0 \end{bmatrix} A= 0000I0000−Rbn000000

输入矩阵为：

B = [ 0 3 × 3 I 3 × 3 0 3 × 3 0 1 × 3 ] \mathbf{B} = \begin{bmatrix} \mathbf{0}{3\times3} \\ \mathbf{I}{3\times3} \\ \mathbf{0}{3\times3} \\ \mathbf{0}{1\times3} \end{bmatrix} B= 03×3I3×303×301×3

注意 B \mathbf{B} B 是单位矩阵映射，因为 u \mathbf{u} u 已经是导航系下的加速度，无需额外旋转。

---

2. 连续时间协方差传播方程

2.1 从状态传播到协方差传播

第一篇介绍了 LPE 采用 RK4 积分传播状态：

x k + 1 = x k + h 6 ( k 1 + 2 k 2 + 2 k 3 + k 4 ) \mathbf{x}_{k+1} = \mathbf{x}_k + \frac{h}{6}(\mathbf{k}_1 + 2\mathbf{k}_2 + 2\mathbf{k}_3 + \mathbf{k}_4) xk+1=xk+6h(k1+2k2+2k3+k4)

状态传播只告诉我们"估计值去哪里了"。但卡尔曼滤波的核心是不确定性的管理 ------我们不仅需要知道估计值，还需要知道对这个估计值有多"自信"。这就是协方差矩阵 P \mathbf{P} P 的作用。

在连续时间框架下，协方差通过 Riccati 方程传播：

d P d t = A P + P A T + B R B T + Q \frac{d\mathbf{P}}{dt} = \mathbf{A}\mathbf{P} + \mathbf{P}\mathbf{A}^T + \mathbf{B}\mathbf{R}\mathbf{B}^T + \mathbf{Q} dtdP=AP+PAT+BRBT+Q

LPE 中的实现为（predict() 函数）：

Matrix<float, n_x, n_x> dP = (m_A * m_P + m_P * m_A.transpose() +
                              m_B * m_R * m_B.transpose() + m_Q) * getDt();

2.2 方程各项的物理含义

项	名称	物理作用	数学来源
A P + P A T \mathbf{A}\mathbf{P} + \mathbf{P}\mathbf{A}^T AP+PAT	动力学扩散	已有不确定度通过系统动力学自然扩散	线性系统协方差的李导数
B R B T \mathbf{B}\mathbf{R}\mathbf{B}^T BRBT	输入噪声注入	加速度计测量噪声通过输入通道进入系统	输入噪声协方差的传播
Q \mathbf{Q} Q	过程噪声补偿	补偿模型未捕捉的动态	模型误差的随机过程假设

关键认识 ： B R B T \mathbf{B}\mathbf{R}\mathbf{B}^T BRBT 和 Q \mathbf{Q} Q 是独立叠加 的。即使加速度计完美无噪声（ R = 0 \mathbf{R}=\mathbf{0} R=0），系统不确定性仍然会因为 Q Q Q 而增长（风扰、振动等）；反之，即使模型完美（ Q = 0 Q=0 Q=0），加速度计的测量噪声仍然会通过 R R R 不断注入不确定性。

2.3 为什么需要两项而不是一项？

初学者常问：为什么不把 R R R 和 Q Q Q 合并成一个"总的不确定性"？答案是：它们的物理来源不同，可调参的杠杆点不同。

• R \mathbf{R} R 描述的是传感器层面的不确定性 。更换更好的加速度计、加装减振垫、改善姿态估计精度，都可以减小 R R R。
• Q \mathbf{Q} Q 描述的是模型层面的不确定性 。无论加速度计多么完美，风扰、机体柔性、温度漂移这些物理现象都存在，只能通过 Q Q Q 来补偿。

分离这两项的意义在于：当问题出现时，你能判断该从哪个层面解决 ------是换硬件（调 R R R），还是改参数（调 Q Q Q）？

---

3. 输入不确定性： B R B T \mathbf{B}\mathbf{R}\mathbf{B}^T BRBT

3.1 噪声的完整传播链

机体系加速度计噪声 η b o d y \boldsymbol{\eta}_{body} ηbody 经过以下路径进入系统：

噪声传播链：机体系噪声 η b o d y \boldsymbol{\eta}_{body} ηbody 经姿态旋转 R b n \mathbf{R}_b^n Rbn 得到导航系噪声 η u \boldsymbol{\eta}_u ηu，再通过输入矩阵 B \mathbf{B} B 进入速度协方差。

代入输入的完整表达式：

u = a t r u e n + R b n b ⏟ 零偏（通过 A 扣除） + R b n η b o d y ⏟ 噪声（通过 R 描述） \mathbf{u} = \mathbf{a}^n_{true} + \underbrace{\mathbf{R}b^n \mathbf{b}}{\text{零偏（通过 } A \text{ 扣除）}} + \underbrace{\mathbf{R}b^n \boldsymbol{\eta}{body}}_{\text{噪声（通过 } R \text{ 描述）}} u=atruen+零偏（通过 A 扣除） Rbnb+噪声（通过 R 描述） Rbnηbody

其中等效导航系噪声的协方差为：

Var ( η u ) = R b n   Var ( η b o d y )   ( R b n ) T \text{Var}(\boldsymbol{\eta}_u) = \mathbf{R}b^n \, \text{Var}(\boldsymbol{\eta}{body}) \, (\mathbf{R}_b^n)^T Var(ηu)=RbnVar(ηbody)(Rbn)T

3.2 各向同性与 LPE 的简化

如果机体系噪声是各向同性 的（即 Var ( η b o d y ) = σ 2 I \text{Var}(\boldsymbol{\eta}_{body}) = \sigma^2 \mathbf{I} Var(ηbody)=σ2I），则：

Var ( η u ) = R b n ⋅ σ 2 I ⋅ ( R b n ) T = σ 2 R b n ( R b n ) T = σ 2 I \text{Var}(\boldsymbol{\eta}_u) = \mathbf{R}_b^n \cdot \sigma^2 \mathbf{I} \cdot (\mathbf{R}_b^n)^T = \sigma^2 \mathbf{R}_b^n (\mathbf{R}_b^n)^T = \sigma^2 \mathbf{I} Var(ηu)=Rbn⋅σ2I⋅(Rbn)T=σ2Rbn(Rbn)T=σ2I

正交旋转不改变各向同性噪声的协方差结构。旋转后的噪声仍然是各向同性的。

但 LPE 区分了 XY 和 Z 两个参数（LPE_ACC_XY 和 LPE_ACC_Z），这说明：

1. 机体系下噪声不是各向同性的：大多数 MEMS 加速度计的 Z 轴敏感结构与 XY 不同，Z 轴噪声通常更大。
2. 旋转后协方差不是对角矩阵 ：若机体系协方差为 diag ( σ x y 2 , σ x y 2 , σ z 2 ) \text{diag}(\sigma_{xy}^2, \sigma_{xy}^2, \sigma_z^2) diag(σxy2,σxy2,σz2)，则导航系下精确协方差为 R b n ⋅ diag ( σ x y 2 , σ x y 2 , σ z 2 ) ⋅ ( R b n ) T \mathbf{R}b^n \cdot \text{diag}(\sigma{xy}^2, \sigma_{xy}^2, \sigma_z^2) \cdot (\mathbf{R}_b^n)^T Rbn⋅diag(σxy2,σxy2,σz2)⋅(Rbn)T，这是一个非对角满矩阵。
3. LPE 的简化 ：LPE 将旋转后的协方差近似为对角矩阵 R = diag ( σ x y 2 , σ x y 2 , σ z 2 ) \mathbf{R} = \text{diag}(\sigma_{xy}^2, \sigma_{xy}^2, \sigma_z^2) R=diag(σxy2,σxy2,σz2)。这在飞行器接近水平时近似成立，但在大姿态角时会引入一定误差。

3.3 B R B T \mathbf{B}\mathbf{R}\mathbf{B}^T BRBT 的矩阵结构

将 B R B T \mathbf{B}\mathbf{R}\mathbf{B}^T BRBT 展开到 10×10 状态空间中：

B R B T = [ 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 1 0 3 × 3 R 0 3 × 3 0 3 × 1 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 1 0 1 × 3 0 1 × 3 0 1 × 3 0 ] \mathbf{B}\mathbf{R}\mathbf{B}^T = \begin{bmatrix} \mathbf{0}{3 \times 3} & \mathbf{0}{3 \times 3} & \mathbf{0}{3 \times 3} & \mathbf{0}{3 \times 1} \\ \mathbf{0}{3 \times 3} & \mathbf{R} & \mathbf{0}{3 \times 3} & \mathbf{0}{3 \times 1} \\ \mathbf{0}{3 \times 3} & \mathbf{0}{3 \times 3} & \mathbf{0}{3 \times 3} & \mathbf{0}{3 \times 1} \\ \mathbf{0}{1 \times 3} & \mathbf{0}{1 \times 3} & \mathbf{0}{1 \times 3} & 0 \end{bmatrix} BRBT= 03×303×303×301×303×3R03×301×303×303×303×301×303×103×103×10

观察 ： B R B T \mathbf{B}\mathbf{R}\mathbf{B}^T BRBT 只在速度子空间（第 4-6 行/列）有直接注入。位置、零偏、地形子空间在该矩阵中为零。

但这不意味着位置的不确定性不受影响 。协方差传播是一个整体过程：速度不确定性增大后，通过 A P + P A T \mathbf{A}\mathbf{P} + \mathbf{P}\mathbf{A}^T AP+PAT 中位置与速度的耦合（ p ˙ = v \dot{\mathbf{p}} = \mathbf{v} p˙=v），位置的不确定性会间接增大。同理，位置的现有不确定性也会通过同一耦合路径影响速度。Riccati 方程的整体结构决定了：任何状态的不确定性增长最终都会通过动力学耦合扩散到所有相关状态。

3.4 参数化：LPE_ACC_XY 与 LPE_ACC_Z

m_R.setZero();
m_R(U_ax, U_ax) = _param_lpe_acc_xy.get() * _param_lpe_acc_xy.get();
m_R(U_ay, U_ay) = _param_lpe_acc_xy.get() * _param_lpe_acc_xy.get();
m_R(U_az, U_az) = _param_lpe_acc_z.get() * _param_lpe_acc_z.get();

参数	默认值	单位	物理意义
LPE_ACC_XY	0.012	m/s²/√Hz	XY 方向加速度计噪声谱密度的标准差
LPE_ACC_Z	0.02	m/s²/√Hz	Z 方向加速度计噪声谱密度的标准差

量纲验证：

参数单位 m/s²/√Hz 的平方为 m²/s⁴/Hz = m²/s⁴ × s = m²/s³。速度协方差的变化率单位也是 m²/s³（速度单位 m/s，协方差 m²/s²，变化率 m²/s³），匹配 ✓。

3.5 为什么默认值比数据手册大 10 倍？

典型 MEMS 加速度计的数据手册噪声密度约为 150 μg/√Hz = 0.0015 m/s²/√Hz。LPE 的默认值 0.012（XY）和 0.02（Z）比数据手册值大约 6~13 倍。

源码注释给出了原因：

Larger than data sheet to account for tilt error.

这里的"tilt error"不仅是姿态估计误差，还包括：

• 机体振动：高频振动通过加速度计的机械耦合进入测量
• 安装误差：IMU 与质心之间的微小偏移导致旋转时产生离心加速度
• 姿态估计误差：俯仰/横滚角估计不准时，重力在水平方向的分量投影偏差
• 未建模的比力分量：如螺旋桨产生的推力波动、气动阻力变化

LPE 将这些因素统一打包进 R \mathbf{R} R ，通过增大参数来补偿。这是一种工程上的保守策略------宁可高估噪声，也不要让滤波器过于自信。

---

4. 建模不确定性： Q \mathbf{Q} Q

4.1 Q Q Q 的物理含义

Q Q Q 补偿的是状态方程本身无法描述的不确定性。回顾 LPE 的状态方程：

p ˙ = v , v ˙ = u − R b n b , b ˙ = 0 , t ˙ z = 0 \dot{\mathbf{p}} = \mathbf{v}, \quad \dot{\mathbf{v}} = \mathbf{u} - \mathbf{R}_b^n \mathbf{b}, \quad \dot{\mathbf{b}} = \mathbf{0}, \quad \dot{t}_z = 0 p˙=v,v˙=u−Rbnb,b˙=0,t˙z=0

这些方程是高度理想化的：

• 位置导数等于速度：忽略了地球曲率、导航系旋转
• 速度导数等于加速度减零偏：忽略了风扰、机体柔性振动、推力波动
• 零偏导数等于零：忽略了温漂、老化、电源波动
• 地形导数等于零：忽略了地面起伏

真实世界中的这些未建模动态被抽象为随机过程，通过 Q Q Q 注入协方差。

4.2 LPE 中 Q Q Q 的设置

m_Q.setZero();
float pn_p_sq = _param_lpe_pn_p.get() * _param_lpe_pn_p.get();
float pn_v_sq = _param_lpe_pn_v.get() * _param_lpe_pn_v.get();
m_Q(X_x, X_x) = pn_p_sq;
m_Q(X_y, X_y) = pn_p_sq;
m_Q(X_z, X_z) = pn_p_sq;
m_Q(X_vx, X_vx) = pn_v_sq;
m_Q(X_vy, X_vy) = pn_v_sq;
m_Q(X_vz, X_vz) = pn_v_sq;

float pn_b_sq = _param_lpe_pn_b.get() * _param_lpe_pn_b.get();
m_Q(X_bx, X_bx) = pn_b_sq;
m_Q(X_by, X_by) = pn_b_sq;
m_Q(X_bz, X_bz) = pn_b_sq;

float pn_t_noise_density = _param_lpe_pn_t.get() +
    (_param_lpe_t_max_grade.get() / 100.0f) * sqrtf(_x(X_vx)*_x(X_vx) + _x(X_vy)*_x(X_vy));
m_Q(X_tz, X_tz) = pn_t_noise_density * pn_t_noise_density;

4.3 各参数的含义与量纲

参数	作用状态	默认值	单位	平方后的单位	物理意义
LPE_PN_P	位置 p \mathbf{p} p	0.1	m/s/√Hz	m²/s	位置随机游走功率谱密度
LPE_PN_V	速度 v \mathbf{v} v	0.1	m/s²/√Hz	m²/s³	速度随机游走功率谱密度
LPE_PN_B	零偏 b \mathbf{b} b	0.001	m/s³/√Hz	m²/s⁵	零偏漂移功率谱密度
LPE_PN_T	地形 t z t_z tz	0.001	m/s/√Hz	m²/s	地形随机游走功率谱密度

量纲验证（以位置为例）：

连续时间 Riccati 方程中 d P / d t d\mathbf{P}/dt dP/dt 的单位是 [状态协方差]/s 。位置状态的协方差单位是 m²，因此 Q p Q_p Qp 的单位必须是 m²/s。

• LPE_PN_P = 0.1 m/s/√Hz
• 平方后： ( 0.1 ) 2 (0.1)^2 (0.1)2 = 0.01 m²/s²/Hz
• 由于 1/Hz = s，故 0.01 m²/s² × s = 0.01 m²/s ✓

速度同理：LPE_PN_V = 0.1 m/s²/√Hz → 平方 → 0.01 m²/s⁴/Hz = 0.01 m²/s⁴ × s = 0.01 m²/s³。速度协方差单位 m²/s²，变化率单位 m²/s³，匹配 ✓。

4.4 从噪声谱密度到 Q Q Q

LPE 的参数命名中带有 pn（process noise），单位为 .../√Hz。这实际上是噪声谱密度的标准差形式 ，即 S \sqrt{S} S （ S S S 为功率谱密度 PSD）。

代码中直接取平方：pn_p_sq = pn_p²。这恰好等于功率谱密度 S S S。因此 LPE 中的 m_Q 对角元素就是各状态对应的功率谱密度。

这与 EKF2 中的做法不同：EKF2 通常先定义变化率标准差（如 m/s³），再计算 Q = ( σ ⋅ d t ) 2 Q = (\sigma \cdot dt)^2 Q=(σ⋅dt)2。LPE 的连续时间框架允许直接使用 PSD，无需乘以 d t dt dt。

4.5 地形过程噪声的状态依赖性

地形过程噪声是 LPE 中最精巧的设计：

σ t = σ t , 0 + g m a x 100 ⋅ v h o r i z , Q t z = σ t 2 \sigma_t = \sigma_{t,0} + \frac{g_{max}}{100} \cdot v_{horiz}, \quad Q_{tz} = \sigma_t^2 σt=σt,0+100gmax⋅vhoriz,Qtz=σt2

第一项 σ t , 0 \sigma_{t,0} σt,0 = LPE_PN_T：地形自身的固有不确定性（如地形测量误差、地面起伏）。

第二项 ：地形梯度引起的不确定性。 g m a x / 100 g_{max}/100 gmax/100 将百分比坡度转为斜率（如 100% = 45°，斜率 = 1.0）。水平速度 v h o r i z v_{horiz} vhoriz 乘以斜率，得到垂直方向的等效变化率。

物理直觉 ：飞行器水平运动时，即使地形测量本身没有误差，飞过的地形起伏也会引入高度不确定度。飞得越快、地形越陡， Q t z Q_{tz} Qtz 越大。这是状态依赖的过程噪声------Q 的大小随飞行状态动态调整，体现了运动与环境耦合的物理现实。

---

5. R R R 与 Q Q Q 的协同：什么时候调哪个？

5.1 物理区分

维度	R \mathbf{R} R（输入噪声）	Q \mathbf{Q} Q（过程噪声）
来源	传感器测量不完美	模型未捕捉的动态
物理层	硬件/信号层面	动力学/环境层面
影响路径	直接注入速度协方差	直接注入对应状态的协方差
典型场景	振动、传感器老化、安装松动	风扰、温度变化、地形崎岖、突风
解决方式	换硬件、减振、改善姿态估计	调参数、增加传感器冗余

5.2 调参场景分析

场景 1：悬停时位置缓慢漂移

现象：无人机悬停时，位置估计缓慢偏离设定点，漂移方向随机。

诊断：

• 漂移缓慢（秒级时间尺度）→ 不是高频振动导致的 → 不是 R R R 的问题
• 漂移随机无规律 → 零偏估计可能不够准确，或外部扰动（风）持续存在

调整方向：

• 尝试增大 LPE_PN_B（允许零偏更快适应真实漂移）
• 或减小 LPE_PN_P（让位置估计更信任积分，减少随机游走）

场景 2：机动后恢复缓慢或超调

现象：做急转弯或快速爬升后，位置估计需要较长时间才能收敛回正确值。

诊断：

• 机动时加速度计饱和或振动 → 加速度测量不可信 → R R R 问题
• 机动后风扰导致持续偏移 → 外部动力学未建模 → Q Q Q 问题

调整方向：

• 若振动是主因 → 增大 LPE_ACC_XY/Z
• 若风扰是主因 → 增大 LPE_PN_V

场景 3：GPS 信号间歇丢失

现象：GPS 丢失期间，纯积分发散过快；GPS 恢复后，位置跳跃过大。

诊断：

• GPS 丢失期间发散过快 → 位置过程噪声 LPE_PN_P 过大，模型过于不信任自身
• GPS 恢复后跳跃大 → 丢失期间协方差增长过快，新息权重过大

调整方向：

• 减小 LPE_PN_P（让积分更自信，但也不能过小，否则对风扰响应迟钝）
• 检查 LPE_ACC_XY/Z 是否合理（过小会导致速度协方差增长过快，间接加速位置发散）

重要 ：GPS 丢失本身不改变加速度计噪声特性，因此 R R R 不应因 GPS 可用性而调整。

5.3 一个关键直觉：增益与 R/Q 比例的关系

卡尔曼增益的本质是"预测不确定度 vs 测量不确定度"的比值：

K = P H T ( H P H T + R m e a s ) − 1 \mathbf{K} = \mathbf{P} \mathbf{H}^T (\mathbf{H}\mathbf{P}\mathbf{H}^T + \mathbf{R}_{meas})^{-1} K=PHT(HPHT+Rmeas)−1

其中 P \mathbf{P} P 包含了 R R R 和 Q Q Q 的共同贡献。可以粗略理解为：

增益    ∝    预测不确定度 预测不确定度 + 测量不确定度 \text{增益} \; \propto \; \frac{\text{预测不确定度}}{\text{预测不确定度} + \text{测量不确定度}} 增益∝预测不确定度+测量不确定度预测不确定度

• R R R 主导时（加速度计噪声大）：预测不确定度相对较小，滤波器更信任外部测量（GPS、光流）。行为接近"高频测量校正"------响应快但可能引入测量噪声。
• Q Q Q 主导时（模型不确定度大）：预测不确定度大，滤波器对自身预测不自信，也更依赖外部测量。

但注意： R R R 和 Q Q Q 的作用是非对称 的。 R R R 只影响速度协方差（通过 B ⋅ R ⋅ B T B\cdot R \cdot B^T B⋅R⋅BT），而 Q Q Q 影响所有状态。因此调整 LPE_ACC_XY 主要改变速度估计的平滑度，间接影响位置；而调整 LPE_PN_P 直接影响位置的不确定性增长。

---

6. 连续时间 vs 离散时间：LPE 与 EKF2 的对比

LPE 和 EKF2 代表了卡尔曼滤波实现的两种范式：

特性	LPE（连续时间）	EKF2（离散时间）
协方差传播	Δ P = ( A P + P A T + B R B T + Q ) ⋅ d t \Delta \mathbf{P} = (\mathbf{A}\mathbf{P} + \mathbf{P}\mathbf{A}^T + \mathbf{B}\mathbf{R}\mathbf{B}^T + \mathbf{Q}) \cdot dt ΔP=(AP+PAT+BRBT+Q)⋅dt	P k + 1 = F k P k F k T + Q k \mathbf{P}_{k+1} = \mathbf{F}_k \mathbf{P}_k \mathbf{F}_k^T + \mathbf{Q}_k Pk+1=FkPkFkT+Qk
Q Q Q 的单位	[协方差]/s（功率谱密度 PSD）	[协方差]（离散协方差）
R R R 的处理	显式出现在 Riccati 方程中，与 Q Q Q 并列	通常隐式包含在 F ⋅ P ⋅ F T F \cdot P \cdot F^T F⋅P⋅FT 的 IMU 噪声传播中
参数化方式	PSD 标准差：.../√Hz，平方即为 PSD	变化率标准差：rad/s²、m/s³ 等
状态积分	RK4（四阶龙格-库塔）	通常一阶近似或符号推导的精确离散化
计算复杂度	直接数值积分，一步完成	需构造离散状态转移矩阵 F F F

6.1 Q Q Q 的单位差异

LPE 的 m_Q 直接就是功率谱密度 （单位 [协方差]/s）。连续时间 Riccati 方程中 d P / d t d\mathbf{P}/dt dP/dt 的单位本身就是 [协方差]/s， Q Q Q 与之匹配即可。

EKF2 的 Q Q Q 是离散协方差（单位 [协方差]）。从连续时间 PSD 转换到离散协方差需要乘以步长：

Q d i s c r e t e = S ⋅ d t \mathbf{Q}_{discrete} = \mathbf{S} \cdot dt Qdiscrete=S⋅dt

因此 EKF2 中的参数通常定义为"变化率标准差"，计算时先乘 d t dt dt 再平方： Q = ( σ ⋅ d t ) 2 Q = (\sigma \cdot dt)^2 Q=(σ⋅dt)2。

6.2 LPE 的设计哲学

1. R R R 和 Q Q Q 物理分离 ：在连续时间方程中， B R B T \mathbf{B}\mathbf{R}\mathbf{B}^T BRBT 和 Q \mathbf{Q} Q 是两项独立相加的输入，物理含义清晰。离散时间 EKF 中，IMU 噪声通常通过 F ⋅ P ⋅ F T F \cdot P \cdot F^T F⋅P⋅FT 中的交叉项隐式传播，物理直观性较弱。

2. 参数直接可读 ：.../√Hz 的单位直接就是噪声谱密度的标准差形式，工程上更容易与传感器数据手册对照。

3. 计算简洁：直接数值积分避免了离散化状态转移矩阵的复杂计算，适合资源受限平台。

代价是精度依赖于积分步长 d t dt dt 足够小。LPE 运行频率通常为 100~250 Hz， d t dt dt 在 4~10 ms 量级，对于典型多旋翼动力学而言足够。

---

7. 小结

LPE 的协方差传播方程：

d P d t = A P + P A T ⏟ 动力学扩散 + B R B T ⏟ 输入不确定性 + Q ⏟ 建模不确定性 \frac{d\mathbf{P}}{dt} = \underbrace{\mathbf{A}\mathbf{P} + \mathbf{P}\mathbf{A}^T}{\text{动力学扩散}} + \underbrace{\mathbf{B}\mathbf{R}\mathbf{B}^T}{\text{输入不确定性}} + \underbrace{\mathbf{Q}}_{\text{建模不确定性}} dtdP=动力学扩散 AP+PAT+输入不确定性 BRBT+建模不确定性 Q

揭示了连续时间卡尔曼滤波中不确定性的双重来源：

** R \mathbf{R} R（输入噪声）**描述传感器测量的不完美性。加速度计噪声 η b o d y \boldsymbol{\eta}_{body} ηbody 经姿态旋转后进入导航系，通过 B R B T \mathbf{B}\mathbf{R}\mathbf{B}^T BRBT 直接注入速度协方差。LPE 的 LPE_ACC_XY/Z 比数据手册大 6~13 倍，是因为实际飞行中的"加速度计误差"远不止电噪声，还包括振动、姿态误差、安装偏差等未建模因素。

** Q \mathbf{Q} Q（过程噪声）**描述状态方程本身的不完备性。位置、速度、零偏、地形各自有独立的 Q Q Q 参数，反映不同层面的模型不确定性。其中地形 Q Q Q 的状态依赖设计（随水平速度和坡度动态变化）体现了飞行器与环境的耦合。

调参的核心原则 ：先判断不确定性的物理来源------是传感器层面的（调 R R R）还是动力学/环境层面的（调 Q Q Q），再针对性调整。 R R R 主导速度估计的平滑度， Q Q Q 主导各状态随机游走的速率，两者通过 Riccati 方程的整体结构协同决定滤波器行为。

---

附录：参数汇总表

参数名	符号	默认值	单位	作用	调参方向
LPE_ACC_XY	σ a c c , x y \sigma_{acc,xy} σacc,xy	0.012	m/s²/√Hz	XY 输入噪声谱密度	振动大→增大
LPE_ACC_Z	σ a c c , z \sigma_{acc,z} σacc,z	0.02	m/s²/√Hz	Z 输入噪声谱密度	振动大→增大
LPE_PN_P	σ p \sigma_p σp	0.1	m/s/√Hz	位置过程噪声 PSD	漂移快→减小
LPE_PN_V	σ v \sigma_v σv	0.1	m/s²/√Hz	速度过程噪声 PSD	风扰大→增大
LPE_PN_B	σ b \sigma_b σb	0.001	m/s³/√Hz	零偏过程噪声 PSD	温漂大→增大
LPE_PN_T	σ t \sigma_t σt	0.001	m/s/√Hz	地形过程噪声 PSD	地形崎岖→增大
LPE_T_MAX_GRADE	g m a x g_{max} gmax	100	%	最大地形坡度	丘陵→增大

关于我们：

灵智实验室（LingzhiLab）成立于2020年，核心团队源自西北工业大学，由一群深耕无人系统、自动控制与机器人技术的青年工程师与科研人员组成。我们始终秉持"开放、协同、智能、可靠"的理念，致力于推动无人智能体在复杂环境下的自主感知、决策与控制能力。

实验室聚焦于基于开源飞控（如PX4）与ROS 2的深度融合，构建高可靠、模块化、可扩展的无人系统软件架构。依托扎实的工程实践与学术背景，灵智实验室积极参与开源社区建设，助力科研教育与产业落地。