PX4位置速度估计技术详解（四）：LPE 激光雷达高度融合的实现错误

核心结论 ：LPE 的 lidarCorrect() 和 sonarCorrect() 不是标准 EKF，也不是合格的近似 EKF。它在 lidarMeasure() 中把原始斜距测量通过三角函数转换成垂直高度，然后在 lidarCorrect() 中用线性矩阵做卡尔曼更新。这不是"工程简化"，是实现错误------它把非线性观测强行变成线性观测，却未正确处理噪声方差的坐标系转换，导致卡尔曼增益计算错误。本文逐行分析错误机理，给出正确的标准 EKF 实现。

---

1. 两种测量模型的根本分野

卡尔曼滤波处理测量更新时，存在两种数学上等价、但工程实现上截然不同的思路：

1.1 标准 EKF：Forward Model（正道）

核心原则 ：状态估计 x^\hat{\mathbf{x}}x^ 通过非线性函数 h(⋅)h(\cdot)h(⋅) 投影到测量空间 ，与原始测量 y\mathbf{y}y 直接比较。

r=y−h(x^) \mathbf{r} = \mathbf{y} - h(\hat{\mathbf{x}}) r=y−h(x^)

测量方程的推导

LPE 的状态向量：

x=[x,  y,  z,  vx,  vy,  vz,  bx,  by,  bz,  tz]T \mathbf{x} = [x,\; y,\; z,\; v_x,\; v_y,\; v_z,\; b_x,\; b_y,\; b_z,\; t_z]^T x=[x,y,z,vx,vy,vz,bx,by,bz,tz]T

其中 zzz 为飞行器在 NED 坐标系中的高度（向下为正 ），tzt_ztz 为地形高度（同样向下为正）。飞行器到地面的垂直距离为：

hvert=tz−z h_{\text{vert}} = t_z - z hvert=tz−z

激光雷达测的是斜距 ddd（沿雷达光束方向的距离）。设雷达垂直向下安装，机体有 roll ϕ\phiϕ 和 pitch θ\thetaθ，则斜距与垂直距离的几何关系为：

d=tz−zcos⁡ϕ⋅cos⁡θ+νd d = \frac{t_z - z}{\cos\phi \cdot \cos\theta} + \nu_d d=cosϕ⋅cosθtz−z+νd

其中 νd\nu_dνd 为斜距测量噪声（在原始测量空间定义）。这就是测量方程 h(x)h(\mathbf{x})h(x)：

h(x)=tz−zcos⁡ϕ⋅cos⁡θ h(\mathbf{x}) = \frac{t_z - z}{\cos\phi \cdot \cos\theta} h(x)=cosϕ⋅cosθtz−z

新息在原始测量空间计算：

r=d−h(x^)=d−t^z−z^cos⁡ϕ⋅cos⁡θ \mathbf{r} = d - h(\hat{\mathbf{x}}) = d - \frac{\hat{t}_z - \hat{z}}{\cos\phi \cdot \cos\theta} r=d−h(x^)=d−cosϕ⋅cosθt^z−z^

雅可比矩阵的逐步推导

EKF 需要计算 H=∂h∂x\mathbf{H} = \frac{\partial h}{\partial \mathbf{x}}H=∂x∂h。测量函数 hhh 只依赖于状态中的 zzz 和 tzt_ztz 两个分量：

对 zzz 求偏导数：

∂h∂z=∂∂z(tz−zcos⁡ϕ⋅cos⁡θ)=−1cos⁡ϕ⋅cos⁡θ \frac{\partial h}{\partial z} = \frac{\partial}{\partial z}\left(\frac{t_z - z}{\cos\phi \cdot \cos\theta}\right) = -\frac{1}{\cos\phi \cdot \cos\theta} ∂z∂h=∂z∂(cosϕ⋅cosθtz−z)=−cosϕ⋅cosθ1

对 tzt_ztz 求偏导数：

∂h∂tz=∂∂tz(tz−zcos⁡ϕ⋅cos⁡θ)=+1cos⁡ϕ⋅cos⁡θ \frac{\partial h}{\partial t_z} = \frac{\partial}{\partial t_z}\left(\frac{t_z - z}{\cos\phi \cdot \cos\theta}\right) = +\frac{1}{\cos\phi \cdot \cos\theta} ∂tz∂h=∂tz∂(cosϕ⋅cosθtz−z)=+cosϕ⋅cosθ1

对其他状态分量求偏导数：均为 0。

因此，雅可比矩阵为：

H=∂h∂x=[0,  0,  −1cos⁡ϕcos⁡θ,  0,  0,  0,  0,  0,  0,  +1cos⁡ϕcos⁡θ] \mathbf{H} = \frac{\partial h}{\partial \mathbf{x}} = \left[ 0,\; 0,\; -\frac{1}{\cos\phi\cos\theta},\; 0,\; 0,\; 0,\; 0,\; 0,\; 0,\; +\frac{1}{\cos\phi\cos\theta} \right] H=∂x∂h=[0,0,−cosϕcosθ1,0,0,0,0,0,0,+cosϕcosθ1]

其中第 3 个元素对应 ∂h/∂z\partial h/\partial z∂h/∂z（负号 ），第 10 个元素对应 ∂h/∂tz\partial h/\partial t_z∂h/∂tz（正号）。

卡尔曼增益

K=PHT(HPHT+Rd)−1 \mathbf{K} = \mathbf{P} \mathbf{H}^T (\mathbf{H} \mathbf{P} \mathbf{H}^T + R_d)^{-1} K=PHT(HPHT+Rd)−1

其中 Rd=E[νd2]R_d = E[\nu_d^2]Rd=E[νd2] 是驱动直接提供的斜距空间 测量噪声方差，无需任何坐标转换。

关键特征：

• 噪声方差 RdR_dRd 在原始测量空间定义，直接可用
• 姿态误差通过 H\mathbf{H}H 中的 cos⁡\coscos 项自然传播到位置估计（见下文 2.2 节）
• 新息与原始测量同维度，故障检测直观

1.2 LPE 的做法：Inverse Model（野路子）

核心操作 ：先把原始测量转换到状态空间，再用线性矩阵比较。

// lidar.cpp:55-58  lidarMeasure()
matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q));
y(0) = (d + _param_lpe_ldr_off_z.get()) *
       cosf(euler.phi()) *
       cosf(euler.theta());

代码把斜距 ddd 乘以 cos⁡ϕ⋅cos⁡θ\cos\phi \cdot \cos\thetacosϕ⋅cosθ，"假设"得到了垂直高度。然后在 lidarCorrect() 中：

// lidar.cpp:72-75
C(Y_lidar_z, X_z)  = -1;
C(Y_lidar_z, X_tz) =  1;

// lidar.cpp:90
Vector<float, n_y_lidar> r = y - C * _x;   // 垂直高度的新息

这等价于：先构造一个假想的"垂直高度传感器"，再用线性 KF 更新。

---

2. 这为什么是错误的

2.1 注释已经承认了错误

LPE 源码中的注释：

// TODO could add trig to make this an EKF correction

作者自己知道这不是 EKF。问题在于，这个 "TODO" 从来没有被修复，而代码却以 EKF 的名义运行了多年。

2.2 三个具体错误

错误一：噪声方差 R 的坐标系混乱（直接导致增益计算错误）

LPE 的 R 设置：

float cov = _sub_lidar->get().variance;   // 驱动提供的斜距空间方差
// ...
if (cov < 1.0e-3f) {
    R(0, 0) = _param_lpe_ldr_z.get() * _param_lpe_ldr_z.get();
} else {
    R(0, 0) = cov;   // ❌ 错误：斜距方差直接用于垂直高度空间
}

cov 是斜距空间的方差 σd2\sigma_d^2σd2，但 y 已经被转换到了垂直高度空间。正确的垂直高度方差必须是：

σy2=σd2⋅(cos⁡ϕ⋅cos⁡θ)2 \sigma_y^2 = \sigma_d^2 \cdot (\cos\phi \cdot \cos\theta)^2 σy2=σd2⋅(cosϕ⋅cosθ)2

LPE 漏掉了 (cos⁡ϕ⋅cos⁡θ)2(\cos\phi \cdot \cos\theta)^2(cosϕ⋅cosθ)2 缩放因子。飞行器倾斜时（cos⁡<1\cos < 1cos<1），LPE 使用的噪声方差比真实值大，卡尔曼增益被系统性压低，修正力度不足。

即使走参数路径（cov < 1e-3，使用 _param_lpe_ldr_z），两条路径的噪声定义也不一致：参数路径假设 R 是垂直高度方差，驱动路径却假设 R 是斜距方差。同一函数内对 R 的物理定义自相矛盾。

错误二：姿态误差被强行截断

标准 EKF 的雅可比包含姿态偏导数：

∂h∂ϕ=tz−zcos⁡ϕ⋅cos⁡θ⋅tan⁡ϕ \frac{\partial h}{\partial \phi} = \frac{t_z - z}{\cos\phi \cdot \cos\theta} \cdot \tan\phi ∂ϕ∂h=cosϕ⋅cosθtz−z⋅tanϕ

姿态不确定性通过这个通道传播到位置估计。LPE 的 C = [-1, 1] 是常数矩阵，姿态信息被完全剔除。即使姿态估计存在显著误差，lidar 更新也不会产生任何补偿------姿态误差和位置估计被人为解耦。

错误三：非线性被预处理掩盖

y = d * cos(phi) * cos(theta)   // 非线性预处理，发生在测量函数外部
r = y - (-z + tz)               // 线性比较，假装这是线性系统

预处理步骤把非线性从卡尔曼框架中抽走了。飞行器剧烈机动时（大角度倾斜），cos(phi) * cos(theta) 的动态变化完全不被状态估计器感知。标准 EKF 每次更新都重新求 H\mathbf{H}H，自然适应这种动态；LPE 的线性 C 矩阵对此无能为力。

---

3. 试图打补丁只会更糟

假设有人想"修复"LPE 而不改动整体框架，那么在 inverse model 上打补丁至少需要：

// 补丁 1：把 cos_tilt 传出来
float cos_tilt = cosf(euler.phi()) * cosf(euler.theta());
y(0) = (d + offset) * cos_tilt;

// 补丁 2：R 必须转换
float cov = _sub_lidar->get().variance;
if (cov < 1.0e-3f) {
    R(0, 0) = _param_lpe_ldr_z.get() * _param_lpe_ldr_z.get();
} else {
    R(0, 0) = cov * cos_tilt * cos_tilt;   // 斜距方差→垂直高度方差
}

// 补丁 3：协方差更新用约瑟夫形式
// ...

但打补丁不能解决根本问题：inverse model 把非线性观测强行变成了线性框架，姿态信息被截断，测量空间的物理意义丢失。与其在错误的基础上堆补丁，不如直接重写为标准 EKF。

---

4. 正道：标准 EKF 的代码应该怎么写

// ========== 标准 EKF Forward Model（推荐）==========

void lidarCorrect()
{
    // 1. 获取原始斜距测量（不转换）
    float d = _sub_lidar->get().current_distance + _param_lpe_ldr_off_z.get();
    
    // 2. 从状态计算预测的斜距（将状态投影到测量空间）
    matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q));
    float cos_tilt = cosf(euler.phi()) * cosf(euler.theta());
    float h_x = (_x(X_tz) - _x(X_z)) / cos_tilt;   // h(x) = (tz - z) / cos_tilt
    
    // 3. 新息在原始测量空间计算
    float r = d - h_x;
    
    // 4. 计算雅可比矩阵 H = dh/dx
    Matrix<float, 1, n_x> H;
    H.setZero();
    H(0, X_z)  = -1.0f / cos_tilt;   // dh/dz
    H(0, X_tz) =  1.0f / cos_tilt;   // dh/dtz
    // 如果需要，还可以加 dh/dphi, dh/dtheta（如果姿态也在状态里）
    
    // 5. R 直接使用驱动提供的斜距方差（无需转换）
    float R = _sub_lidar->get().variance;
    if (R < 1.0e-3f) {
        R = _param_lpe_ldr_z.get() * _param_lpe_ldr_z.get();
    }
    
    // 6. 标准 EKF 更新
    Matrix<float, 1, 1> S = H * m_P * H.transpose() + R;
    Matrix<float, 1, 1> S_I = inv<float, 1>(S);
    Matrix<float, n_x, 1> K = m_P * H.transpose() * S_I;
    
    _x += K * r;
    m_P -= K * H * m_P;   // 或约瑟夫稳定形式
}

标准 EKF 的优势：

1. R 直接可用 ：驱动提供的斜距方差就是所需的 RdR_dRd，无需任何转换
2. 姿态自然传播 ：如果姿态在状态内，H\mathbf{H}H 自动包含姿态偏导数；即使姿态不在状态内（由外部提供），forward model 的物理意义也更清晰
3. 新息在传感器空间：与原始测量同维度同物理意义，故障检测更直观
4. 没有隐藏的非线性 ：h(x)h(\mathbf{x})h(x) 的每次求值都使用当前状态，没有预处理的"快照"问题

---

5. EKF2 的对比：同样的框架，更严谨的实现

EKF2 的 range_height_fusion.cpp 也先把斜距转垂直距离：

float measurement = _range_sensor.getDistBottom();  // = rng * cos_tilt
float innovation = getHagl() - measurement;

ComputeHaglH 雅可比同样是 [-1 (pos_z), +1 (terrain)]。

但 EKF2 做了三件 LPE 没做的事：

1. 倾斜角计算正确 ：使用完整旋转矩阵 + 雷达安装角偏移，不是 cos(phi)*cos(theta) 小角度近似
2. 噪声模型完整 ：getRngVar() 包含状态不确定性、固定噪声、距离相关噪声三项
3. 协方差更新稳定：使用约瑟夫形式，避免简化更新的数值漂移

这不能为 LPE 开脱。 EKF2 的实现更规范，但框架本身仍是 inverse model。从数学严谨性角度，标准 EKF 的 forward model 才是正确的做法。EKF2 沿用此框架是工程历史原因，不是数学上的正当性。

---

6. 结论

维度	LPE 实现	标准 EKF	结论
测量模型	Inverse（先转测量，再线性比较）	Forward（状态投影到测量空间）	LPE 错误
噪声 R	斜距方差直接用于垂直高度空间	斜距方差用于斜距空间，无需转换	LPE 错误
姿态传播	C = [-1, 1] 常数矩阵，不含姿态	H 含姿态偏导数	LPE 错误
数值稳定	P -= K*C*P 简化形式	约瑟夫稳定形式	LPE 欠妥
代码复杂度	先转换测量，再线性更新	直接非线性投影	两者相当

LPE 的 lidarCorrect() 和 sonarCorrect() 不是标准 EKF，也不是可接受的近似。 它把非线性观测强行拆解为"三角函数预处理 + 线性 KF"，没有获得任何计算简化，却在三个关键点上犯错：噪声方差坐标系混乱、姿态信息被截断、非线性动态被掩盖。这不是"工程妥协"，是实现错误。

如果重新实现激光雷达/超声波高度融合，应当直接采用标准 EKF 的 forward model：状态通过非线性函数投影到斜距测量空间，新息在原始测量空间计算，R 直接使用驱动提供的斜距方差，雅可比矩阵包含完整的姿态偏导数。

---

附录：LPE 问题代码速览

// ==== 文件：src/modules/local_position_estimator/sensors/lidar.cpp ====

// 问题 1：inverse model，先转换测量
int BlockLocalPositionEstimator::lidarMeasure(Vector<float, n_y_lidar> &y)
{
    float d = _sub_lidar->get().current_distance;
    matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q));
    y(0) = (d + _param_lpe_ldr_off_z.get()) *
           cosf(euler.phi()) *          // 非线性预处理
           cosf(euler.theta());
    return OK;
}

// 问题 2：线性 C 矩阵，不含姿态
void BlockLocalPositionEstimator::lidarCorrect()
{
    // ...
    C(Y_lidar_z, X_z) = -1;   // 常数，不含 cos(phi)
    C(Y_lidar_z, X_tz) = 1;   // 常数，不含 cos(theta)
    
    // 问题 3：R 未转换
    float cov = _sub_lidar->get().variance;
    R(0, 0) = cov;   // 斜距方差直接用于垂直高度空间
    
    // 问题 4：简化协方差更新
    Matrix<float, n_x, n_y_lidar> K = m_P * C.transpose() * S_I;
    _x += K * r;
    m_P -= K * C * m_P;   // 非约瑟夫形式
}

// ==== 文件：src/modules/local_position_estimator/sensors/sonar.cpp ====
// 完全相同的模式，同样的四个问题

关于我们：

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

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