Kalman Filter 公式推导过程

卡尔曼滤波是一种用于处理线性高斯系统的估计理论，其目标是提供最佳线性无偏估计（BLUE）。它的核心思想是通过结合系统的状态模型和观测模型，不断更新对系统状态的估计，以减小不确定性。 1. **系统模型**： - **过程方程**：\( x_{k+1} = A_k x_k + B_k u_k + w_k \) 这里，\( x_k \) 是系统在时间步 \( k \) 的状态，\( A_k \) 是状态转移矩阵，\( B_k \) 是控制输入矩阵，\( u_k \) 是控制输入，\( w_k \) 是过程噪声，假设服从高斯分布 \( N(0, Q_k) \)，其中 \( Q_k \) 是过程噪声的协方差矩阵。 2. **观测模型**： - **测量方程**：\( z_{k+1} = H_{k+1} x_{k+1} + v_{k+1} \) 在这里，\( z_{k+1} \) 是时间步 \( k+1 \) 的观测值，\( H_{k+1} \) 是观测矩阵，\( v_{k+1} \) 是测量误差，同样假设服从高斯分布 \( N(0, R_k) \)，其中 \( R_k \) 是测量噪声的协方差矩阵。 3. **状态更新**： - **线性无偏误差方程**：\( \tilde{x}_{k+1} = x - \hat{x}_{k+1} \) 其中，\( \hat{x}_{k+1} \) 是对系统状态的预测，\( \tilde{x}_{k+1} \) 是状态误差。为了得到无偏估计，我们希望 \( E[\tilde{x}_{k+1}] = 0 \)。这导出了状态更新方程： \( \hat{x}_{k+1|k+1} = (I - K_{k+1} H_{k+1}) \hat{x}_{k+1|k} + K_{k+1} z_{k+1} \) 其中，\( \hat{x}_{k+1|k} \) 是先验估计，\( \hat{x}_{k+1|k+1} \) 是后验估计，\( K_{k+1} \) 是卡尔曼增益。 4. **时间推进阶段**： - 如何从 \( \hat{x}_{k|k} \) 计算 \( \hat{x}_{k+1|k} \)？ 线性情况下，\( \hat{x}_{k+1|k} = A_k \hat{x}_{k|k} \)。 - 如何确保 \( \hat{x}_{k+1|k} \) 也是无偏的？ 解决方案是 \( S_k = A_k \)，这样线性无偏估计就包括了时间推进步骤。 5. **最小方差估计**： - 状态误差的方差表示为 \( P_{k/k} = E[\tilde{x}_{k/k} \tilde{x}_k^T/k] \)，其中 \( x_k \) 是实际系统状态，\( \hat{x}_{k/k} \) 是时间步 \( k \) 的估计状态向量。 - 通过假设 \( P_{k/k} \) 和 \( P_{k+1/k} \) 已知，我们可以表达 \( P_{k+1/k+1} \) 关于 \( K_{k+1} \) 的函数： \( P_{k+1/k+1} = (I - K_{k+1} H_{k+1}) P_{k+1/k} (I - K_{k+1} H_{k+1})^T + K_{k+1} R_{k+1} K_{k+1}^T \) 这里的 \( P_{k+1/k+1} \) 表示当前状态估计的不确定性（协方差）矩阵，而 \( P_{k+1/k} \) 是先验估计的不确定性。 6. **卡尔曼增益**： 卡尔曼增益 \( K_{k+1} \) 是一个关键参数，它决定了如何将观测值融合进状态估计。它的计算涉及当前状态估计的不确定性 \( P_{k+1/k} \) 和观测噪声的不确定性 \( R_{k+1} \)。 7. **卡尔曼滤波的迭代过程**： - 在每个时间步，先用上一时刻的状态和过程模型预测下一时刻的状态。 - 然后，利用观测模型和卡尔曼增益来更新预测，得到更准确的状态估计。 - 更新状态的不确定性矩阵，准备下一次迭代。 卡尔曼滤波算法在许多领域有广泛的应用，如自动驾驶、航空航天、信号处理和图像处理等，它能够有效地处理噪声并提供最优的线性估计。