Kalman Filter、Extended Kalman Filter以及Unscented Kalman Filter介绍

模型定义

如上图所示，卡尔曼滤波(Kalman Filter)的基本模型和隐马尔可夫模型类似，不同的是隐马尔科夫模型考虑离散的状态空间，而卡尔曼滤波的状态空间以及观测空间都是连续的，并且都属于高斯分布，因此卡尔曼滤波又称为linear Gaussian Markov model，它的数学定义如下：$$underbrace{s_{t}=C s_{t-1}+G h_{t}+gamma_{t}}_{ ext { latent process }}, quad underbrace{x_{t}=D s_{t}+varepsilon_{t}}_{ ext { observed process }}$$其中$h_t$表示控制向量(control vector)，是已知量；$gamma_{t} sim N(0, Q)$表示状态误差，它包含了状态转换公式$C s_{t-1}+G h_{t}$中未考虑到的其它因素，是状态转换公式准确性的度量；$varepsilon_{t} sim N(0, V)$表示观测误差，是观测精度的度量。下面举一个简单的例子：

• 假设有一个二维空间上的物体位置的观测序列($x_{t} in mathbb{R}^{2}$)，观测有一定误差；该物体的状态$s_t=[p_{t1},v_{t1},p_{t2},v_{t2}]^T$，其中$p_t$和$v_t$表示物体位置和速度，下标1和2表示方向；控制向量为$h_t=[a_{t1},a_{t2}]^T$，$a_t$表示加速度。由基本的物理公式可知$$s_{t}=underbrace{left[egin{array}{cccc}{1} & {Delta t} & {0} & {0} \ {0} & {1} & {0} & {0} \ {0} & {0} & {1} & {Delta t} \ {0} & {0} & {0} & {1}end{array} ight]}_{C}s_{t-1}+underbrace{left[egin{array}{cc} frac{1}{2}(Delta t)^{2} & {0} \ {Delta t} & {0} \ {0} & frac{1}{2}(Delta t)^{2} \ {0} & {Delta t}end{array} ight]}_{G}h_t+gamma_t ext{ 以及 }x_{t}=underbrace{left[egin{array}{cccc}{1} & {0} & {0} & {0} \ {0} & {0} & {1} & {0}end{array} ight]}_{D}s_{t}+varepsilon_{t}$$

卡尔曼滤波的目标是已知观测序列$x_1,x_2,cdots,x_t$，计算当前隐藏状态的分布函数，即$$s_{t}left|s_{t-1} sim Nleft(C s_{t-1}+G h_{t}, Q ight), quad x_{t} ight| s_{t} sim Nleft(D s_{t}, V ight)quad
Rightarrowquad pleft(s_{t} | x_{1}, dots, x_{t} ight);quad 1leq t leq T$$注意除观测序列以外，矩阵$C,G,Q,D,V$以及控制向量$h_t$也是给定的。 

模型求解

• 定义$S_t=(s_{t} | x_{1}, ldots, x_{t})$，容易看出$S_t$满足高斯分布$N(mu_{t}, Sigma_{t})$，$mu_t$以及$Sigma_t$即为需要求解的量
  • 为方便之后的计算，令$S_{t-1}=(s_{t-1} | x_{1}, ldots, x_{t-1})=underbrace{mu_{t-1}}_ ext{mean}+Delta S_{t-1},quad Delta S_{t-1} sim N(0,Sigma_{t-1})$
• 定义$P_t= (s_{t} | x_{1}, ldots, x_{t-1})$，有$P_t=CS_{t-1}+G h_{t}+gamma_{t}$
  • 为方便之后的计算，令$P_t=underbrace{Cmu_{t-1}+Gh_t}_{mu_{P_t}}+underbrace{CDelta S_{t-1}+gamma_t}_{Delta P_t}$
• 定义$O_t= (x_{t} | x_{1}, ldots, x_{t-1})$，有$O_t=DP_{t}+varepsilon_t$
  • 为方便之后的计算，令$O_t=underbrace{D(Cmu_{t-1}+Gh_t)}_{mu_{O_t}}+underbrace{DCDelta S_{t-1}+Dgamma_t+varepsilon_t}_{Delta O_t}$

由上述定义可知 $$left[egin{array}{c} P_t \ O_t end{array} ight] sim Nleft(left[egin{array}{c} mu_{P_t} \ mu_{O_t} end{array} ight], left[egin{array}{cc} Sigma_{PP} & Sigma_{PO}\ Sigma_{PO}^T & Sigma_{OO} end{array} ight] ight)$$接下来计算协方差矩阵的这些项：

• $Sigma_{PP}=mathbb{E}[{Delta P_t (Delta P_t)^T}]=Cmathbb{E}[Delta S_{t-1} (Delta S_{t-1})^T]C^T+mathbb{E}[gamma_tgamma_t^T]=CSigma_{t-1}C^T+Q$
• $Sigma_{PO}=mathbb{E}[{Delta P_t (Delta O_t)^T}]=Cmathbb{E}[Delta S_{t-1} (Delta S_{t-1})^T]C^TD^T+mathbb{E}[gamma_tgamma_t^T]D^T=CSigma_{t-1}C^TD^T+QD^T$
• $Sigma_{OO}=mathbb{E}[{Delta O_t (Delta O_t)^T}]=DCmathbb{E}[Delta S_{t-1} (Delta S_{t-1})^T]C^TD^T+Dmathbb{E}[gamma_tgamma_t^T]D^T+mathbb{E}[varepsilon_tvarepsilon_t^T]=DCSigma_{t-1}C^TD^T+DQD^T+V$

容易看出$S_t=(P_t | O_t)$，此外定义$$hat{mu}_t=mu_{P_t}=C mu_{t-1}+Gh_t， ext{  }hat{Sigma}_t=Sigma_{PP}=C Sigma_{t-1} C^{T}+Q ext{以及卡尔曼增益矩阵}K_t=hat{Sigma}_{t}D^T[Dhat{Sigma}_{t}D^T+V]^{-1}$$由高斯分布的性质可知

• $Sigma_t=Sigma_{PP}-Sigma_{PO}Sigma_{OO}^{-1}Sigma_{PO}^T=(I-K_tD)hat{Sigma}_t$
• $mu_t=mu_{P_t}+Sigma_{PO}Sigma_{OO}^{-1}(O_t-mu_{O_t})=hat{mu}_t+K_t(x_t-Dhat{mu}_t)$

上述求解过程可归纳为：

1. 初始化$mu_0$以及$Sigma_0$
2. 预测：$hat{mu}_t=C mu_{t-1}+Gh_t$以及$hat{Sigma}_t=C Sigma_{t-1} C^{T}+Q$
3. 计算卡尔曼增益矩阵$K_t=hat{Sigma}_{t}D^T[Dhat{Sigma}_{t}D^T+V]^{-1}$
4. 更新：$mu_t=hat{mu}_t+K_t(x_t-Dhat{mu}_t)$以及$Sigma_t=(I-K_tD)hat{Sigma}_t$

Extended Kalman Filter

在Extended Kalman Filter中，状态之间的转化以及状态向观测的转化是非线性的，即$$s_t=g(s_{t-1},h_t)+gamma_t, ext{  }x_{t}=f(s_{t})+varepsilon_{t}; ext{  其中}g,f ext{代表非线性函数}$$此时考虑使用泰勒公式将非线性函数近似为线性函数，延续上一部分的定义，有

• $P_t=g(S_{t-1},h_t)+gamma_{t}=g(mu_{t-1}+delta S_{t-1},h_t)+gamma_{t}=underbrace{g(mu_{t-1},h_t)}_{mu_{P_t}( ext{i.e., }hat{mu}_t)}+underbrace{J_gDelta S_{t-1}+gamma_{t}}_{Delta P_t}$
• $O_t=h(P_t)+varepsilon_t=f(mu_{P_t}+Delta P_t)+varepsilon_t=f(mu_{P_t})+J_fDelta P_t+varepsilon_t=underbrace{f(hat{mu}_{t})}_{mu_{O_t}}+underbrace{J_fJ_gDelta S_{t-1}+J_fgamma_t+varepsilon_t}_{Delta O_t}$

其中$J_g$和$J_f$为Jacobian矩阵，假设状态为$m$维向量，观测为$n$维向量，并且$g(s,h)=[g_1(s,h),cdots,g_m(s,h)]^T$以及$f(s)=[f_1(s),cdots,f_n(s)]^T$，则有$$J_g=left[egin{array}{cccc}frac{partial g_1}{partial mu_{t-1,1}} & frac{partial mu_1}{partial s_{t-1,2}} & cdots & frac{partial g_1}{partial mu_{t-1,m}} \ vdots & vdots & vdots & vdots \ frac{partial g_m}{partial mu_{t-1,1}} & frac{partial g_m}{partial mu_{t-1,2}} & cdots & frac{partial g_m}{partial mu_{t-1,m}}end{array} ight], ext{   }J_f=left[egin{array}{cccc}frac{partial f_1}{partial hat{mu}_{t,1}} & frac{partial f_1}{partial hat{mu}_{t,2}} & cdots & frac{partial f_1}{partial hat{mu}_{t,m}} \ vdots & vdots & vdots & vdots \ frac{partial f_n}{partial hat{mu}_{t,1}} & frac{partial f_n}{partial hat{mu}_{t,2}} & cdots & frac{partial f_n}{partial hat{mu}_{t,m}}end{array} ight]$$容易看出Extended Kalman Filter的求解过程可归纳为：

1. 初始化$mu_0$以及$Sigma_0$
2. 预测：$hat{mu}_t=g(mu_{t-1},h_t)$以及$hat{Sigma}_t=J_g Sigma_{t-1} J_g^{T}+Q$
3. 计算卡尔曼增益矩阵$K_t=hat{Sigma}_{t}J_f^T[J_fhat{Sigma}_{t}J_f^T+V]^{-1}$
4. 更新：$mu_t=hat{mu}_t+K_t[x_t-f(hat{mu}_t)]$以及$Sigma_t=(I-K_tJ_f)hat{Sigma}_t$

Unscented Kalman Filter

Unscented Kalman Filter和Extended Kalman Filter的模型定义一样，只是具体求解方法不同。相对于Extended Kalman Filter使用泰勒公式近似非线性函数，Unscented Kalman Filter通过选取多个样本点(the sigma points)直接估计均值和方差。仍然延续之前的定义，假设状态为$m$维向量，从随机变量$S_{t-1}$中选取$2m+1$个样本点，记为矩阵$mathcal{X}_{t-1}$($m$行$2m+1$列)，选取方式为$$mathcal{X}_{t-1}=left[egin{array}{ccc}mu_{t-1} & mu_{t-1}+sqrt{(m+lambda )Sigma_{t-1}} & mu_{t-1}-sqrt{(m+lambda )Sigma_{t-1}} end{array} ight]$$若将$Sigma_{t-1}$进行Cholesky分解得到$LL^T$，则$sqrt{Sigma_{t-1}}=L$；或者对$Sigma_{t-1}$进行特征值分解得到$ULambda U^T$(其中$Lambda$为对角阵)，则$sqrt{Sigma_{t-1}}=ULambda^{1/2}$。接下来对每个采样点分配权重：

• $vec{w}_a=left[egin{array}{ccccc}frac{lambda}{m+lambda} & frac{1}{2(m+lambda)} & frac{1}{2(m+lambda)} & cdots & frac{1}{2(m+lambda)}end{array} ight]$
• $vec{w}_c=left[egin{array}{ccccc}frac{lambda}{m+lambda}+(1-alpha^2+eta) & frac{1}{2(m+lambda)} & frac{1}{2(m+lambda)} & cdots & frac{1}{2(m+lambda)}end{array} ight]$

其中$vec{w}_a$为求均值时的权重，$vec{w}_c$为求协方差矩阵时的权重。针对一些参数的取值有以下建议：$$alpha in (0,1], ext{  }eta=2, ext{  }lambda=alpha^2(m+kappa)-m, ext{  }kappageq 0$$将$mathcal{X}_{t-1}$代入函数$g$可以得到$$hat{mathcal{X}}_{t}=left[egin{array}{cccc}g(mathcal{X}_{t-1}^{[1]},h_t) & g(mathcal{X}_{t-1}^{[2]},h_t)  & cdots & g(mathcal{X}_{t-1}^{[2m+1]},h_t)end{array} ight]$$其中上标表示矩阵的列数，由$hat{mathcal{X}}_{t}$可以估计出$hat{mu}_t$以及$hat{Sigma}_t$，接下来可以通过两种方式得到观测的采样点$mathcal{Z}_t$：

1. 直接通过$hat{mathcal{X}}_{t}$进行计算，即$$mathcal{Z}_{t}=left[egin{array}{cccc}h(hat{mathcal{X}}_{t}^{[1]}) & h(hat{mathcal{X}}_{t}^{[2]})  & cdots & h(hat{mathcal{X}}_{t}^{[2m+1]}) end{array} ight]$$
2. 通过得到的$hat{mu}_t$以及$hat{Sigma}_t$重新采样，有公式$$hat{mathcal{X}}_{t}^*=left[egin{array}{ccc}hat{mu}_{t} & hat{mu}_{t}+sqrt{(m+lambda )hat{Sigma}_{t}} & hat{mu}_{t}-sqrt{(m+lambda )hat{Sigma}_{t}} end{array} ight]$$然后计算过程同第一种方式，即$$mathcal{Z}_{t}=left[egin{array}{cccc}h(hat{mathcal{X}}_{t}^{*[1]}) & h(hat{mathcal{X}}_{t}^{*[2]})  & cdots & h(hat{mathcal{X}}_{t}^{*[2m+1]}) end{array} ight]$$

最后估计观测的均值和协方差矩阵，进而得到最终的结果，Unscented Kalman Filter的求解过程可归纳为：

1. 初始化$mu_0$以及$Sigma_0$
2. 预测：$hat{mu}_t=sum_{j=1}^{2m+1}w_{aj}hat{mathcal{X}}_{t}^{[j]}$以及$hat{Sigma}_t=sum_{j=1}^{2m+1}w_{cj}(hat{mathcal{X}}_{t}^{[j]}-hat{mu}_t)(hat{mathcal{X}}_{t}^{[j]}-hat{mu}_t)^T+Q$
3. 计算$mathcal{Z}_{t}$（从上述两种方式中选择一种），得到$mu_{O_t}=sum_{j=1}^{2m+1}w_{aj}mathcal{Z}_{t}^{[j]}$以及$Sigma_{OO}=sum_{j=1}^{2m+1}w_{cj}({mathcal{Z}}_{t}^{[j]}-mu_{O_t})({mathcal{Z}}_{t}^{[j]}-mu_{O_t})^T+V$
4. 计算$Sigma_{PO}=sum_{j=1}^{2m+1}w_{cj}(hat{mathcal{X}}_{t}^{[j]}-hat{mu}_{t})({mathcal{Z}}_{t}^{[j]}-mu_{O_t})^T$，注意若使用第二种方式计算$mathcal{Z}_t$，需将公式中的$hat{mathcal{X}}_{t}$替换为$hat{mathcal{X}}_{t}^*$
5. 计算卡尔曼增益矩阵$K_t=Sigma_{PO}Sigma_{OO}^{-1}$
6. 更新：$mu_t=hat{mu}_t+K_t[x_t-mu_{O_t}]$以及$Sigma_t=hat{Sigma}_t-Sigma_{PO}Sigma_{OO}^{-1}Sigma_{PO}^T=hat{Sigma}_t-K_tSigma_{OO}K_t^T$