kalman滤波公式推导

**卡尔曼滤波**是一种在线估计系统状态的优化算法，尤其适用于处理带有噪声的数据序列。在信号处理和控制理论中，卡尔曼滤波器是最优线性无偏估计器（Minimum Mean Square Error Estimator，MMSE）之一。它基于动态系统的线性模型，并假设系统噪声和观测噪声是高斯白噪声。 ### 离散时间线性随机动力系统 卡尔曼滤波的核心是建立一个离散时间的线性随机动力系统模型。这个模型由两个关键方程组成： 1. **状态转移方程** (1): 描述了系统状态从时间 \( k-1 \) 到时间 \( k \) 的演化，其中 \( x_k \) 是时间 \( k \) 的状态向量，\( F_k \) 是状态转移矩阵，\( G_k \) 是输入噪声矩阵，\( w_k \) 是过程噪声。 2. **观测方程** (2): 描述了如何通过传感器测量到的状态 \( z_k \)，其中 \( H_k \) 是观测矩阵，\( v_k \) 是观测噪声。 ### 卡尔曼滤波步骤 1. **初始化**: 设定初始状态估计 \( \hat{x}_{0|0} \) 和其协方差 \( P_{0|0} \)。 2. **预测**: 使用状态转移方程计算下一个时间步的先验状态估计 \( \hat{x}_{k|k-1} \) 和预测误差协方差矩阵 \( P_{k|k-1} \)。 3. **更新**: 接收观测值 \( z_k \)，通过观测方程计算测量值 \( \hat{x}_{k\text{mea}} \)，然后应用卡尔曼增益 \( K_k \) 更新后验状态估计 \( \hat{x}_{k|k} \) 和协方差矩阵 \( P_{k|k} \)。 4. **卡尔曼增益计算**: 卡尔曼增益 \( K_k \) 是通过最小化协方差矩阵 \( P_{k|k} \) 来确定的，它衡量了测量值对状态估计的贡献程度。 ### 具体公式推导 1. **预测值** \( \hat{x}_{k|k-1} \) 是前一时刻的后验状态估计 \( \hat{x}_{k-1|k-1} \) 通过状态转移矩阵 \( F_k \) 更新的结果。 2. **测量值** \( \hat{x}_{k\text{mea}} \) 是当前观测值 \( z_k \) 通过观测矩阵 \( H_k \) 转换而来的。 3. **后验值** \( \hat{x}_{k|k} \) 是通过卡尔曼增益 \( K_k \) 将预测值和测量值融合的结果，目的是减小真实值与估计值之间的误差。 4. **卡尔曼增益** \( K_k \) 的计算涉及到对协方差矩阵的最小化操作，通过求导找到 \( K_k \) 的最优值，使得误差协方差矩阵 \( P_{k|k} \) 最小。 ### 矩阵求导法则 在卡尔曼滤波的优化过程中，需要利用矩阵求导法则来找到卡尔曼增益 \( K_k \)。通常情况下，这涉及到计算矩阵的迹（tr()）以及矩阵乘积的导数。 ### 扩展滤波器 卡尔曼滤波是基础版本，还有其扩展形式如： - **扩展卡尔曼滤波** (EKF): 应用于非线性系统，通过线性化处理非线性函数。 - **无迹卡尔曼滤波** (UKF): 利用随机抽样（sigma points）来近似非线性函数，更适合处理非线性问题。 - **平方根卡尔曼滤波** (CKF): 通过对协方差矩阵进行平方根操作，避免了矩阵逆运算可能引入的数值不稳定问题。 理解并熟练掌握这些公式和推导对于实际应用卡尔曼滤波解决工程问题至关重要。