KalmanFiltering
Kalman Filtering(卡尔曼滤波)
- 卡尔曼滤波是最常用最重要的状态估计算法之一。卡尔曼滤波能从不确定且非精确的测量中估计隐藏状态，同时还可以根据历史估计值对未来系统状态进行预测。滤波算法以 Rudolf E.Kalman的名字命名。在1960年，卡尔曼发表了著名的论文，描述了一个离散数据的线性滤波问题的递归算法。如今它被广泛应用于目标追踪、定位和导航系统、控制系统等领域。
五个公式：
- 时间更新(预测方程)： \[先验估计， \hat{x}_{k}^{-}=A\hat{x}_{k-1}+Bu_{k-1} + w_{k-1}\]
\[先验估计协方差， P_{k}^{-}=AP_{k-1}A^T+Q\]
- 测量更新(校正方程)： \[计算卡尔曼增益：K_{k}=P_{k}^{-}H^{T}(HP_{k}^{-}H^{T}+R)^{-1}\] \[ 由观测变z_k更新估计： \hat{x}_{k}=\hat{x}_{k}^{-}+K_{k}(z_{k}-H\hat{x}_{k}^{-})，这里的\hat{x_k} 就是最优估计\] \[ 更新误差协方差： P_{k}=(I-K_{k}H)P_{k}^{-}\]
参数解释：状态变量 \(x_k\), 观测变量\(z_k = Hx_k + v_k\)。 A为增益矩阵，B表示控制输入的增益。H表示状态变量 \(x_k\)对测量变量 \(z_k\)的增益。实际中H可能随时间变化，这里假设为常数。 随机信号 \(w_k\) 和 \(v_k\)分别表示过程激励噪声和观测噪声，假设它们为相互独立、正态分布的白色噪声： \[p(w)\sim N(0,Q),\\p(v)\sim N(0,R).\]
实际系统中，过程激励噪声协方差矩阵Q和观测噪声协方差矩阵R可能会随每次迭代计算而变化。\(u_{k-1}\)为控制函数。\(\hat{x}_{k}^{-}\) 为在已知第k步以前状态情况下第k步的先验状态估计。\(\hat{x}_{k}\) 为已知测量变量 \(z_k\)时第k步的后验状态估计。由此定义先验估计误差和后验估计误差：
\[ e_{k}^{-} \equiv x_{k}-\hat{x}_{k}^{-},\\ e_{k} \equiv x_{k}-\hat{x}_{k} \]
扩展卡尔曼滤波器(Extended Kalman Filter, EKF)：卡尔曼滤波器估计一个用线性随机差分方程描述的离散时间过程的状态变量x。但如果被估计的过程和观测变量与过程的关系是非线性的，那应该怎么办？将期望和方差线性化的卡尔曼滤波器称作扩展卡尔曼滤波器。简称(EKF)。同泰勒级数类似，面对非线性关系时，可以通过求过程和量测方程的偏导来线性化并计算当前估计。假设过程仍具有状态向量x。但其状态方程已变为非线性随机差分方程的形式。 \[ x_{k}=f ( x_{k-1}, u_{k-1}, w_{k-1} ), \] 观测变量z为： \[ z_{k}=h ( x_{k}, v_{k} ), \] 随机变量 \(w_k\)和\(v_k\)分别表示过程激励噪声和观测噪声，上面的公式中的非线性函数 \(f\)将上一时刻 \(k-1\)的状态映射到当前时刻 \(k\)的状态。\(u_k\) 和 \(w_k\) 是它的参数。非线性函数h反映了状态变量 \(x_k\) 和观测变量 \(z_k\)的关系。假设每一时刻的噪声 \(w_k\) 和 \(v_k\) 为0，从而估计状态向量和观测向量为： \[ \tilde{x}_{k}=f ( \hat{x}_{k-1}, u_{k-1}, 0 ) \\ \tilde{z}_{k}=h ( \hat{x}, 0 ) \]
\(\hat{x}_{k}\) 是过程相对前一时刻k的后验估计。 扩展卡尔曼滤波的一个基本缺陷：离散随机变量的分布(或连续随机变量的密度)在经过非线性系统转化后不再是正态的了。扩展卡尔曼滤波器其实就是一个通过线性化而达到渐进最优贝叶斯决策的特殊状态估计器。
为了估计一个具有非线性差分和量测关系的过程，给出线性化的表示： \[ x_{k} \approx\tilde{x}_{k}+A ( x_{k-1}-\hat{x}_{k-1} )+W w_{k-1}, \] \[ z_{k} \approx\tilde{z}_{k}+H ( x_{k}-\tilde{x}_{k} ) V v_{k}. \]
其中， \(x_k\)和 \(z_k\)是状态变量和观测向量的真值，\(\hat{x}_{k}，\hat{z}_{k}\) 是状态向量和观测向量的观测值。\(\hat{x}_{k}\) 是 k 时刻状态向量的后验估计。随机变量 \(w_k，v_k\) 表示过程激励噪声和观测噪声。A 是\(f\)对 \(x\)的偏导的雅可比矩阵： \[ A_{[ i, j ]}=\frac{\partial f_{[ i ]}} {\partial x_{[ j ]}} ( \hat{x}_{k-1}, u_{k-1}, 0 ), \]
H 是 \(h\)对 \(x\)的偏导的雅可比矩阵： \[ H_{[ i, j ]}=\frac{\partial h_{[ i ]}} {\partial x_{[ j ]}} ( \hat{x}_{k}, 0 ). \]
W是h对x的偏导的雅可比矩阵： \[ W_{[ i, j ]}=\frac{\partial f_{[ i ]}} {\partial w_{[ j ]}} ( \hat{x}_{k-1}, u_{k-1}, 0 ), \]
V是h对v的偏导的雅可比矩阵：
\[ V_{[ i, j ]}=\frac{\partial h_{[ i ]}} {\partial v_{[ j ]}} ( \tilde{x}_{k}, 0 ). \] 定义一个新的预测误差的表达式：
\[ \tilde{e}_{x_{k}} \equiv x_{k}-\tilde{x}_{k}, \] 观测变量的残余： \[ \tilde{e}_{z_{k}} \equiv z_{k}-\tilde{z}_{k}, \]
由以上两个式子，写出误差过程的表达式： \[ \tilde{e}_{x_{k}} \approx A ( x_{k-1}-\hat{x}_{k-1} )+\epsilon_{k}, \] \[ \tilde{e}_{z_{k}} \approx H \tilde{e}_{x_{k}}+\eta_{k}, \]
\(\epsilon_{k}, \eta{k}\) 代表具有零均值和协方差矩阵 \(WQW^T\)，\(VRV^T\) 的独立随机变量。
时间更新方程：
\[ \hat{x}_{k}^{-}=f ( \hat{x}_{k-1}, u_{k-1}, 0 ) \] \[ P_{k}^{-}=A_{k} P_{k-1} A_{k}^{T}+W_{k} Q_{k-1} W_{k}^{T} \] 状态更新方程：
\[K_{k}=P_{k}^{-}H_{k}^{T}(H_{k}P_{k}^{-}H_{k}^{T}+V_{k}R_{k}V_{k}^{T})^{-1}\\\hat{x}_{k}=\hat{x}_{k}^{-}+K_{k}(z_{k}-h(\hat{x}_{k}^{-},0))\\P_{k}=(I-K_{k}H_{k})P_{k}^{-}\]
扩展卡尔曼滤波器的一个重要特性是卡尔曼增益 \(K_k\)的表达式中的雅可比矩阵 \(H_k\)能够正确地传递或加权观测信息中的有用部分。例如，如果通过h观测变量 \(z_k\)和状态变量没有一一对应的关系，雅可比矩阵 \(H_k\)便通过改变卡尔曼增益从而使得残余 \(z_k - h(\hat{x}_{k}^{-},0)\)中真正作用域状态变量的部分被加权。当然，如果整个观测中观测变量 \(z_k\) 和状态变量通过h都没有一个一一对应的关系，那么滤波器很快就发散，这种情况下过程是不可观测的。
参考链接:
https://www.kalmanfilter.net/CN/default_cn.aspxhttps://www.cs.unc.edu/~welch/kalman/media/pdf/kalman_intro_chinese.pdfhttps://www.kalmanfilter.net/CN/multiSummary_cn.html