PX4 EKF：状态与协方差传播解析

"这篇文档主要介绍了协方差的传播在无人机估计与控制库(ECL EKF)中的应用，特别是在PX4飞行控制器中。文档详细探讨了状态向量的传播和协方差的传播，并给出了相关矩阵的计算公式。" 在EKF（扩展卡尔曼滤波）中，协方差的传播是估算系统不确定性的重要部分。状态向量的传播描述了如何通过时间将当前状态的不确定性转换到下一时刻。在无人机导航系统中，如PX4使用的ECL EKF，状态向量包括四元数（q）、速度（v）、位置（p）、角度偏差（∆θb）、速度偏差（∆vb）以及环境因素如地磁（mNED）和风速（vwind）等。 状态向量的传播涉及到转移矩阵F，它表示了状态在时间步长之间的动态关系。例如，四元数状态的传播考虑到旋转的影响，速度状态的传播考虑了加速度，而位置状态的传播则基于速度和时间。转移矩阵F和输入矩阵G（通常与系统噪声相关）一起用于计算状态误差矩阵Q，该矩阵反映了状态变量之间的不确定性。 协方差的传播公式是Q = G * Ω * G^T，其中Ω是过程噪声的协方差矩阵，G是转移矩阵F关于状态向量的雅可比矩阵。这个公式表明，当前状态的不确定性将如何通过系统动力学和噪声传递到下一次预测。 在实际应用中，如PX4的EKF，会针对每种传感器（如IMU、GPS、气压计、磁力计、光流和ZED相机）进行特定的传播和更新规则。对于每种传感器，都有相应的观测矩阵Jacobian来描述其测量值与状态向量之间的关系，这些Jacobian矩阵在更新步骤中用于融合传感器数据，从而更准确地估计状态。 例如，GPS更新会校正位置估计，气压计更新会提供高度信息，磁力计更新则用于修正地磁模型并估计航向。每个传感器的观测矩阵Jacobian的推导对于理解它们如何影响状态估计至关重要。 通过这样的方式，EKF能够综合多种传感器数据，即使在存在噪声和不准确性的环境下，也能有效地估计无人机的状态，从而实现稳定和精确的飞行控制。在PX4中，EKF的参数调优对于确保系统的性能和可靠性是至关重要的。