扩展卡尔曼滤波EKF

如果将置信度和噪声限制为高斯分布，并且对运动模型和观测进行线性化，计算贝叶斯滤波中的积分（以及归一化积），即可得到扩展卡尔曼滤波（EKF）。

为了推导EKF，首先假设 ${\mathbf{x}}_k$ 的置信度函数限制为高斯分布：

$$p\left({\mathbf{x}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k}\right)=\mathcal{N}\left({\hat{\mathbf{x}}}_k,{\hat{\mathbf{P}}}_k\right)$$

其中，${\hat{\mathbf{x}}}_k$ 为均值，${\hat{\mathbf{P}}}_k$ 为协方差。

并且假设噪声变量 ${\mathbf{w}}_k$ 和 ${\mathbf{n}}_k$ 也是高斯分布的：

w k ∼ N ( 0 , Q k ) n k ∼ N ( 0 , R k )

wk​∼N(0,Qk​)nk​∼N(0,Rk​)​

对于以下运动和观测模型：

x k = f ( x k − 1 , v k , w k ) y k = g ( x k , n k )

​xk​=f(xk−1​,vk​,wk​)yk​=g(xk​,nk​)​

由于 $\mathbf{f}(\cdot )$ 和 $\mathbf{g}(\cdot )$ 是非线性函数，所以我们需要对其进行线性化。在当前状态的均值处展开，对运动和观测模型进行线性化：

$$\mathbf{f}\left({\mathbf{x}}_{k-1},{\mathbf{v}}_k,{\mathbf{w}}_k\right)\approx {\check{\mathbf{x}}}_k+{\mathbf{F}}_{k-1}\left({\mathbf{x}}_{k-1}-{\hat{\mathbf{x}}}_{k-1}\right)+{\mathbf{w}}_k^{\prime }$$

$$\mathbf{g}\left({\mathbf{x}}_k,{\mathbf{n}}_k\right)\approx {\check{\mathbf{y}}}_k+{\mathbf{G}}_k\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right)+{\mathbf{n}}_k^{\prime }$$

其中：

• ${\check{\mathbf{x}}}_k=\mathbf{f}\left({\hat{\mathbf{x}}}_{k-1},{\mathbf{v}}_k,0\right)$
• ${\mathbf{F}}_{k-1}={\frac{\partial \mathbf{f}\left({\mathbf{x}}_{k-1},{\mathbf{v}}_k,{\mathbf{w}}_k\right)}{\partial {\mathbf{x}}_{k-1}}|}_{{\hat{\mathbf{x}}}_{k-1},{\mathbf{v}}_k,0}$
• ${\mathbf{w}}_k^{\prime }={\frac{\partial \mathbf{f}\left({\mathbf{x}}_{k-1},{\mathbf{v}}_k,{\mathbf{w}}_k\right)}{\partial {\mathbf{w}}_k}|}_{{\hat{\mathbf{x}}}_{k-1},{\mathbf{v}}_k,0}{\mathbf{w}}_k$
• ${\check{\mathbf{y}}}_k=\mathbf{g}\left({\check{\mathbf{x}}}_k,0\right)$
• ${\mathbf{G}}_k={\frac{\partial \mathbf{g}\left({\mathbf{x}}_k,{\mathbf{n}}_k\right)}{\partial {\mathbf{x}}_k}|}_{{\check{\mathbf{x}}}_k,0}$
• ${\mathbf{n}}_k^{\prime }={\frac{\partial \mathbf{g}\left({\mathbf{x}}_k,{\mathbf{n}}_k\right)}{\partial {\mathbf{n}}_k}|}_{{\check{\mathbf{x}}}_k,0}{\mathbf{n}}_k$

给定过去的状态和最新输入，则当前状态 ${\mathbf{x}}_k$ 的统计学特性为：

$${\mathbf{x}}_k\approx {\check{\mathbf{x}}}_k+{\mathbf{F}}_{k-1}\left({\mathbf{x}}_{k-1}-{\hat{\mathbf{x}}}_{k-1}\right)+{\mathbf{w}}_k^{\prime }$$

$$E\left[{\mathbf{x}}_k\right]\approx {\check{\mathbf{x}}}_k+{\mathbf{F}}_{k-1}\left({\mathbf{x}}_{k-1}-{\hat{\mathbf{x}}}_{k-1}\right)+\underset{0}{\underbrace{E\left[{\mathbf{w}}_k^{\prime }\right]}}$$

$$E\left[\left({\mathbf{x}}_k-E\left[{\mathbf{x}}_k\right]\right){\left({\mathbf{x}}_k-E\left[{\mathbf{x}}_k\right]\right)}^{\mathrm{T}}\right]\approx \underset{{\mathbf{Q}}_k^{\prime }}{\underbrace{E[{\mathbf{w}}_k^{\prime }{\mathbf{w}}_k^{{\prime }^{\mathrm{T}}]}}}$$

$$p\left({\mathbf{x}}_k\mid {\mathbf{x}}_{k-1},{\mathbf{v}}_k\right)\approx \mathcal{N}\left({\check{\mathbf{x}}}_k+{\mathbf{F}}_{k-1}\left({\mathbf{x}}_{k-1}-{\hat{\mathbf{x}}}_{k-1}\right),{\mathbf{Q}}_k^{\prime }\right)$$

给定当前状态，则当前观测 ${\check{\mathbf{y}}}_k$ 的统计学特性为：

$${\mathbf{y}}_k\approx {\check{\mathbf{y}}}_k+{\mathbf{G}}_k\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right)+{\mathbf{n}}_k^{\prime }$$

$$E\left[{\mathbf{y}}_k\right]\approx {\check{\mathbf{y}}}_k+{\mathbf{G}}_k\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right)+\underset{0}{\underbrace{E\left[{\mathbf{n}}_k^{\prime }\right]}}$$

$$E\left[\left({\mathbf{y}}_k-E\left[{\mathbf{y}}_k\right]\right){\left({\mathbf{y}}_k-E\left[{\mathbf{y}}_k\right]\right)}^{\mathrm{T}}\right]\approx \underset{{\mathbf{R}}_k^{\prime }}{\underbrace{E\left[{\mathbf{n}}_k^{\prime }{\mathbf{n}}_k^{\prime \mathrm{T}}\right]}}$$

$$p\left({\mathbf{y}}_k\mid {\mathbf{x}}_k\right)\approx \mathcal{N}\left({\check{\mathbf{y}}}_k+{\mathbf{G}}_k\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right),{\mathbf{R}}_k^{\prime }\right)$$

由贝叶斯滤波器有：

p ( x k ∣ x ˇ 0 , v 1 : k , y 0 : k ) ⏟ 后验置信度  = η p ( y k ∣ x k ) ⏟ 利用  g ( ⋅ )  进行更新  ∫ p ( x k ∣ x k − 1 , v k ) ⏟ 利用  f ( ⋅ )  进行预测  p ( x k − 1 ∣ x ˇ 0 , v 1 : k − 1 , y 0 : k − 1 ) ⏟ 先验置信度  d x k − 1

=​后验置信度  p(xk​∣xˇ0​,v1:k​,y0:k​)​​η利用 g(⋅) 进行更新  p(yk​∣xk​)​​∫利用 f(⋅) 进行预测  p(xk​∣xk−1​,vk​)​​先验置信度  p(xk−1​∣xˇ0​,v1:k−1​,y0:k−1​)​​dxk−1​​

将线性化之后的运动和观测模型代入到贝叶斯滤波器中，有：

p ( x k ∣ x ˇ 0 , v 1 : k , y 0 : k ) ⏟ N ( x ^ k , P ^ k ) = η p ( y k ∣ x k ) ⏟ N ( y ˇ k + G k ( x k − x ˇ k ) , R k ′ ) × ∫ p ( x k ∣ x k − 1 , v k ) ⏟ N ( x ˇ k + F k − 1 ( x k − 1 − x ^ k − 1 ) , Q k ′ ) p ( x k − 1 ∣ x ˇ 0 , v 1 : k − 1 , y 0 : k − 1 ) ⏟ N ( x ^ k − 1 , P ^ k − 1 ) d x k − 1

N(x^k​,P^k​) p(xk​∣xˇ0​,v1:k​,y0:k​)​​=ηN(yˇ​k​+Gk​(xk​−xˇk​),Rk′​) p(yk​∣xk​)​​×∫N(xˇk​+Fk−1​(xk−1​−x^k−1​),Qk′​) p(xk​∣xk−1​,vk​)​​N(x^k−1​,P^k−1​) p(xk−1​∣xˇ0​,v1:k−1​,y0:k−1​)​​dxk−1​​

将服从高斯分布的变量传入到非线性函数中，积分之后仍然服从高斯分布：

p ( x k ∣ x ˇ 0 , v 1 : k , y 0 : k ) ⏟ N ( x ^ k , P ^ k ) = η p ( y k ∣ x k ) ⏟ N ( y ˇ k + G k ( x k − x ˇ k ) , R k ′ ) × ∫ p ( x k ∣ x k − 1 , v k ) p ( x k − 1 ∣ x ˇ 0 , v 1 : k − 1 , y 0 : k − 1 ) d x k − 1 ⏟ N ( x ˇ k , F k − 1 P ^ k − 1 F k − 1 T + Q k ′ )

N(x^k​,P^k​) p(xk​∣xˇ0​,v1:k​,y0:k​)​​=ηN(yˇ​k​+Gk​(xk​−xˇk​),Rk′​) p(yk​∣xk​)​​×N(xˇk​,Fk−1​P^k−1​Fk−1T​+Qk′​) ∫p(xk​∣xk−1​,vk​)p(xk−1​∣xˇ0​,v1:k−1​,y0:k−1​)dxk−1​​​​

再利用高斯概率密度函数的归一化积的性质，有：

p ( x k ∣ x ˇ 0 , v 1 : k , y 0 : k ) ⏟ N ( x ^ k , P ^ k ) = η p ( y k ∣ x k ) ∫ p ( x k ∣ x k − 1 , v k ) p ( x k − 1 ∣ x ˇ 0 , v 1 : k − 1 , y 0 : k − 1 ) d x k − 1 ⏟ N ( x ˇ k + K k ( y k − y ˇ k ) , ( 1 − K k G k ) ( F k − 1 P ^ k − 1 F k − 1 T + Q k ′ ) )

N(x^k​,P^k​) p(xk​∣xˇ0​,v1:k​,y0:k​)​​=​N(xˇk​+Kk​(yk​−yˇ​k​),(1−Kk​Gk​)(Fk−1​P^k−1​Fk−1T​+Qk′​)) ηp(yk​∣xk​)∫p(xk​∣xk−1​,vk​)p(xk−1​∣xˇ0​,v1:k−1​,y0:k−1​)dxk−1​​​​

其中，${\mathbf{K}}_k$ 为卡尔曼增益。比较上面式子的左右两侧，有：

预测

$${\check{\mathbf{x}}}_k=\mathbf{f}\left({\hat{\mathbf{x}}}_{k-1},{\mathbf{v}}_k,0\right)$$

$${\check{\mathbf{P}}}_k={\mathbf{F}}_{k-1}{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}_{k-1}^{\mathrm{T}}+{\mathbf{Q}}_k^{\prime }$$

卡尔曼增益

$${\mathbf{K}}_k={\check{\mathbf{P}}}_k{\mathbf{G}}_k^{\mathrm{T}}{\left({\mathbf{G}}_k{\check{\mathbf{P}}}_k{\mathbf{G}}_k^{\mathrm{T}}+{\mathbf{R}}_k^{\prime }\right)}^{-1}$$

更新

$${\hat{\mathbf{x}}}_k={\check{\mathbf{x}}}_k+{\mathbf{K}}_k\underset{\text{更新量 }}{\underbrace{\left({\mathbf{y}}_k-\mathbf{g}\left({\check{\mathbf{x}}}_k,0\right)\right)}}$$

$${\hat{\mathbf{P}}}_k=\left(1-{\mathbf{K}}_k{\mathbf{G}}_k\right){\check{\mathbf{P}}}_k$$