Kalman Filter卡尔曼滤波器之估计误差的协方差矩阵推导

前言：在不确定性中寻找最优估计，在预测与测量之间建立平衡

摘要：卡尔曼滤波是一种在不确定性环境中寻找最优估计的递推算法。其核心思想是在预测与测量之间建立动态平衡：通过状态方程进行预测，利用测量方程进行校正，卡尔曼增益作为桥梁自适应地权衡两者的可信度。算法以最小化估计误差方差为准则，通过协方差矩阵的递推更新，将带噪声的预测值与观测值最优融合，实现对系统真实状态的精确估计。这种预测-校正的迭代机制为多传感器融合和动态系统状态估计提供了理论框架。

卡尔曼滤波推导

基本定义

$$\begin{array}{rl}& A:状态矩阵\\ & B:控制矩阵\\ & H:传输矩阵\\ & w_k:计算误差\\ & V_k:测量误差\\ & Q:过程噪声w_k的协方差矩阵\\ & R:测量噪声V_k的协方差矩阵\end{array}$$

状态方程与估计

状态方程：
$$X_k=A{X}_{k-1}+B{U}_k+w_{k-1}$$

先验估计：
$${\hat{X}}_k^{-}=A{\hat{X}}_{k-1}+B{U}_{k-1}$$

其中 $A$ 是系统模型建立的状态转移矩阵，${\hat{X}}_{k-1}$ 是上一次的估计值，$B{U}_{k-1}$ 是输入值。

后验估计：
$${\hat{X}}_k={\hat{X}}_k^{-}+K_k(Z_k-H{\hat{X}}_k^{-})$$

其中测量值 $Z_k$，$H$ 是测量结果的状态方程。

卡尔曼增益

卡尔曼增益：
$$K_k=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R}$$

$P_k^{-}$ 是误差 $e_k$ 的协方差矩阵 $P_k$ 的先验：
$$P_k^{-}=E[e_k^{-}\cdot e_k^{-T}]$$

误差定义

先验误差：
$$e_k^{-}=X_k-{\hat{X}}_k^{-}$$

后验误差：
$$e_k=X_k-{\hat{X}}_k$$

先验误差推导

结合状态方程：
$$X_k=A{X}_{k-1}+B{U}_{k-1}+w_{k-1}$$

先验估计值：
$${\hat{X}}_k^{-}=A{\hat{X}}_{k-1}+B{U}_{k-1}$$

后验估计值：
$${\hat{X}}_k={\hat{X}}_k^{-}+K_k(Z_k-H{\hat{X}}_k^{-}),K_k\in [0,H^{-1}]$$

因此先验误差：
$$￥\begin{array}{rl}{e}_k^{-}& =X_k-{\hat{X}}_k^{-}\\ & =A{X}_{k-1}+B{U}_{k-1}+w_{k-1}-(A{\hat{X}}_{k-1}+B{U}_{k-1})\\ & =A(X_{k-1}-{\hat{X}}_{k-1})+w_{k-1}\\ & =A{e}_{k-1}+w_{k-1}\end{array}$$

先验协方差矩阵推导

误差 $e_k$ 的协方差矩阵的先验：
$$￥\begin{array}{rl}{P}_k^{-}& =E[e_k^{-}\cdot e_k^{-T}]\\ & =E[(A{e}_{k-1}+w_{k-1})\cdot (A{e}_{k-1}+w_{k-1}{)}^T]\\ & =E[A{e}_{k-1}{e}_{k-1}^T{A}^T+A{e}_{k-1}{w}_{k-1}^T+w_{k-1}{e}_{k-1}^T{A}^T+w_{k-1}{w}_{k-1}^T]|\end{array}$$

关键步骤： 考虑交叉项
$$E(A{e}_{k-1}{w}_{k-1}^T)$$

根据 $e_k$ 的定义式：
$$e_{k-1}=X_{k-1}-{\hat{X}}_{k-1}$$

即 $e_{k-1}$ 作用在 $X_{k-1}$，而 $w_{k-1}$ 在状态方程中
$$X_k=A{X}_{k-1}+B{U}_{k-1}+w_{k-1}$$
作用于 $X_k$。

因此 $e_{k-1}$ 和 $w_{k-1}$ 是相互独立的，且它们遵从正态分布、期望为0：
$$E(e_{k-1}{w}_{k-1}^T)=AE(e_{k-1})E(w_{k-1}^T)=0$$

所以：
$$￥\begin{array}{rl}{P}_k^{-}& =AE(e_{k-1}{e}_{k-1}^T)A^T+E(w_{k-1}{w}_{k-1}^T)\\ & =A{P}_{k-1}{A}^T+Q\end{array}$$

卡尔曼滤波算法

至此，我们可以用卡尔曼滤波来估计状态变量的值。

卡尔曼滤波分为：预测 + 校正

预测步骤

先验估计：
$${\hat{X}}_k^{-}=A{\hat{X}}_{k-1}+B{U}_{k-1}$$

先验误差协方差矩阵：
$$P_k^{-}=A{P}_{k-1}{A}^T+Q$$

校正步骤

卡尔曼增益：
$$K_k=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R}$$

后验估计：
$${\hat{X}}_k={\hat{X}}_k^{-}+K_k(Z_k-H{\hat{X}}_k^{-})$$

更新协方差误差：
$$P_k=(I-K_{k}H)P_k^{-}$$

协方差矩阵的进一步化简

前面推出了：
$$P_k=P_k^{-}-K_{k}H{P}_k^{-}-P_k^{-}{H}^T{K}_k^T+K_{k}H{P}_k^{-}{H}^T{K}_k^T+K_{k}R{K}_k^T$$

合并一下：
$$K_k(H{P}_k^{-}{H}^T+R)K_k^T$$

将 $K_k$ 的表达式代入：
$$\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R}(H{P}_k^{-}{H}^T+R)K_k^T=P_k^{-}{H}^T{K}_k^T$$

因此：
$$￥\begin{array}{rl}{P}_k& =P_k^{-}-K_{k}H{P}_k^{-}-P_k^{-}{H}^T{K}_k^T+P_k^{-}{H}^T{K}_k^T\\ & =P_k^{-}-K_{k}H{P}_k^{-}\\ & =(I-K_{k}H)P_k^{-}\end{array}$$

总结

有了这五项公式，就可以得到最优估计值 ${\hat{X}}_k$，前提需要给出 ${\hat{X}}_0$ 初始值和 $P_0$ 初始值。

完整算法流程

1. 初始化： ${\hat{X}}_0$, $P_0$

2. 预测：

   • ${\hat{X}}_k^{-}=A{\hat{X}}_{k-1}+B{U}_{k-1}$
   • $P_k^{-}=A{P}_{k-1}{A}^T+Q$

3. 校正：

   • $K_k=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R}$
   • ${\hat{X}}_k={\hat{X}}_k^{-}+K_k(Z_k-H{\hat{X}}_k^{-})$
   • $P_k=(I-K_{k}H)P_k^{-}$

卡尔曼滤波的理论基础与最优估计准则

卡尔曼滤波是一种动态的递推估计方法，适用于有噪声的动态系统状态估计。它不仅考虑当前观测，还结合系统的动态模型和历史估计，递推地给出最优估计。从信息论的角度来看，卡尔曼滤波实质上是一个贝叶斯滤波器在线性高斯系统下的解析解，它通过不断更新状态的后验概率分布来实现对系统状态的最优估计。

卡尔曼滤波采用的准则是使得误差的方差最小，即最优估计准则，这也被称为最小均方误差（MMSE, Minimum Mean Square Error）准则。如果后验估计值和真实值越接近，那么误差的变化就很小，即误差的方差很小。数学上，这可以表示为最小化代价函数：

$$J=E[(X_k-{\hat{X}}_k{)}^T(X_k-{\hat{X}}_k)]$$

其中 $E[\cdot ]$ 表示数学期望，这个代价函数正是估计误差的均方值。在控制理论中，这种准则保证了估计器在统计意义上的最优性，即在所有线性无偏估计器中，卡尔曼滤波给出的估计具有最小的误差协方差。

进一步推导，考虑到误差会有很多不同的分量（因为状态量不同，比如说此例子中有状态量 $X_1$ 表示位置，$X_2$ 表示速度，那么它们就分别有误差 $e_1$ 和 $e_2$）。要使得总误差方差最小，那么误差各个分量的方差之和加起来就要最小。而"误差各分量的方差之和"正好是误差的协方差矩阵的主对角线之和——迹（trace），即：

$$J=E[e_k^T{e}_k]=E[\text{tr}(e_k{e}_k^T)]=\text{tr}(E[e_k{e}_k^T])=\text{tr}(P_k)$$

这里利用了迹的循环性质和期望的线性性质。最小化 $\text{tr}(P_k)$ 等价于最小化所有状态分量的估计误差方差之和，这正是卡尔曼滤波的优化目标。在多维状态空间中，这种标量化的代价函数使得优化问题具有明确的数学解。

因此我们引入了过程噪声 $w_k$ 的协方差矩阵 $Q$，观测噪声 $V_k$ 的协方差矩阵 $R$。它们的方差越大，说明误差越大，即越不可以相信。在卡尔曼滤波框架中，$Q$ 矩阵刻画了系统动态模型的不确定性，反映了建模误差和未建模动态的影响；而 $R$ 矩阵则量化了传感器测量的不确定性，体现了测量噪声的统计特性。这两个协方差矩阵本质上定义了滤波器对模型预测和传感器测量的信任程度：当 $Q$ 较大时，滤波器会更依赖测量值进行校正；当 $R$ 较大时，滤波器则更相信模型预测。卡尔曼增益 $K_k$ 正是通过 $P_k^{-}$、$Q$ 和 $R$ 的相对大小自适应地调整这种信任权衡，实现了预测和测量的最优融合。从现代估计理论的视角看，这种基于协方差传播的递推估计方法为后续的扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）等非线性滤波算法奠定了坚实的理论基础，也为多传感器信息融合、目标跟踪、导航定位等领域提供了强大的数学工具。

卡尔曼增益的物理意义

卡尔曼增益 $K_k$ 是卡尔曼滤波的核心，它决定了我们有多相信预测值，还是有多相信测量值：

$$K_k=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R}$$

• 当 $K_k\to 0$ 时，说明我们更相信预测值（模型），测量值的权重很小
• 当 $K_k\to 1$ 时，说明我们更相信测量值（传感器），预测值的权重很小

这个增益是自适应计算的，会根据预测误差协方差 $P_k^{-}$ 和测量噪声 $R$ 动态调整。

一维卡尔曼滤波

对于一维系统（单状态量），卡尔曼滤波公式可以简化为：

$$\begin{array}{rl}{\hat{X}}_k^{-}& ={\hat{X}}_{k-1}\\ P_k^{-}& =P_{k-1}+Q\\ K_k& =\frac{P_k^{-}}{P_k^{-}+R}\\ {\hat{X}}_k& ={\hat{X}}_k^{-}+K_k(Z_k-{\hat{X}}_k^{-})\\ P_k& =(1-K_k)P_k^{-}\end{array}$$

这是最简单的卡尔曼滤波形式，常用于单一传感器的信号滤波，比如气压高度的平滑估计。

二维卡尔曼滤波：高度与速度估计

在实际的无人机应用中，我们通常不仅要估计高度，还要同时估计垂直速度。这就需要用到二维卡尔曼滤波。

系统建模

状态向量：
$$X_k=\left[\begin{array}{c}{h}_k\\ v_k\end{array}\right]$$

其中 $h_k$ 是高度，$v_k$ 是垂直速度。

状态转移方程：
$$X_k=A{X}_{k-1}+B{u}_k+w_{k-1}$$

其中：
$$A=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right],B=\left[\begin{array}{c}\frac{1}{2}\Delta t^2\\ \Delta t\end{array}\right]$$

• $A$ 描述了高度和速度的运动学关系：$h_k=h_{k-1}+v_{k-1}\cdot \Delta t$
• $B$ 描述了加速度输入对状态的影响（来自IMU的垂直加速度）
• $u_k$ 是控制输入（垂直加速度测量值）

测量方程：
$$Z_k=H{X}_k+V_k$$

对于气压计测量高度：
$$H=\left[\begin{array}{cc}1& 0\end{array}\right]$$

这表示我们只能直接测量高度，不能直接测量速度。速度需要通过卡尔曼滤波从高度变化中推算出来。

协方差矩阵的选择

过程噪声协方差 $Q$：
$$Q=\left[\begin{array}{cc}{q}_h& 0\\ 0& q_v\end{array}\right]$$

• $q_h$：高度的过程噪声方差，通常取 $0.01\sim 0.1$
• $q_v$：速度的过程噪声方差，通常取 $0.1\sim 1.0$

速度的过程噪声通常设置得比高度大，因为加速度测量（IMU）存在较大误差，会累积到速度估计中。

测量噪声协方差 $R$：

$R$ 取决于传感器精度。对于气压计：

• 室内环境：$R\approx 0.5\sim 1.0$ （较稳定）
• 室外环境：$R\approx 2.0\sim 5.0$ （受风、温度影响）

代码实现：二维高度-速度卡尔曼滤波

数据结构定义

typedef struct {
    float dt;              // 采样时间间隔
    float x[2];            // 状态向量 [高度, 速度]
    float P[2][2];         // 误差协方差矩阵
    float A[2][2];         // 状态转移矩阵 (2x2)
    float B[2];            // 控制输入矩阵 (2x1)
    float H[2];            // 测量矩阵 (1x2)
    float Q[2][2];         // 过程噪声协方差矩阵 (2x2)
    float R;               // 测量噪声协方差
    float K[2];            // 卡尔曼增益 (2x1)
} KalmanFilter;

初始化函数

/**
 * @brief 初始化卡尔曼滤波器
 * @param kf 卡尔曼滤波器结构体指针
 * @param dt 采样周期 (秒)
 * @param process_noise_h 高度过程噪声标准差
 * @param process_noise_v 速度过程噪声标准差
 * @param measurement_noise 测量噪声标准差
 */
void kalman_init(KalmanFilter *kf, float dt, 
                 float process_noise_h, 
                 float process_noise_v,
                 float measurement_noise)
{
    kf->dt = dt;
    
    // 初始化状态向量 [高度, 速度]
    kf->x[0] = 0.0f;  // 初始高度
    kf->x[1] = 0.0f;  // 初始速度
    
    // 初始化误差协方差矩阵（表示初始估计的不确定性）
    kf->P[0][0] = 1.0f;   // 高度方差
    kf->P[0][1] = 0.0f;   // 高度-速度协方差
    kf->P[1][0] = 0.0f;   // 速度-高度协方差
    kf->P[1][1] = 1.0f;   // 速度方差
    
    // 状态转移矩阵 A
    // [1  dt]   表示: h(k) = h(k-1) + v(k-1)*dt
    // [0   1]          v(k) = v(k-1)
    kf->A[0][0] = 1.0f;
    kf->A[0][1] = dt;
    kf->A[1][0] = 0.0f;
    kf->A[1][1] = 1.0f;
    
    // 控制输入矩阵 B (将加速度积分到状态中)
    // [0.5*dt^2]   表示: h = h + 0.5*a*dt^2
    // [dt      ]          v = v + a*dt
    kf->B[0] = 0.5f * dt * dt;
    kf->B[1] = dt;
    
    // 测量矩阵 H (只测量高度)
    // [1  0]  表示: z = h
    kf->H[0] = 1.0f;
    kf->H[1] = 0.0f;
    
    // 过程噪声协方差矩阵 Q
    kf->Q[0][0] = process_noise_h * process_noise_h;
    kf->Q[0][1] = 0.0f;
    kf->Q[1][0] = 0.0f;
    kf->Q[1][1] = process_noise_v * process_noise_v;
    
    // 测量噪声协方差 R
    kf->R = measurement_noise * measurement_noise;
}

预测步骤

/**
 * @brief 卡尔曼滤波预测步骤
 * @param kf 卡尔曼滤波器结构体指针
 * @param accel_z 垂直加速度输入 (m/s^2)
 */
void kalman_predict(KalmanFilter *kf, float accel_z)
{
    // 1. 状态预测: x_prior = A*x + B*u
    float x_prior[2];
    x_prior[0] = kf->A[0][0] * kf->x[0] + kf->A[0][1] * kf->x[1] + kf->B[0] * accel_z;
    x_prior[1] = kf->A[1][0] * kf->x[0] + kf->A[1][1] * kf->x[1] + kf->B[1] * accel_z;
    
    // 2. 协方差预测: P_prior = A*P*A^T + Q
    float P_prior[2][2];
    
    // 计算 A*P
    float AP[2][2];
    AP[0][0] = kf->A[0][0] * kf->P[0][0] + kf->A[0][1] * kf->P[1][0];
    AP[0][1] = kf->A[0][0] * kf->P[0][1] + kf->A[0][1] * kf->P[1][1];
    AP[1][0] = kf->A[1][0] * kf->P[0][0] + kf->A[1][1] * kf->P[1][0];
    AP[1][1] = kf->A[1][0] * kf->P[0][1] + kf->A[1][1] * kf->P[1][1];
    
    // 计算 A*P*A^T
    P_prior[0][0] = AP[0][0] * kf->A[0][0] + AP[0][1] * kf->A[0][1];
    P_prior[0][1] = AP[0][0] * kf->A[1][0] + AP[0][1] * kf->A[1][1];
    P_prior[1][0] = AP[1][0] * kf->A[0][0] + AP[1][1] * kf->A[0][1];
    P_prior[1][1] = AP[1][0] * kf->A[1][0] + AP[1][1] * kf->A[1][1];
    
    // 加上过程噪声: P_prior = A*P*A^T + Q
    P_prior[0][0] += kf->Q[0][0];
    P_prior[0][1] += kf->Q[0][1];
    P_prior[1][0] += kf->Q[1][0];
    P_prior[1][1] += kf->Q[1][1];
    
    // 更新状态和协方差
    kf->x[0] = x_prior[0];
    kf->x[1] = x_prior[1];
    kf->P[0][0] = P_prior[0][0];
    kf->P[0][1] = P_prior[0][1];
    kf->P[1][0] = P_prior[1][0];
    kf->P[1][1] = P_prior[1][1];
}

更新步骤

/**
 * @brief 卡尔曼滤波更新步骤
 * @param kf 卡尔曼滤波器结构体指针
 * @param measurement 测量值（高度）
 */
void kalman_update(KalmanFilter *kf, float measurement)
{
    // 1. 计算卡尔曼增益: K = P*H^T / (H*P*H^T + R)
    
    // 计算 H*P (1x2 矩阵)
    float HP[2];
    HP[0] = kf->H[0] * kf->P[0][0] + kf->H[1] * kf->P[1][0];
    HP[1] = kf->H[0] * kf->P[0][1] + kf->H[1] * kf->P[1][1];
    
    // 计算 H*P*H^T (标量)
    float HPHT = HP[0] * kf->H[0] + HP[1] * kf->H[1];
    
    // 计算 S = H*P*H^T + R (新息协方差)
    float S = HPHT + kf->R;
    
    // 计算 P*H^T (2x1 矩阵)
    float PHT[2];
    PHT[0] = kf->P[0][0] * kf->H[0] + kf->P[0][1] * kf->H[1];
    PHT[1] = kf->P[1][0] * kf->H[0] + kf->P[1][1] * kf->H[1];
    
    // 计算卡尔曼增益 K = P*H^T / S
    kf->K[0] = PHT[0] / S;
    kf->K[1] = PHT[1] / S;
    
    // 2. 状态更新: x = x + K*(z - H*x)
    
    // 计算预测测量值: H*x
    float predicted_measurement = kf->H[0] * kf->x[0] + kf->H[1] * kf->x[1];
    
    // 计算新息(innovation): y = z - H*x
    float innovation = measurement - predicted_measurement;
    
    // 状态更新
    kf->x[0] += kf->K[0] * innovation;
    kf->x[1] += kf->K[1] * innovation;
    
    // 3. 协方差更新: P = (I - K*H)*P
    
    // 计算 K*H (2x2 矩阵)
    float KH[2][2];
    KH[0][0] = kf->K[0] * kf->H[0];
    KH[0][1] = kf->K[0] * kf->H[1];
    KH[1][0] = kf->K[1] * kf->H[0];
    KH[1][1] = kf->K[1] * kf->H[1];
    
    // 计算 I - K*H
    float I_KH[2][2];
    I_KH[0][0] = 1.0f - KH[0][0];
    I_KH[0][1] = -KH[0][1];
    I_KH[1][0] = -KH[1][0];
    I_KH[1][1] = 1.0f - KH[1][1];
    
    // 计算 (I - K*H)*P
    float P_new[2][2];
    P_new[0][0] = I_KH[0][0] * kf->P[0][0] + I_KH[0][1] * kf->P[1][0];
    P_new[0][1] = I_KH[0][0] * kf->P[0][1] + I_KH[0][1] * kf->P[1][1];
    P_new[1][0] = I_KH[1][0] * kf->P[0][0] + I_KH[1][1] * kf->P[1][0];
    P_new[1][1] = I_KH[1][0] * kf->P[0][1] + I_KH[1][1] * kf->P[1][1];
    
    // 更新协方差矩阵
    kf->P[0][0] = P_new[0][0];
    kf->P[0][1] = P_new[0][1];
    kf->P[1][0] = P_new[1][0];
    kf->P[1][1] = P_new[1][1];
}

使用示例

// 全局变量
KalmanFilter altitude_kf;

void altitude_estimation_init(void)
{
    // 初始化卡尔曼滤波器
    // dt = 0.01s (100Hz更新频率)
    // 高度过程噪声 = 0.05m
    // 速度过程噪声 = 0.5m/s
    // 测量噪声 = 1.0m
    kalman_init(&altitude_kf, 0.01f, 0.05f, 0.5f, 1.0f);
}

void altitude_estimation_update(float baro_altitude, float accel_z)
{
    // 1. 预测步骤（使用IMU加速度）
    kalman_predict(&altitude_kf, accel_z);
    
    // 2. 更新步骤（使用气压计测量值）
    kalman_update(&altitude_kf, baro_altitude);
    
    // 3. 获取估计结果
    float estimated_altitude = altitude_kf.x[0];
    float estimated_velocity = altitude_kf.x[1];
    
    // 使用估计值进行控制...
}

PX4悬停油门估计中的卡尔曼滤波

在PX4中，悬停油门（hover thrust）的估计是一个典型的一维卡尔曼滤波应用。这个估计对于多旋翼的高度控制至关重要。

为什么需要估计悬停油门？

1. 电池电压变化：随着电池放电，电压下降，需要更大的油门来维持悬停
2. 负载变化：携带不同重量的载荷时，悬停油门不同
3. 环境变化：海拔高度、温度、气压影响螺旋桨效率

PX4中的实现思路

PX4的悬停油门估计器使用了一维卡尔曼滤波，其核心思想是：

状态量：悬停油门值 ${\theta }_{hover}$

测量值：当垂直速度接近0且垂直加速度接近0时，当前油门值即为悬停油门

状态方程：
$${\theta }_{hover,k}={\theta }_{hover,k-1}+w_{k-1}$$

这是一个随机游走模型，假设悬停油门缓慢变化。

测量方程：
$$z_k={\theta }_{hover,k}+v_k$$

当满足悬停条件时，测量值 $z_k$ 就是当前油门值。

简化实现代码

typedef struct {
    float hover_thrust;      // 悬停油门估计值 [0, 1]
    float P;                 // 估计误差方差
    float Q;                 // 过程噪声方差
    float R;                 // 测量噪声方差
    float K;                 // 卡尔曼增益
} HoverThrustEstimator;

/**
 * @brief 初始化悬停油门估计器
 */
void hover_thrust_init(HoverThrustEstimator *est)
{
    est->hover_thrust = 0.5f;  // 初始估计值50%油门
    est->P = 0.1f;              // 初始误差方差
    est->Q = 0.0001f;           // 过程噪声（悬停油门变化很慢）
    est->R = 0.01f;             // 测量噪声
}

/**
 * @brief 更新悬停油门估计
 * @param est 估计器结构体
 * @param current_thrust 当前油门值 [0, 1]
 * @param velocity_z 垂直速度 (m/s)
 * @param accel_z 垂直加速度 (m/s^2)
 * @param dt 时间间隔 (s)
 */
void hover_thrust_update(HoverThrustEstimator *est, 
                        float current_thrust,
                        float velocity_z, 
                        float accel_z,
                        float dt)
{
    // 1. 预测步骤
    // 状态预测（悬停油门基本不变）
    float hover_thrust_prior = est->hover_thrust;
    
    // 协方差预测
    float P_prior = est->P + est->Q;
    
    // 2. 判断是否处于悬停状态
    float velocity_threshold = 0.3f;  // 速度阈值 m/s
    float accel_threshold = 1.0f;     // 加速度阈值 m/s^2
    
    bool is_hovering = (fabsf(velocity_z) < velocity_threshold) && 
                       (fabsf(accel_z) < accel_threshold);
    
    // 3. 如果处于悬停状态，进行测量更新
    if (is_hovering) {
        // 计算卡尔曼增益
        est->K = P_prior / (P_prior + est->R);
        
        // 状态更新（使用当前油门作为测量值）
        est->hover_thrust = hover_thrust_prior + 
                           est->K * (current_thrust - hover_thrust_prior);
        
        // 协方差更新
        est->P = (1.0f - est->K) * P_prior;
        
        // 限制悬停油门范围
        if (est->hover_thrust < 0.2f) est->hover_thrust = 0.2f;
        if (est->hover_thrust > 0.8f) est->hover_thrust = 0.8f;
    } else {
        // 不处于悬停状态，只进行预测，不更新
        est->hover_thrust = hover_thrust_prior;
        est->P = P_prior;
    }
}

PX4实际代码参考

在PX4固件中，悬停油门估计位于 MulticopterPositionControl 模块：

// src/modules/mc_pos_control/PositionControl/PositionControl.cpp

void PositionControl::updateHoverThrust(const float thrust_setpoint)
{
    // 只有在接近悬停状态时才更新
    if (_hover_thrust_initialized && 
        fabsf(_vel(2)) < 0.3f &&  // 垂直速度小
        fabsf(_acc(2)) < 1.0f) {  // 垂直加速度小
        
        // 一阶低通滤波（简化的卡尔曼滤波）
        const float alpha = 0.01f;  // 滤波系数
        _hover_thrust = _hover_thrust * (1.0f - alpha) + 
                       thrust_setpoint * alpha;
        
        // 限幅
        _hover_thrust = math::constrain(_hover_thrust, 0.2f, 0.8f);
    }
}

PX4实际使用的是简化版本——指数加权移动平均（EWMA），这可以看作是卡尔曼滤波在稳态下的近似。

参数调优指南

Q矩阵（过程噪声）调优

• $q_h$ 过小：滤波器对模型预测过于自信，响应慢，对突变不敏感
• $q_h$ 过大：滤波器对模型不信任，估计值会跟随测量值剧烈波动

推荐起点：

• 高度：$q_h=0.01\sim 0.1$
• 速度：$q_v=0.1\sim 1.0$

R矩阵（测量噪声）调优

• $R$ 过小：过于相信传感器，估计值会受噪声干扰
• $R$ 过大：不相信传感器，滤波效果不明显

推荐起点：

• 气压计：$R=1.0\sim 5.0$
• 超声波：$R=0.05\sim 0.2$
• GPS高度：$R=5.0\sim 10.0$

调参方法

1. 先调R：根据传感器datasheet和实际测试确定
2. 再调Q：观察估计值的响应速度和平滑度
3. 迭代优化：在实际飞行中微调，平衡响应速度和稳定性

实用技巧

// 动态调整测量噪声（自适应卡尔曼滤波）
void adaptive_R_update(KalmanFilter *kf, float innovation)
{
    // 如果新息过大，说明测量可能不可靠，增大R
    float innovation_threshold = 5.0f;
    
    if (fabsf(innovation) > innovation_threshold) {
        kf->R *= 1.5f;  // 增大测量噪声
    } else {
        kf->R *= 0.95f;  // 逐渐恢复
    }
    
    // 限制R的范围
    if (kf->R < 1.0f) kf->R = 1.0f;
    if (kf->R > 10.0f) kf->R = 10.0f;
}

扩展卡尔曼滤波（EKF）

对于非线性系统（如姿态估计），需要使用扩展卡尔曼滤波（EKF）。PX4的姿态估计器 ecl_ekf 就是一个典型应用。

EKF的核心思想是：在每个时间步对非线性函数进行线性化（泰勒展开），然后应用标准卡尔曼滤波。

线性化通过雅可比矩阵实现：
$$F_k=\frac{\partial f}{\partial x}{|}_{x={\hat{x}}_{k-1}},H_k=\frac{\partial h}{\partial x}{|}_{x={\hat{x}}_k^{-}}$$

这将在后续文章中详细展开。

总结

卡尔曼滤波的核心优势：

1. 递推算法：只需存储上一时刻的状态，内存占用小
2. 最优估计：在线性高斯系统下给出最小方差估计
3. 融合多传感器：自然地融合模型预测和传感器测量
4. 自适应权重：卡尔曼增益自动平衡预测和测量

在无人机应用中，卡尔曼滤波广泛用于：

• 高度估计（融合气压计、IMU、超声波、GPS）
• 姿态估计（融合陀螺仪、加速度计、磁力计）
• 位置估计（融合GPS、视觉、惯导）
• 参数估计（悬停油门、电机常数、质量估计）

掌握卡尔曼滤波是理解现代飞控算法的基础，也是进阶学习EKF、UKF等高级估计算法的必经之路。