卡尔曼滤波三

扩展卡尔曼滤波(EKF)

我们之前的系统考虑的都线性的系统，在线性系统中，我们证明了卡尔曼滤波是最优的！
如果是非线性呢？我们又该如何处理的呢？
如果我们直接就是将非线性的系统利用泰勒展开将其线性化，这就引出了另一类卡尔曼滤波器了，扩展卡尔曼滤波器(EKF)
下面，我们进行详细的说明：
首先，我们简单的回顾一下我们泰勒展开的基本概念：

• 对于一维的泰勒展开：
  $$f(x)\approx f(x_0)+k(x-x_0)$$
  其中，$k$为函数$f(x)$在操作点$x_0$处的斜率，即：$k=f^{\prime }(x_0)$

• 而对于高维的泰勒展开：
  $$f\text{f}f(x\text{x}x,y\text{y}y)\approx f\text{f}f({x\text{x}x}_0,{y\text{y}y}_0)+A\text{A}A(x\text{x}x-{x\text{x}x}_0)+B\text{B}B(y\text{y}y-{y\text{y}y}_0)$$
  其中，$A\text{A}A$和$B\text{B}B$为对应的雅可比矩阵：
  $$\begin{array}{rl}A\text{A}A& =\left[\begin{array}{c}\frac{\partial f_1}{\partial x\text{x}x}\\ \frac{\partial f_2}{\partial x\text{x}x}\\ ⋮\\ \frac{\partial f_n}{\partial x\text{x}x}\end{array}\right]\\ B\text{B}B& =\left[\begin{array}{c}\frac{\partial f_1}{\partial y\text{y}y}\\ \frac{\partial f_2}{\partial y\text{y}y}\\ ⋮\\ \frac{\partial f_n}{\partial y\text{y}y}\end{array}\right]\end{array}$$

对于我们的非线性系统来说有，有状态方程：
$$\begin{array}{rl}{x\text{x}x}_k& =f\text{f}f({x\text{x}x}_{x-1},{u\text{u}u}_{k-1},{w\text{w}w}_{k-1})\\ {z\text{z}z}_k& =h\text{h}h({x\text{x}x}_k,{\nu \text{ν}\nu }_k)\end{array}$$
当然，我们这里的噪声仍然假设服从正态分布的，而且相互独立的。即：
$$\begin{gathered} P({w\text{w}w}_{k-1})\sim N(0,Q\text{Q}Q) \\ P({\nu \text{ν}\nu }_k)\sim N(0,R\text{R}R) \end{gathered}$$
然而我们在展开上述非线性的方程的时候，将会发现我们的操作点是不能准确的得到的(最主要的原因就是我们的误差是不确定的)，因此我们将在一个次一点的操作点(即上一次的估计值中取得)$({\widehat{x\text{x}x}}_{k-1},{u\text{u}u}_{k-1},0)$处展开(由于噪声的不确定性，为了简化问题，我们假设此时的噪声为0)，因此得到：
$$\begin{array}{rl}{x\text{x}x}_k& =f\text{f}f({x\text{x}x}_{k-1},{u\text{u}u}_{k-1},{w\text{w}w}_{k-1})\\ & \approx f\text{f}f({\widehat{x\text{x}x}}_{k-1},{u\text{u}u}_{k-1},0)+A\text{A}A({x\text{x}x}_{k-1}-{\widehat{x\text{x}x}}_{k-1})+B\text{B}B({u\text{u}u}_{k-1}-{u\text{u}u}_{k-1})+W\text{W}W({w\text{w}w}_{k-1}-0)\\ & =f\text{f}f({\widehat{x\text{x}x}}_{k-1},{u\text{u}u}_{k-1},0)+A\text{A}A({x\text{x}x}_{k-1}-{\widehat{x\text{x}x}}_{k-1})+W\text{W}W{w\text{w}w}_{k-1}\end{array}$$
其中，$A\text{A}A,B\text{B}B,W\text{W}W$分别为对应的雅可比矩阵，即：
$$\begin{array}{rl}A\text{A}A& =\frac{\partial f\text{f}f}{\partial x\text{x}x}{|}_{({\widehat{x\text{x}x}}_{k-1},{u\text{u}u}_{k-1},0)}\\ B\text{B}B& =\frac{\partial f\text{f}f}{\partial u\text{u}u}{|}_{({\widehat{x\text{x}x}}_{k-1},{u\text{u}u}_{k-1},0)}\\ W\text{W}W& =\frac{\partial f\text{f}f}{\partial w\text{w}w}{|}_{({\widehat{x\text{x}x}}_{k-1},{u\text{u}u}_{k-1},0)}\end{array}$$
在这里，我们记${\widetilde{x\text{x}x}}_k=f\text{f}f({\widehat{x\text{x}x}}_{k-1},{u\text{u}u}_{k-1},0)$，也就是我们简化问题，将其作为当前的估计值，此时我们对${z\text{z}z}_k$进行线性化，在操作点$({\widetilde{x\text{x}x}}_k,0)$处进行泰勒展开(同样我们将误差取为0，因为我们不知道它到底是多少！！)：
$$\begin{array}{rl}{z\text{z}z}_k& =h\text{h}h({x\text{x}x}_k,{\nu \text{ν}\nu }_k)\\ & \approx h\text{h}h({\widetilde{x\text{x}x}}_k,0)+H\text{H}H({x\text{x}x}_k-{\widetilde{x\text{x}x}}_k)+V\text{V}V{\nu \text{ν}\nu }_k\\ & ={\widetilde{z\text{z}z}}_k+H\text{H}H({x\text{x}x}_k-{\widetilde{x\text{x}x}}_k)+V\text{V}V{\nu \text{ν}\nu }_k\end{array}$$
其中，$h\text{h}h({\widetilde{x\text{x}x}}_k,0)$为简化后的估计值，记为：${\widetilde{z\text{z}z}}_k$
$${\widetilde{z\text{z}z}}_k=h\text{h}h({\widetilde{x\text{x}x}}_k,0)$$
雅可比矩阵$H\text{H}H,V\text{V}V$分别为：
$$\begin{array}{rl}H\text{H}H& =\frac{\partial h\text{h}h}{\partial x\text{x}x}{|}_{({\widetilde{x\text{x}x}}_k,0)}\\ V\text{V}V& =\frac{\partial h\text{h}h}{\partial \nu \text{ν}\nu }{|}_{({\widetilde{x\text{x}x}}_k,0)}\end{array}$$
至此，我们完成了我们的线性化的工作：
$$\begin{array}{rl}{x\text{x}x}_k& ={\widetilde{x\text{x}x}}_k+A\text{A}A({x\text{x}x}_{k-1}-{\widehat{x\text{x}x}}_{k-1})+W\text{W}W{w\text{w}w}_{k-1}\\ {z\text{z}z}_k& ={\widetilde{z\text{z}z}}_k+H\text{H}H({x\text{x}x}_k-{\widetilde{x\text{x}x}}_k)+V\text{V}V{\nu \text{ν}\nu }_k\end{array}$$
最后还需要说明的是，$W\text{W}W{w\text{w}w}_{k-1}$和$V\text{V}V{\nu \text{ν}\nu }_k$仍然服从正态分布(毕竟这是一个线性变换啊！！)，即有：
$$\begin{array}{rl}P(W\text{W}W{w\text{w}w}_k)& \sim N(0,W\text{W}WQ\text{Q}Q{W\text{W}W}^T)\\ P(V\text{V}V{\nu \text{ν}\nu }_k)& \sim N(0,V\text{V}VR\text{R}R{V\text{V}V}^T)\end{array}$$
这里我们再次强调一下，为什么我们要费这么大的劲把这个系统进行线性化的处理呢？如果我们把线性化又会怎样呢？

• 我们答案现在是非常的明显了，因为我们不进行线性化我们的噪声将不在服从正态分布了的，之前我们的卡尔曼滤波就不能被得到了。
• 当然我们还有非常重要的一点就是，不线性化后，系统根本就不可能被化简成之前的卡尔曼滤波器的好吗！！
• 最终的式子我们要理解为误差的线性状态方程，由此来类比之前的线性的卡尔曼滤波。最终我们再将误差代回就可以得到最终的EKF公式了(可以参考附件的教材)

现在我们直接就给出我们的EKF(扩展卡尔曼滤波)

• 先验估计

${\widehat{x\text{x}x}}_k^{-}={\widetilde{x\text{x}x}}_k=f\text{f}f({\widehat{x\text{x}x}}_{k-1},{u\text{u}u}_{k-1},0)$
${P\text{P}P}_k^{-}=A\text{A}A{P\text{P}P}_{k-1}{A}^T+W\text{W}WQ{W\text{W}W}^T$

• 后验估计

${K\text{K}K}_k={P\text{P}P}_k^{-}{H\text{H}H}^T(H\text{H}H{P\text{P}P}_k^{-}{H\text{H}H}^T+V\text{V}VR\text{R}R{V\text{V}V}^T{)}^{-1}$
${\widehat{x\text{x}x}}_k={\widehat{x\text{x}x}}_k^{-}+{K\text{K}K}_k({Z\text{Z}Z}_k-h\text{h}h({\widetilde{x\text{x}x}}_k,0))$
${P\text{P}P}_k=(I\text{I}I-{K\text{K}K}_{k}H){P\text{P}P}_k^{-}$

卡尔曼滤波和扩展卡尔曼滤波的对比

linear kalman filter	EKF
${\widehat{X\text{X}X}}_k^{-}=A{\widehat{X\text{X}X}}_{k-1}+B{u\text{u}u}_{k-1}$	${\widehat{x\text{x}x}}_k^{-}={\widetilde{x\text{x}x}}_k=f\text{f}f({\widehat{x\text{x}x}}_{k-1},{u\text{u}u}_{k-1},0)$
${P\text{P}P}_k^{-}=A{P\text{P}P}_{k-1}{A}^T+Q$	$\begin{array}{r}{P\text{P}P}_k^{-}=A\text{A}A{P\text{P}P}_{k-1}{A}^T+W\text{W}WQ{W\text{W}W}^T\end{array}$
${K\text{K}K}_k={P\text{P}P}_k^{-}{H}^T(H{P\text{P}P}_k^{-}{H}^T+R\text{R}R{)}^{-1}$	${K\text{K}K}_k={P\text{P}P}_k^{-}{H\text{H}H}^T(H\text{H}H{P\text{P}P}_k^{-}{H\text{H}H}^T+V\text{V}VR\text{R}R{V\text{V}V}^T{)}^{-1}$
${\widehat{X\text{X}X}}_k={\widehat{X\text{X}X}}_k^{-}+{K\text{K}K}_k({Z\text{Z}Z}_k-H{\widehat{X\text{X}X}}_k^{-})$	${\widehat{x\text{x}x}}_k={\widehat{x\text{x}x}}_k^{-}+{K\text{K}K}_k({Z\text{Z}Z}_k-h\text{h}h({\widetilde{x\text{x}x}}_k,0))$
${P\text{P}P}_k=(I\text{I}I-{K\text{K}K}_{k}H){P\text{P}P}_k^{-}$	${P\text{P}P}_k=(I\text{I}I-{K\text{K}K}_{k}H){P\text{P}P}_k^{-}$

参考资料

卡尔曼滤波算法

补充资料

卡尔曼滤波算法与最小二乘