PX4位置速度估计技术详解（一）：引言与问题建模

1. 组合导航的背景与定位

1.1 低成本组合导航的概念

在现代无人机系统中，组合导航（Integrated Navigation）是指将多种导航传感器的信息进行融合，以获得比单一传感器更精确、更可靠的导航结果。组合导航系统可分为两大类：

类型	典型传感器	精度	成本	应用场景
高精度组合导航	战术级/导航级IMU、双天线RTK-GPS、高精度里程计	厘米级	数万元	自动驾驶、测绘、精密农业
低成本组合导航	MEMS级IMU、单天线GPS、光流传感器、气压计	米级	数百元	消费级无人机、小型机器人

PX4的local_position_estimator（LPE）模块属于低成本组合导航 范畴。这一领域的核心挑战在于：如何在MEMS传感器精度受限、计算资源受限的条件下，实现满足飞行控制需求的导航解算。

1.2 MEMS-IMU的误差特性与积分发散问题

低成本方案的核心瓶颈在于MEMS惯性测量单元的误差特性。以典型消费级IMU（如MPU6000）为例：

零偏不稳定性（Bias Instability）：

加速度计零偏：σb∼10−2∼10−1 m/s2\sigma_b \sim 10^{-2} \sim 10^{-1} \, \text{m/s}^2σb∼10−2∼10−1m/s2
陀螺仪零偏：σb∼10−2∼100 °/s\sigma_b \sim 10^{-2} \sim 10^{0} \, \text{°/s}σb∼10−2∼100°/s

此外还存在比例因子误差、交轴耦合、温度漂移等误差源。

致命问题：双重积分的发散特性

惯导系统通过积分获得速度和位置：

v(t)=v0+∫0ta(τ) dτ \mathbf{v}(t) = \mathbf{v}_0 + \int_0^t \mathbf{a}(\tau) \, d\tau v(t)=v0+∫0ta(τ)dτ

p(t)=p0+∫0tv(τ) dτ \mathbf{p}(t) = \mathbf{p}_0 + \int_0^t \mathbf{v}(\tau) \, d\tau p(t)=p0+∫0tv(τ)dτ

若加速度存在恒定零偏 bab_aba，经过双重积分后，位置误差将按时间的平方增长：

δp(t)=12bat2 \delta \mathbf{p}(t) = \frac{1}{2} b_a t^2 δp(t)=21bat2

数值示例 ：设 ba=0.1 m/s2b_a = 0.1 \, \text{m/s}^2ba=0.1m/s2，则：

时间	速度误差	位置误差
1 s	0.1 m/s	0.05 m
10 s	1.0 m/s	5 m
60 s	6.0 m/s	180 m

这种发散特性使得纯惯导系统无法独立用于需要长期精确定位的应用。

1.3 互补滤波：组合导航的核心思想

不同传感器具有互补的频率特性：

传感器	带宽	低频特性	高频特性	信息维度
IMU	高（~1kHz）	❌ 漂移	✅ 精确	高带宽加速度/角速度
GPS	低（~10Hz）	✅ 无漂移	❌ 延迟/噪声	绝对位置/速度
气压计	中（~50Hz）	✅ 稳定	❌ 噪声	高度辅助
光流	中（~100Hz）	❌ 累计误差	✅ 高频速度	水平相对速度

融合策略的本质：

利用IMU的高带宽 特性提供短时精确的航迹推算（Dead Reckoning），同时利用GPS等外部传感器的长期稳定性估计并补偿IMU误差。

LPE采用卡尔曼滤波框架实现这一组合策略，在计算复杂度和估计精度之间取得平衡。

2. 传感器特性与融合必要性

2.1 加速度计：零偏与积分发散

加速度计测量的是比力（Specific Force），包含物理加速度和重力分量。其误差模型可简化为：

a~=a+ba+na \tilde{a} = a + b_a + n_a a~=a+ba+na

其中：

a~\tilde{a}a~：测量值
aaa：真实加速度
bab_aba：零偏（慢时变）
nan_ana：测量噪声（高频）

纯惯导的不可行性：

即使零偏很小（如0.01 m/s²），100秒后的位置误差：

δp=12×0.01×1002=50 m \delta p = \frac{1}{2} \times 0.01 \times 100^2 = 50 \, \text{m} δp=21×0.01×1002=50m

这对于需要精确悬停的无人机是不可接受的。

2.2 GPS：低频但长期稳定

GPS提供绝对位置（WGS84坐标系），虽然：

更新率低（1-20 Hz）
存在多路径、遮挡等问题
单点定位精度约2-5米

但其关键优势是无漂移性------长期统计均值稳定，可作为外部参考修正惯导累积误差。

2.3 其他辅助传感器

传感器	测量量	主要作用	局限性
气压计	气压→高度	高频高度辅助	受天气、风压影响
光流传感器	图像光流→速度	室内/无GPS环境下的水平速度	需一定高度和纹理
激光/超声波	距离→离地高度	精确AGL高度，用于起降	量程有限
视觉/VIO	视觉特征→位姿	室内定位，六自由度	计算量大，受光照影响

融合的必要性总结：

没有任何单一传感器能在所有场景下提供可靠的导航信息。LPE的核心价值在于设计一套多源异构传感器融合框架，在各种飞行条件下维持可用的定位精度。

3. 比力方程与惯性导航基础

3.1 比力的物理定义

比力（Specific Force）是惯性导航的核心概念，定义为作用在单位质量上的非引力外力：

f=Fnon-gravitym \mathbf{f} = \frac{\mathbf{F}_{\text{non-gravity}}}{m} f=mFnon-gravity

加速度计测量的正是比力，而非单纯的加速度。根据牛顿第二定律在地球附近的表达：

f=a−g \mathbf{f} = \mathbf{a} - \mathbf{g} f=a−g

其中：

a\mathbf{a}a：相对于惯性空间的绝对加速度
g\mathbf{g}g：当地重力加速度（引力与离心力的合成）

物理直觉：

自由落体时，a=g\mathbf{a} = \mathbf{g}a=g，故 f=0\mathbf{f} = 0f=0（失重）
静止在地表时，a=0\mathbf{a} = 0a=0，故 f=−g\mathbf{f} = -\mathbf{g}f=−g（地面支持力的反作用力）

3.2 完整的比力方程

在地球附近的惯性导航中，比力方程的完整形式为：

v˙n=fn−(2ωien+ωenn)×vn+gn \dot{\mathbf{v}}^n = \mathbf{f}^n - (2\boldsymbol{\omega}{ie}^n + \boldsymbol{\omega}{en}^n) \times \mathbf{v}^n + \mathbf{g}^n v˙n=fn−(2ωien+ωenn)×vn+gn

各项物理意义：

项	名称	物理来源	典型量级
fn\mathbf{f}^nfn	比力	加速度计测量（导航系）	取决于运动
2ωien×vn2\boldsymbol{\omega}_{ie}^n \times \mathbf{v}^n2ωien×vn	哥氏加速度	地球自转	∼10−2 m/s2\sim 10^{-2} \, \text{m/s}^2∼10−2m/s2
ωenn×vn\boldsymbol{\omega}_{en}^n \times \mathbf{v}^nωenn×vn	牵连加速度	导航系转动	∼10−3 m/s2\sim 10^{-3} \, \text{m/s}^2∼10−3m/s2
gn\mathbf{g}^ngn	重力	引力+离心力	∼10 m/s2\sim 10 \, \text{m/s}^2∼10m/s2

其中：

ωie\boldsymbol{\omega}_{ie}ωie：地球自转角速度（约 7.292×10−5 rad/s7.292 \times 10^{-5} \, \text{rad/s}7.292×10−5rad/s）
ωen\boldsymbol{\omega}_{en}ωen：导航系相对于地球的转动角速度

3.3 坐标变换：从机体系到导航系

加速度计固连于机体，测量的是机体系 下的比力 fb\mathbf{f}^bfb。需通过姿态旋转矩阵转换到导航系：

fn=Rbn⋅fb \mathbf{f}^n = R_b^n \cdot \mathbf{f}^b fn=Rbn⋅fb

其中 RbnR_b^nRbn 为从机体系到导航系的旋转矩阵，由姿态估计模块（如attitude_estimator_q或EKF2）提供。

3.4 比力方程的推导与简化证明

3.4.1 基本比力方程

根据比力的物理定义，在机体系中有：

fb=ab−gb \mathbf{f}^b = \mathbf{a}^b - \mathbf{g}^b fb=ab−gb

其中：

fb\mathbf{f}^bfb：机体系下的比力（加速度计测量值）
ab\mathbf{a}^bab：机体系下的绝对加速度
gb\mathbf{g}^bgb：机体系下的重力加速度

3.4.2 坐标变换关系

为了将加速度从导航系转换到机体系（与加速度计测量对齐），使用旋转矩阵：

ab=Rnb an=(Rbn)T an \mathbf{a}^b = R_n^b \, \mathbf{a}^n = (R_b^n)^T \, \mathbf{a}^n ab=Rnban=(Rbn)Tan

gb=Rnb gn=(Rbn)T gn \mathbf{g}^b = R_n^b \, \mathbf{g}^n = (R_b^n)^T \, \mathbf{g}^n gb=Rnbgn=(Rbn)Tgn

其中 Rnb=(Rbn)TR_n^b = (R_b^n)^TRnb=(Rbn)T 是从导航系到机体系的旋转矩阵。

3.4.3 推导导航系下的加速度表达式

将坐标变换代入比力定义：

fb=Rnb an−Rnb gn=Rnb(an−gn) \mathbf{f}^b = R_n^b \, \mathbf{a}^n - R_n^b \, \mathbf{g}^n = R_n^b (\mathbf{a}^n - \mathbf{g}^n) fb=Rnban−Rnbgn=Rnb(an−gn)

两边同时左乘 RbnR_b^nRbn（利用 RbnRnb=IR_b^n R_n^b = IRbnRnb=I）：

Rbn fb=an−gn R_b^n \, \mathbf{f}^b = \mathbf{a}^n - \mathbf{g}^n Rbnfb=an−gn

整理得到导航系下的加速度表达式：

an=Rbn fb+gn \mathbf{a}^n = R_b^n \, \mathbf{f}^b + \mathbf{g}^n an=Rbnfb+gn

或写成速度导数形式：

v˙n=Rbn fb+gn \dot{\mathbf{v}}^n = R_b^n \, \mathbf{f}^b + \mathbf{g}^n v˙n=Rbnfb+gn

这就是简化后的比力方程，是LPE使用的核心运动学方程。

3.4.4 低成本导航的简化依据

对比完整比力方程与简化方程：

方程形式	表达式	适用场景
完整方程	v˙n=Rbnfb+gn−(2ωien+ωenn)×vn\dot{\mathbf{v}}^n = R_b^n \mathbf{f}^b + \mathbf{g}^n - (2\boldsymbol{\omega}{ie}^n + \boldsymbol{\omega}{en}^n) \times \mathbf{v}^nv˙n=Rbnfb+gn−(2ωien+ωenn)×vn	高精度惯导
简化方程	v˙n=Rbnfb+gn\dot{\mathbf{v}}^n = R_b^n \mathbf{f}^b + \mathbf{g}^nv˙n=Rbnfb+gn	低成本无人机

PX4实现 （BlockLocalPositionEstimator.cpp）：

cpp 复制代码

void BlockLocalPositionEstimator::predict(const sensor_combined_s &imu)
{
    // 从姿态估计模块获取旋转矩阵 R_b^n
    _R_att = matrix::Dcm<float>(matrix::Quatf(_sub_att.get().q));
    
    // 读取机体系加速度测量值（包含重力分量）
    Vector3f a(imu.accelerometer_m_s2);
    
    // 转换到导航系：f^n = R_b^n * f^b
    _u = _R_att * a;
    
    // 补偿重力（在NED系中，重力指向下为正）
    // 注意：这里加上g是因为加速度计测的是比力
    // 要得到惯性加速度，需要加回重力
    _u(U_az) += CONSTANTS_ONE_G;  // g ≈ 9.80665 m/s²
    
    // ... 后续状态预测
}

关键理解：

加速度计测量值 ameasb\mathbf{a}_{\text{meas}}^bameasb 实际上是比力 fb\mathbf{f}^bfb（单位化质量后的非引力外力）
在静止时，fb=−Rnbgn\mathbf{f}^b = -R_n^b \mathbf{g}^nfb=−Rnbgn（地面支持力平衡重力）
转换到导航系后，fn=Rbnfb=−gn\mathbf{f}^n = R_b^n \mathbf{f}^b = -\mathbf{g}^nfn=Rbnfb=−gn，加上gn\mathbf{g}^ngn后得到真实加速度为0

4. LPE的简化模型

4.1 简化假设的合理性

对于低成本无人机应用，LPE采用了大幅简化的比力方程。这些简化基于以下物理分析：

假设1：忽略地球自转（哥氏加速度）

对于低速飞行器（速度 v<50 m/sv < 50 \, \text{m/s}v<50m/s），哥氏加速度的量级：

∣2ωie×v∣≈2×7.29×10−5×50≈0.007 m/s2 |2\boldsymbol{\omega}_{ie} \times \mathbf{v}| \approx 2 \times 7.29 \times 10^{-5} \times 50 \approx 0.007 \, \text{m/s}^2 ∣2ωie×v∣≈2×7.29×10−5×50≈0.007m/s2

远小于MEMS加速度计的典型噪声水平（σ∼0.1 m/s2\sigma \sim 0.1 \, \text{m/s}^2σ∼0.1m/s2），可安全忽略。

假设2：导航系与地球固连

采用当地水平当地北（Local Level Local North, LLEN）坐标系，假设在飞行区域内导航系不随地球旋转而转动：

ωen=0 \boldsymbol{\omega}_{en} = 0 ωen=0

这意味着忽略载体运动引起的导航系转动（在几公里范围内误差极小）。

假设3：重力为常数

gn=[0,0,g]T,g≈9.80665 m/s2\mathbf{g}^n = [0, 0, g]^T, \quad g \approx 9.80665 \, \text{m/s}^2gn=[0,0,g]T,g≈9.80665m/s2

忽略重力随纬度（≈0.5%\approx 0.5\%≈0.5%变化）和高度（每1000m减小约0.03%0.03\%0.03%）的变化。

4.2 简化后的比力方程

综合上述假设，简化后的比力方程为：

v˙n=fn+gn \dot{\mathbf{v}}^n = \mathbf{f}^n + \mathbf{g}^n v˙n=fn+gn

或写成分量形式（NED坐标系）：

v˙xnv˙ynv˙zn\]=\[fxnfynfzn\]+\[00g\] \\begin{bmatrix} \\dot{v}_x\^n \\\\ \\dot{v}_y\^n \\\\ \\dot{v}_z\^n \\end{bmatrix} = \\begin{bmatrix} f_x\^n \\\\ f_y\^n \\\\ f_z\^n \\end{bmatrix} + \\begin{bmatrix} 0 \\\\ 0 \\\\ g \\end{bmatrix} v˙xnv˙ynv˙zn = fxnfynfzn + 00g **注意Z轴方向** ：在NED坐标系中，gn=\[0,0,g\]T\\mathbf{g}\^n = \[0, 0, g\]\^Tgn=\[0,0,g\]T 表示重力指向下方（与Z轴正方向一致）。 #### 4.3 积分模型的进一步简化 在离散时间实现中，速度和位置更新方程为： **速度更新**： vk+1n=vkn+fkn⋅Ts+gnTs \\mathbf{v}_{k+1}\^n = \\mathbf{v}_k\^n + \\mathbf{f}_k\^n \\cdot T_s + \\mathbf{g}\^n T_s vk+1n=vkn+fkn⋅Ts+gnTs **位置更新**： pk+1n=pkn+vkn⋅Ts+12fkn⋅Ts2+12gnTs2 \\mathbf{p}_{k+1}\^n = \\mathbf{p}_k\^n + \\mathbf{v}_k\^n \\cdot T_s + \\frac{1}{2}\\mathbf{f}_k\^n \\cdot T_s\^2 + \\frac{1}{2}\\mathbf{g}\^n T_s\^2 pk+1n=pkn+vkn⋅Ts+21fkn⋅Ts2+21gnTs2 其中 TsT_sTs 为采样周期（LPE中通常为10ms，即100Hz）。 #### 4.4 关于旋转补偿与划桨效应的忽略 在高端惯导系统中，为消除圆锥误差（Coning Error）和划桨误差（Sculling Error），需要进行复杂的**旋转补偿** 和**划桨补偿**。这些误差源于姿态在积分周期内的变化。 **LPE的简化处理**： **假设4：积分周期内旋转矩阵恒定** Rbn(t)≈Rbn(tk),t∈\[tk,tk+1\]R_b\^n(t) \\approx R_b\^n(t_k), \\quad t \\in \[t_k, t_{k+1}\]Rbn(t)≈Rbn(tk),t∈\[tk,tk+1

即在单个预测周期内（10ms），假设飞行器姿态不变。对于小型无人机，在10ms内的姿态变化通常小于1°，引入的误差可接受。

假设5：忽略划桨效应

划桨误差由线运动和角运动的耦合引起，在振动环境中尤为明显。LPE采用简单积分，不进行划桨补偿，依靠：

较高的IMU采样率（通常1kHz降采样到100Hz）
后续卡尔曼滤波的平滑作用
外部传感器（GPS、光流）的周期性校正

来抑制相关误差。

与高精度惯导的对比：

特性	高精度惯导	LPE低成本方案
采样率	>200 Hz	100 Hz
划桨补偿	有	无
旋转补偿	有	无（假设RRR恒定）
计算复杂度	高	低
适用场景	高速、长时间纯惯导	低速、频繁外部校正

简化的合理性：

对于典型的多旋翼无人机（速度<20 m/s，姿态变化率<100°/s），上述简化引入的误差远小于MEMS传感器本身的噪声水平，是工程上的合理折中。

5. LPE状态空间建模

5.1 状态向量的定义

LPE估计的状态向量为：

x=[pnvnbbtz]=[pxpypzvxvyvzbxbybztz]T∈R10 \mathbf{x} = \begin{bmatrix} \mathbf{p}^n \\ \mathbf{v}^n \\ \mathbf{b}^b \\ t_z \end{bmatrix} = \begin{bmatrix} p_x & p_y & p_z & v_x & v_y & v_z & b_x & b_y & b_z & t_z \end{bmatrix}^T \in \mathbb{R}^{10} x= pnvnbbtz =[pxpypzvxvyvzbxbybztz]T∈R10

状态分量详解：

状态分量	符号	维度	描述	单位
位置	pn\mathbf{p}^npn	3	NED坐标系下的位置	m
速度	vn\mathbf{v}^nvn	3	NED坐标系下的速度	m/s
加速度计零偏	bb\mathbf{b}^bbb	3	机体系下的加速度计零偏	m/s²
地形高度	tzt_ztz	1	地形在海平面坐标系下的高度	m

关于加速度计零偏 bb\mathbf{b}^bbb：

零偏不是常数，会随温度、老化等因素漂移，必须实时在线估计。实际的"纯净"加速度：

atrueb=ameasb−bb \mathbf{a}{\text{true}}^b = \mathbf{a}{\text{meas}}^b - \mathbf{b}^b atrueb=ameasb−bb

关于地形高度 tzt_ztz：

t_z用于计算飞行器相对于地面的高度（Above Ground Level, AGL）：

hagl=tz−pz h_{\text{agl}} = t_z - p_z hagl=tz−pz

这对于起飞、着陆和地形跟随控制至关重要。

5.2 输入向量的定义

输入向量为导航系下的加速度：

u=[axnaynazn]T=Rbn⋅(ameasb−bb)+gn \mathbf{u} = \begin{bmatrix} a_x^n & a_y^n & a_z^n \end{bmatrix}^T = R_b^n \cdot (\mathbf{a}_{\text{meas}}^b - \mathbf{b}^b) + \mathbf{g}^n u=[axnaynazn]T=Rbn⋅(ameasb−bb)+gn

实际代码中，零偏的扣除在状态转移矩阵中隐式处理，输入 u\mathbf{u}u 为经姿态旋转并重力补偿后的导航系加速度。

5.3 连续时间状态方程

基于简化的比力方程，连续时间状态方程为：

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

其中 w\mathbf{w}w 为过程噪声，系统矩阵为：

A=[03×3I3×303×303×103×303×3−Rbn03×103×303×303×303×101×301×301×30]∈R10×10 A = \begin{bmatrix} 0_{3\times3} & I_{3\times3} & 0_{3\times3} & 0_{3\times1} \\ 0_{3\times3} & 0_{3\times3} & -R_b^n & 0_{3\times1} \\ 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times1} \\ 0_{1\times3} & 0_{1\times3} & 0_{1\times3} & 0 \end{bmatrix} \in \mathbb{R}^{10 \times 10} A= 03×303×303×301×3I3×303×303×301×303×3−Rbn03×301×303×103×103×10 ∈R10×10

输入矩阵为：

B=[03×3I3×303×301×3]∈R10×3 B = \begin{bmatrix} 0_{3\times3} \\ I_{3\times3} \\ 0_{3\times3} \\ 0_{1\times3} \end{bmatrix} \in \mathbb{R}^{10 \times 3} B= 03×3I3×303×301×3 ∈R10×3

各子矩阵的物理意义：

左上角 [0,I;0,0][0, I; 0, 0][0,I;0,0]：位置的变化率等于速度
A2,3=−RbnA_{2,3} = -R_b^nA2,3=−Rbn：速度变化受机体系零偏的影响（通过旋转矩阵转换）
零偏和地形行：建模为随机游走（导数为0，仅受噪声驱动）

5.4 离散时间状态方程与RK4积分

LPE采用四阶龙格-库塔法（RK4）进行数值积分，而非简单的欧拉法，以提高精度：

xk+1=xk+h6(k1+2k2+2k3+k4) \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)

其中 h=Tsh = T_sh=Ts 为采样周期，各阶段斜率为：

k1=Axk+Bukk2=A(xk+h2k1)+Bukk3=A(xk+h2k2)+Bukk4=A(xk+hk3)+Buk \begin{aligned} \mathbf{k}_1 &= A\mathbf{x}_k + B\mathbf{u}_k \\ \mathbf{k}_2 &= A(\mathbf{x}_k + \frac{h}{2}\mathbf{k}_1) + B\mathbf{u}_k \\ \mathbf{k}_3 &= A(\mathbf{x}_k + \frac{h}{2}\mathbf{k}_2) + B\mathbf{u}_k \\ \mathbf{k}_4 &= A(\mathbf{x}_k + h\mathbf{k}_3) + B\mathbf{u}_k \end{aligned} k1k2k3k4=Axk+Buk=A(xk+2hk1)+Buk=A(xk+2hk2)+Buk=A(xk+hk3)+Buk

PX4实现 （BlockLocalPositionEstimator.cpp）：

cpp 复制代码

void BlockLocalPositionEstimator::predict(const sensor_combined_s &imu)
{
    // 前文：获取旋转矩阵，转换加速度到导航系
    _R_att = matrix::Dcm<float>(matrix::Quatf(_sub_att.get().q));
    Vector3f a(imu.accelerometer_m_s2);
    _u = _R_att * a;
    _u(U_az) += CONSTANTS_ONE_G;
    
    // 更新状态空间矩阵（基于当前姿态）
    updateSSStates();
    
    // RK4积分
    float h = getDt();  // 采样周期，约0.01s
    Vector<float, n_x> k1, k2, k3, k4;
    
    k1 = dynamics(0, _x, _u);
    k2 = dynamics(h/2, _x + k1*h/2, _u);
    k3 = dynamics(h/2, _x + k2*h/2, _u);
    k4 = dynamics(h, _x + k3*h, _u);
    
    Vector<float, n_x> dx = (k1 + k2*2 + k3*2 + k4) * (h/6);
    
    // 条件积分：未初始化的方向不积分
    // 例如：没有GPS时，不积分水平位置
    if (!(_estimatorInitialized & EST_XY)) {
        dx(X_x) = 0; dx(X_vx) = 0;
        dx(X_y) = 0; dx(X_vy) = 0;
    }
    if (!(_estimatorInitialized & EST_Z)) {
        dx(X_z) = 0;
    }
    if (!(_estimatorInitialized & EST_TZ)) {
        dx(X_tz) = 0;
    }
    
    // 状态更新
    _x += dx;
    
    // 零偏限幅（防止估计发散）
    float bx = _x(X_bx);
    if (std::abs(bx) > BIAS_MAX) {
        _x(X_bx) = BIAS_MAX * bx / std::abs(bx);
    }
    // ... y, z方向类似
}

5.5 过程噪声模型

过程噪声 w\mathbf{w}w 的协方差矩阵 QQQ 为对角阵：

Q=diag(qp,qp,qp,qv,qv,qv,qb,qb,qb,qt) Q = \text{diag}(q_p, q_p, q_p, q_v, q_v, q_v, q_b, q_b, q_b, q_t) Q=diag(qp,qp,qp,qv,qv,qv,qb,qb,qb,qt)

对应PX4参数：

参数	符号	默认值	物理意义
`LPE_PN_P`	qpq_pqp	0.01	位置随机游走标准差
`LPE_PN_V`	qvq_vqv	0.02	速度随机游走标准差
`LPE_PN_B`	qbq_bqb	0.001	零偏随机游走标准差
`LPE_PN_T`	qtq_tqt	0.001	地形高度随机游走标准差

这些参数反映了模型不确定性的统计特性，在卡尔曼滤波中用于平衡预测与校正的权重。

6. 本章小结

核心概念回顾

低成本组合导航定位：LPE面向MEMS-IMU+GPS等低成本传感器，通过卡尔曼滤波实现误差抑制，在计算资源和精度之间取得平衡。
比力与简化方程：
- 加速度计测量的是比力 f=a−g\mathbf{f} = \mathbf{a} - \mathbf{g}f=a−g
- 简化后的运动方程：v˙n=fn+gn\dot{\mathbf{v}}^n = \mathbf{f}^n + \mathbf{g}^nv˙n=fn+gn
- 忽略地球自转、导航系旋转、重力变化，适用于低速无人机
积分简化假设：
- 积分周期内姿态恒定（RbnR_b^nRbn不变）
- 忽略划桨效应和旋转补偿
- 依靠高采样率和外部传感器校正保证精度
LPE状态空间：
- 10维状态：位置(3)、速度(3)、加速度计零偏(3)、地形高度(1)
- 3维输入：导航系加速度（经姿态旋转和重力补偿）
- 离散化采用RK4积分保证数值精度

关键公式汇总

公式	含义
f=a−g\mathbf{f} = \mathbf{a} - \mathbf{g}f=a−g	比力定义
fn=Rbn⋅fb\mathbf{f}^n = R_b^n \cdot \mathbf{f}^bfn=Rbn⋅fb	机体系到导航系的比力转换
v˙n=fn+gn\dot{\mathbf{v}}^n = \mathbf{f}^n + \mathbf{g}^nv˙n=fn+gn	简化后的加速度方程
x=[px,py,pz,vx,vy,vz,bx,by,bz,tz]T\mathbf{x} = [p_x, p_y, p_z, v_x, v_y, v_z, b_x, b_y, b_z, t_z]^Tx=[px,py,pz,vx,vy,vz,bx,by,bz,tz]T	LPE状态向量

关于我们：

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

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