（经典、扩展、开关）卡尔曼滤波

最新推荐文章于 2025-08-30 09:39:49 发布

原创最新推荐文章于 2025-08-30 09:39:49 发布 · 688 阅读

6 ·

CC 4.0 BY-SA版权

文章标签：

#算法 #机器学习

机器学习专栏收录该内容

23 篇文章

订阅专栏

本文详细介绍了卡尔曼滤波的三种变体：经典卡尔曼滤波（KF）、扩展卡尔曼滤波（EKF）和开关卡尔曼滤波（SKF）。KF适用于线性系统，EKF处理非线性动态模型，而SKF则在多模型切换场景下应用。文中给出了每种滤波器的状态预测和更新公式，以及在实际计算中的关键步骤，如雅各比矩阵的使用和贝叶斯概率的计算。

项目说明

本文将对三种卡尔曼滤波算法（经典卡尔曼滤波KF，扩展卡尔曼滤波EKF以及开关卡尔曼滤波SKF）的预测及更新过程进行汇总记录，不涉及具体公式推导过程。

经典卡尔曼滤波KF

经典卡尔曼滤波KF动态模型如下：
$Xt=AXt−1+Wt(1)X_t=AX_{t-1}+W_t \tag{1}$
$Yt=HXt+Vt(2)Y_t=HX_{t}+V_t \tag{2}$
其中

$X_t$ 是 $t$ 时刻的 $n×1n\times 1$ 系统状态矢量， $n$ 是状态变量个数；
$X_{t-1}$ 是 $t - 1$ 时刻系统状态矢量；
$A$ 是 $t - 1$ 时刻至 $t$ 时刻的 $n×nn\times n$ 维状态转移矩阵；
$W_t$ 是 $t$ 时刻的 $n×1n\times 1$ 维过程激励噪声；
$Y_t$ 是 $t$ 时刻的状态测量值；
$H$ 是 $1×n1\times n$ 维测量矩阵；
$V_t$ 是 $t$ 时刻的测量噪声。

预测过程

状态预测：
$Xt^=AXt−1(3)\hat{X_t}=AX_{t-1} \tag{3}$
协方差预测：
$Pt^=APt−1AT+Q(4)\hat{P_t}=AP_{t-1}A^T+Q \tag{4}$
式中， $Xt^\hat{X_t}$ 表示 $t$ 时刻先验状态估计值，这是算法根据前次迭代结果(即上一次循环的后验估计值)做出的不可靠估计； $Pt^\hat{P_t}$ 表示 $t$ 时刻的先验估计协方差，只要确定了初始的协方差 $P_0$ ，此协方差矩阵后续时刻的值都能递推出来，且只要初始协方差 $P0≠0P_0\neq0$ ，它的取值对滤波效果影响很小，都能很快收敛。

更新过程

卡尔曼增益：
$Kt=Pt^HT(HPt^HT+R)−1(5)K_t=\hat{P_t}H^T(H\hat{P_t}H^T+R)^{-1} \tag{5}$
状态更新：
$Xt=Xt^+Kt(Yt−HXt^)(6)X_t=\hat{X_t}+K_t(Y_t-H\hat{X_t}) \tag{6}$
协方差更新：
$Pt=(I−KtH)Pk^(7)P_t=(I-K_tH)\hat{P_k} \tag{7}$
$K_t$ 表示卡尔曼增益，由公式可知其也能在滤波前预先递推计算出来，对卡尔曼增益的确定是建立滤波模型的关键步骤之一，它能显著影响滤波的结果； $X_t$ ， $X_{t-1}$ 表示 $t$ 时刻、 $t - 1$ 时刻的后验状态估计值，也就是要输出的该时刻最优估计值，这个值是卡尔曼滤波的结果； $P_t$ ， $P_{t-1}$ 表示 $t$ 时刻、 $t - 1$ 时刻的后验估计协方差。

扩展卡尔曼滤波EKF

扩展卡尔曼滤波EKF动态模型如下：
$Xt=f(Xt−1)+Wt(8)X_t=f(X_{t-1})+W_t \tag{8}$
$Yt=h(Xt)+Vt(9)Y_t=h(X_{t})+V_t \tag{9}$

预测过程

状态预测：
$Xt^=f(Xt−1)(10)\hat{X_t}=f(X_{t-1}) \tag{10}$
协方差预测：
$Pt^=Ft−1Pt−1Ft−1T+Q(11)\hat{P_t}=F_{t-1}P_{t-1}F_{t-1}^T+Q \tag{11}$

更新过程

卡尔曼增益：
$Kt=Pt^HtT(HtPt^HtT+R)−1(12)K_t=\hat{P_t}H_t^T(H_t\hat{P_t}H_t^T+R)^{-1} \tag{12}$
状态更新：
$Xt=Xt^+Kt(Yt−h(Xt^))(13)X_t=\hat{X_t}+K_t(Y_t-h(\hat{X_t})) \tag{13}$
协方差更新：
$Pt=(I−KtHt)Pt^(14)P_t=(I-K_tH_t)\hat{P_t} \tag{14}$
这里需要引入雅各比矩阵，雅各比矩阵就是方程矩阵对每一个变量的偏导数，举例来说，雅各比矩阵是该函数的所有分量（ $m$ 个）对向量 $x$ 的所有分量（ $n$ 个）的一阶偏导数组成的矩阵。
$J=∂f∂x=[∂f∂x1⋯∂f∂xn]=[∂f1∂x1⋯∂f1∂xn⋮⋱⋮∂fm∂x1⋯∂fm∂xn](15)J=\frac{\partial{\bf{f}}}{\partial{\bf{x}}}=[\frac{\partial{\bf{f}}}{\partial{x_1}} \cdots \frac{\partial{\bf{f}}}{\partial{x_n}}]=\begin{bmatrix} \frac{\partial{f_1}}{\partial{x_1}} & \cdots & \frac{\partial{f_1}}{\partial{x_n}} \\ \vdots & \ddots & \vdots \\ \frac{\partial{f_m}}{\partial{x_1}} & \cdots & \frac{\partial{f_m}}{\partial{x_n}}\end{bmatrix} \tag{15}$
公式（11）中， $F_{t-1}$ 表示 $∂f∂X∣Xt−1\frac{\partial{f}}{\partial{X}}|_{X_{t-1}}$ ， $H_t$ 表示 $∂h∂X∣Xt\frac{\partial{h}}{\partial{X}}|_{X_{t}}$ 。

开关卡尔曼滤波SKF

应用扩展卡尔曼滤波SKF时，首先分析系统各种可能存在的状态，并针对每种状态建立相应的卡尔曼滤波器。随后通过计算每时刻各状态的概率，进而分析出该时刻系统最可能所处的状态。开关卡尔曼滤波的计算过程如下：

依据贝叶斯估计理论，对于一个由 $l$ 个卡尔曼滤波器描述的动态系统，模型变换概率
$Sti∣j=ZijSt−1i∑i=1lZijSt−1i(16)S_t^{i|j}=\frac{Z_{ij}S^i_{t-1}}{\sum^l_{i=1}Z_{ij}S^i_{t-1}} \tag{16}$
式中， $S_t^{i|j}$ 表示模型从 $t - 1$ 时刻的模型 $i$ （状态 $i$ ）变换到 $t$ 时刻的模型 $j$ （状态 $j$ ）的概率； $S_{t-1}^i$ 表示 $t - 1$ 时刻系统是模型 $i$ （状态 $i$ ）的概率； $Z_{ij}$ 表示模型转移矩阵。

加权状态估计：
$X~t−1j=∑i=1lSti∣jXt−1i(17)\tilde{X}_{t-1}^j=\sum^l_{i=1}S_t^{i|j}X_{t-1}^i \tag{17}$
加权协方差估计：
$P~t−1j=∑i=1lSti∣j{Pt−1i+[Xt−1i−Xt−1j][Xt−1i−Xt−1j]T}(18)\tilde{P}_{t-1}^j=\sum^l_{i=1}S_t^{i|j}\{P_{t-1}^i+[X_{t-1}^i-X_{t-1}^j][X_{t-1}^i-X_{t-1}^j]^T\} \tag{18}$
式中， $X_{t-1}^i$ 表示 $t - 1$ 时刻模型 $i$ 的后验状态估计值； $P_{t-1}^i$ 表示 $t - 1$ 时刻模型 $i$ 的后验估计协方差。将式(17)、(18)计算的加权状态和协方差代入到经典卡尔曼滤波算法中的式(3)-(7)中，可得每个模型对应的最优状态估计 $X^t−1j\hat{X}_{t-1}^j$ 和协方差估计 $P^t−1j\hat{P}_{t-1}^j$ 。

每个滤波器模型的似然估计计算如下：
$Lki=N(Vti;0,Cti)(19)L_k^i=N(V_t^i; 0, C_t^i) \tag{19}$
其中测量残差：
$Vt=Yt−HX^t(20)V_t=Y_t-H\hat{X}_t \tag{20}$
残差协方差：
$Ct=HP^tHT+R(21)C_t=H\hat{P}_tH^T+R \tag{21}$
则 $t$ 时刻每个模型的概率：
$Sti=Lti∑i=1lZijSt−1i∑i=1l(Lki∑i=1lZijSt−1i)(22)S_t^i=\frac{L_t^i\sum_{i=1}^lZ_{ij}S_{t-1}^i}{\sum_{i=1}^l(L_k^i\sum_{i=1}^lZ_{ij}S_{t-1}^i)} \tag{22}$
更新的加权状态和协方差计算如下：
$Xt=∑i=1lStiXti(23)X_t=\sum_{i=1}^lS_t^iX_t^i \tag{23}$
$Pt=∑i=1lSti{Pti[Xti−Xt][Xti−Xt]T}(24)P_t=\sum_{i=1}^lS_t^i\{P_t^i[X_t^i-X_t][X_t^i-X_t]^T\} \tag{24}$