卡尔曼滤波
卡尔曼滤波（Kalman Filter）是一种用于估计系统状态的数学算法，不是类似于高通、低通滤波器那样的频域滤波。
卡尔曼滤波基于线性动态系统的假设，它将系统的状态表示为均值和协方差矩阵，通过递归地更新和预测这些值来实现对系统状态的估计。卡尔曼滤波有两个主要的步骤：预测和更新。
卡尔曼滤波具有一些优点，例如对噪声和不确定性的鲁棒性较强，能够提供较为精确的估计结果，并且计算效率较高。然而，卡尔曼滤波的应用前提是系统满足线性动态系统的假设，对于非线性系统，需要通过扩展卡尔曼滤波（Extended Kalman Filter）或无迹卡尔曼滤波（Unscented Kalman Filter）等变种算法来进行处理。
滤波结构
-
预测步骤中，卡尔曼滤波使用系统模型和上一时刻的状态估计来预测当前时刻的状态，并计算出预测状态的均值和协方差矩阵。
-
更新步骤中，卡尔曼滤波使用当前时刻的测量数据和预测状态的均值和协方差矩阵，通过最小二乘法得到对当前状态的估计，并重新计算估计的状态的均值和协方差矩阵。
重要公式
下面从几个重要公式的角度来分析卡尔曼滤波：
-
观测的一步转移，根据当前时刻的X(k-1)、状态转移矩阵 F F F、输入向量 u k − 1 u_{k-1} uk−1计算下一时刻的X_{k}
X ^ k − = F X k − 1 + G u k − 1 \hat X_{k}^- = FX_{k-1}+G u_{k-1} X^k−=FXk−1+Guk−1 -
协方差预测：
P k − = F P k − 1 F T + Q k P_{k}^{-}=FP_{k-1}F^{T}+Q_{k} Pk−=FPk−1FT+Qk -
计算增益：
K k = P k − H T ( H P k − H T + R ) − 1 K_k=P_k^-H^T(HP_k^-H^T+R)^{-1} Kk=Pk−HT(HPk−HT+R)−1 -
计算估计值：
X ^ t = X ^ t − + K t ( Z k − H X ^ k − ) \hat{X}_{t}=\hat{X}_{t}^{-}+K_{t}(Z_{k}-H\hat{X}_{k}^{-}) X^t=X^t−+Kt(Zk−HX^k−) -
更新协方差：
P k = ( I − K k H ) P k − P_k=(I-K_kH)P_k^- Pk=(I−KkH)Pk−
其他公式
表示观测预测的公式：
Z
^
k
=
H
X
k
\hat{Z}_{k} = HX_{k}
Z^k=HXk