扩展卡尔曼滤波EKF

扩展卡尔曼滤波EKF

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

为了推导EKF,首先假设 x k \boldsymbol{x}_{k} xk 的置信度函数限制为高斯分布:

p ( x k ∣ x ˇ 0 , v 1 : k , y 0 : k ) = N ( x ^ k , P ^ k ) p\left(\boldsymbol{x}_{k} \mid \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k}\right)=\mathcal{N}\left(\hat{\boldsymbol{x}}_{k}, \hat{\boldsymbol{P}}_{k}\right) p(xkxˇ0,v1:k,y0:k)=N(x^k,P^k)

其中, x ^ k \hat{\boldsymbol{x}}_{k} x^k 为均值, P ^ k \hat{\boldsymbol{P}}_{k} P^k 为协方差。

并且假设噪声变量 w k \boldsymbol{w}_{k} wk n k \boldsymbol{n}_{k} nk 也是高斯分布的:

w k ∼ N ( 0 , Q k ) n k ∼ N ( 0 , R k ) \begin{array}{l}\boldsymbol{w}_{k} \sim \mathcal{N}\left(\mathbf{0}, \boldsymbol{Q}_{k}\right) \\\boldsymbol{n}_{k} \sim \mathcal{N}\left(\mathbf{0}, \boldsymbol{R}_{k}\right)\end{array} wkN(0,Qk)nkN(0,Rk)

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

x k = f ( x k − 1 , v k , w k ) y k = g ( x k , n k ) \begin{aligned}&\boldsymbol{x}_{k}=f\left(\boldsymbol{x}_{k-1}, \boldsymbol{v}_{k},\boldsymbol{w}_{k}\right) \\&\boldsymbol{y}_{k}=g\left(\boldsymbol{x}_{k}, \boldsymbol{n}_{k}\right)\end{aligned} xk=f(xk1,vk,wk)yk=g(xk,nk)

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

f ( x k − 1 , v k , w k ) ≈ x ˇ k + F k − 1 ( x k − 1 − x ^ k − 1 ) + w k ′ \boldsymbol{f}\left(\boldsymbol{x}_{k-1}, \boldsymbol{v}_{k}, \boldsymbol{w}_{k}\right) \approx \check{\boldsymbol{x}}_{k}+\boldsymbol{F}_{k-1}\left(\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1}\right)+\boldsymbol{w}_{k}^{\prime} f(xk1,vk,wk)xˇk+Fk1(xk1x^k1)+wk

g ( x k , n k ) ≈ y ˇ k + G k ( x k − x ˇ k ) + n k ′ \boldsymbol{g}\left(\boldsymbol{x}_{k}, \boldsymbol{n}_{k}\right) \approx \check{\boldsymbol{y}}_{k}+\boldsymbol{G}_{k}\left(\boldsymbol{x}_{k}-\check{\boldsymbol{x}}_{k}\right)+\boldsymbol{n}_{k}^{\prime} g(xk,nk)yˇk+Gk(xkxˇk)+nk

其中:

  • x ˇ k = f ( x ^ k − 1 , v k , 0 ) \check{\boldsymbol{x}}_{k}=\boldsymbol{f}\left(\hat{\boldsymbol{x}}_{k-1}, \boldsymbol{v}_{k}, \mathbf{0}\right) xˇk=f(x^k1,vk,0)
  • F k − 1 = ∂ f ( x k − 1 , v k , w k ) ∂ x k − 1 ∣ x ^ k − 1 , v k , 0 \boldsymbol{F}_{k-1}=\left.\frac{\partial \boldsymbol{f}\left(\boldsymbol{x}_{k-1}, \boldsymbol{v}_{k}, \boldsymbol{w}_{k}\right)}{\partial \boldsymbol{x}_{k-1}}\right|_{\hat{\boldsymbol{x}}_{k-1}, \boldsymbol{v}_{k}, \mathbf{0}} Fk1=xk1f(xk1,vk,wk) x^k1,vk,0
  • w k ′ = ∂ f ( x k − 1 , v k , w k ) ∂ w k ∣ x ^ k − 1 , v k , 0 w k \boldsymbol{w}_{k}^{\prime}=\left.\frac{\partial \boldsymbol{f}\left(\boldsymbol{x}_{k-1}, \boldsymbol{v}_{k}, \boldsymbol{w}_{k}\right)}{\partial \boldsymbol{w}_{k}}\right|_{\hat{\boldsymbol{x}}_{k-1}, \boldsymbol{v}_{k}, \mathbf{0}} \boldsymbol{w}_{k} wk=wkf(xk1,vk,wk) x^k1,vk,0wk
  • y ˇ k = g ( x ˇ k , 0 ) \check{\boldsymbol{y}}_{k}=\boldsymbol{g}\left(\check{\boldsymbol{x}}_{k}, \mathbf{0}\right) yˇk=g(xˇk,0)
  • G k = ∂ g ( x k , n k ) ∂ x k ∣ x ˇ k , 0 \boldsymbol{G}_{k}=\left.\frac{\partial \boldsymbol{g}\left(\boldsymbol{x}_{k}, \boldsymbol{n}_{k}\right)}{\partial \boldsymbol{x}_{k}}\right|_{\check{\boldsymbol{x}}_{k}, \mathbf{0}} Gk=xkg(xk,nk) xˇk,0
  • n k ′ = ∂ g ( x k , n k ) ∂ n k ∣ x ˇ k , 0 n k \boldsymbol{n}_{k}^{\prime}=\left.\frac{\partial \boldsymbol{g}\left(\boldsymbol{x}_{k}, \boldsymbol{n}_{k}\right)}{\partial \boldsymbol{n}_{k}}\right|_{\check{\boldsymbol{x}}_{k}, \mathbf{0}} \boldsymbol{n}_{k} nk=nkg(xk,nk) xˇk,0nk

给定过去的状态和最新输入,则当前状态 x k \boldsymbol{x}_{k} xk 的统计学特性为:

x k ≈ x ˇ k + F k − 1 ( x k − 1 − x ^ k − 1 ) + w k ′ \boldsymbol{x}_{k} \approx \check{\boldsymbol{x}}_{k}+\boldsymbol{F}_{k-1}\left(\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1}\right)+\boldsymbol{w}_{k}^{\prime} xkxˇk+Fk1(xk1x^k1)+wk

E [ x k ] ≈ x ˇ k + F k − 1 ( x k − 1 − x ^ k − 1 ) + E [ w k ′ ] ⏟ 0 E\left[\boldsymbol{x}_{k}\right] \approx \check{\boldsymbol{x}}_{k}+\boldsymbol{F}_{k-1}\left(\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1}\right)+\underbrace{E\left[\boldsymbol{w}_{k}^{\prime}\right]}_{0} E[xk]xˇk+Fk1(xk1x^k1)+0 E[wk]

E [ ( x k − E [ x k ] ) ( x k − E [ x k ] ) T ] ≈ E [ w k ′ w k ′ T ] ⏟ Q k ′ E\left[\left(\boldsymbol{x}_{k}-E\left[\boldsymbol{x}_{k}\right]\right)\left(\boldsymbol{x}_{k}-E\left[\boldsymbol{x}_{k}\right]\right)^{\mathrm{T}}\right] \approx \underbrace{E\left[\boldsymbol{w}_{k}^{\prime} \boldsymbol{w}_{k}^{\left.\prime^{\mathrm{T}}\right]}\right.}_{\boldsymbol{Q}_{k}^{\prime}} E[(xkE[xk])(xkE[xk])T]Qk E[wkwkT]

p ( x k ∣ x k − 1 , v k ) ≈ N ( x ˇ k + F k − 1 ( x k − 1 − x ^ k − 1 ) , Q k ′ ) p\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{k-1}, \boldsymbol{v}_{k}\right) \approx \mathcal{N}\left(\check{\boldsymbol{x}}_{k}+\boldsymbol{F}_{k-1}\left(\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1}\right), \boldsymbol{Q}_{k}^{\prime}\right) p(xkxk1,vk)N(xˇk+Fk1(xk1x^k1),Qk)

给定当前状态,则当前观测 y ˇ k \check{\boldsymbol{y}}_{k} yˇk 的统计学特性为:

y k ≈ y ˇ k + G k ( x k − x ˇ k ) + n k ′ \boldsymbol{y}_{k} \approx \check{\boldsymbol{y}}_{k}+\boldsymbol{G}_{k}\left(\boldsymbol{x}_{k}-\check{\boldsymbol{x}}_{k}\right)+\boldsymbol{n}_{k}^{\prime} ykyˇk+Gk(xkxˇk)+nk

E [ y k ] ≈ y ˇ k + G k ( x k − x ˇ k ) + E [ n k ′ ] ⏟ 0 E\left[\boldsymbol{y}_{k}\right] \approx \check{\boldsymbol{y}}_{k}+\boldsymbol{G}_{k}\left(\boldsymbol{x}_{k}-\check{\boldsymbol{x}}_{k}\right)+\underbrace{E\left[\boldsymbol{n}_{k}^{\prime}\right]}_{0} E[yk]yˇk+Gk(xkxˇk)+0 E[nk]

E [ ( y k − E [ y k ] ) ( y k − E [ y k ] ) T ] ≈ E [ n k ′ n k ′ T ] ⏟ R k ′ E\left[\left(\boldsymbol{y}_{k}-E\left[\boldsymbol{y}_{k}\right]\right)\left(\boldsymbol{y}_{k}-E\left[\boldsymbol{y}_{k}\right]\right)^{\mathrm{T}}\right] \approx \underbrace{E\left[\boldsymbol{n}_{k}^{\prime} \boldsymbol{n}_{k}^{\prime \mathrm{T}}\right]}_{\boldsymbol{R}_{k}^{\prime}} E[(ykE[yk])(ykE[yk])T]Rk E[nknkT]

p ( y k ∣ x k ) ≈ N ( y ˇ k + G k ( x k − x ˇ k ) , R k ′ ) p\left(\boldsymbol{y}_{k} \mid \boldsymbol{x}_{k}\right) \approx \mathcal{N}\left(\check{\boldsymbol{y}}_{k}+\boldsymbol{G}_{k}\left(\boldsymbol{x}_{k}-\check{\boldsymbol{x}}_{k}\right), \boldsymbol{R}_{k}^{\prime}\right) p(ykxk)N(yˇk+Gk(xkxˇk),Rk)

由贝叶斯滤波器有:

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 \begin{aligned}& \underbrace{p\left(\boldsymbol{x}_{k} \mid \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k}\right)}_{\text {后验置信度 }} \\=& \eta \underbrace{p\left(\boldsymbol{y}_{k} \mid \boldsymbol{x}_{k}\right)}_{\text {利用 } \boldsymbol{g}(\cdot) \text { 进行更新 }} \int \underbrace{p\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{k-1}, \boldsymbol{v}_{k}\right)}_{\text {利用 } \boldsymbol{f}(\cdot) \text { 进行预测 }} \underbrace{p\left(\boldsymbol{x}_{k-1} \mid \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k-1}, \boldsymbol{y}_{0: k-1}\right)}_{\text {先验置信度 }} \mathrm{d} \boldsymbol{x}_{k-1}\end{aligned} =后验置信度  p(xkxˇ0,v1:k,y0:k)η利用 g() 进行更新  p(ykxk)利用 f() 进行预测  p(xkxk1,vk)先验置信度  p(xk1xˇ0,v1:k1,y0:k1)dxk1

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

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 \begin{array}{l}\underbrace{p\left(\boldsymbol{x}_{k} \mid \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k}\right)}_{\mathcal{N}\left(\hat{\boldsymbol{x}}_{k}, \hat{\boldsymbol{P}}_{k}\right)}=\eta \underbrace{p\left(\boldsymbol{y}_{k} \mid \boldsymbol{x}_{k}\right)}_{\mathcal{N}\left(\check{\boldsymbol{y}}_{k}+\boldsymbol{G}_{k}\left(\boldsymbol{x}_{k}-\check{\boldsymbol{x}}_{k}\right), \boldsymbol{R}_{k}^{\prime}\right)}\\\times \int \underbrace{p\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{k-1}, \boldsymbol{v}_{k}\right)}_{\mathcal{N}\left(\check{\boldsymbol{x}}_{k}+\boldsymbol{F}_{k-1}\left(\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1}\right), \boldsymbol{Q}_{k}^{\prime}\right)} \underbrace{p\left(\boldsymbol{x}_{k-1} \mid \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k-1}, \boldsymbol{y}_{0: k-1}\right)}_{\mathcal{N}\left(\hat{\boldsymbol{x}}_{k-1}, \hat{\boldsymbol{P}}_{k-1}\right)} \mathrm{d} \boldsymbol{x}_{k-1}\end{array} N(x^k,P^k) p(xkxˇ0,v1:k,y0:k)=ηN(yˇk+Gk(xkxˇk),Rk) p(ykxk)×N(xˇk+Fk1(xk1x^k1),Qk) p(xkxk1,vk)N(x^k1,P^k1) p(xk1xˇ0,v1:k1,y0:k1)dxk1

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

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 ′ ) \begin{array}{l}\underbrace{p\left(\boldsymbol{x}_{k} \mid \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k}\right)}_{\mathcal{N}\left(\hat{\boldsymbol{x}}_{k}, \hat{\boldsymbol{P}}_{k}\right)}=\eta \underbrace{p\left(\boldsymbol{y}_{k} \mid \boldsymbol{x}_{k}\right)}_{\mathcal{N}\left(\check{\boldsymbol{y}}_{k}+\boldsymbol{G}_{k}\left(\boldsymbol{x}_{k}-\check{\boldsymbol{x}}_{k}\right), \boldsymbol{R}_{k}^{\prime}\right)} \\\times \underbrace{\int p\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{k-1}, \boldsymbol{v}_{k}\right) p\left(\boldsymbol{x}_{k-1} \mid \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k-1}, \boldsymbol{y}_{0: k-1}\right) \mathrm{d} \boldsymbol{x}_{k-1}}_{\mathcal{N}\left(\check{\boldsymbol{x}}_{k}, \boldsymbol{F}_{k-1} \hat{\boldsymbol{P}}_{k-1} \boldsymbol{F}_{k-1}^{\mathrm{T}}+\boldsymbol{Q}_{k}^{\prime}\right)}\end{array} N(x^k,P^k) p(xkxˇ0,v1:k,y0:k)=ηN(yˇk+Gk(xkxˇk),Rk) p(ykxk)×N(xˇk,Fk1P^k1Fk1T+Qk) p(xkxk1,vk)p(xk1xˇ0,v1:k1,y0:k1)dxk1

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

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 ′ ) ) \begin{aligned}\underbrace{p\left(\boldsymbol{x}_{k} \mid \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k}\right)}_{\mathcal{N}\left(\hat{\boldsymbol{x}}_{k}, \hat{\boldsymbol{P}}_{k}\right)} =& \underbrace{\eta p\left(\boldsymbol{y}_{k} \mid \boldsymbol{x}_{k}\right) \int p\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{k-1}, \boldsymbol{v}_{k}\right) p\left(\boldsymbol{x}_{k-1} \mid \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k-1}, \boldsymbol{y}_{0: k-1}\right) \mathrm{d} \boldsymbol{x}_{k-1}}_{\mathcal{N}\left(\check{\boldsymbol{x}}_{k}+\boldsymbol{K}_{k}\left(\boldsymbol{y}_{k}-\check{\boldsymbol{y}}_{k}\right),\left(\mathbf{1}-\boldsymbol{K}_{k} \boldsymbol{G}_{k}\right)\left(\boldsymbol{F}_{k-1} \hat{\boldsymbol{P}}_{k-1} \boldsymbol{F}_{k-1}^{\mathrm{T}}+\boldsymbol{Q}_{k}^{\prime}\right)\right)}\end{aligned} N(x^k,P^k) p(xkxˇ0,v1:k,y0:k)=N(xˇk+Kk(ykyˇk),(1KkGk)(Fk1P^k1Fk1T+Qk)) ηp(ykxk)p(xkxk1,vk)p(xk1xˇ0,v1:k1,y0:k1)dxk1

其中, K k \boldsymbol{K}_{k} Kk 为卡尔曼增益。比较上面式子的左右两侧,有:

预测

x ˇ k = f ( x ^ k − 1 , v k , 0 ) \check{\boldsymbol{x}}_{k} =\boldsymbol{f}\left(\hat{\boldsymbol{x}}_{k-1}, \boldsymbol{v}_{k}, \mathbf{0}\right) xˇk=f(x^k1,vk,0)

P ˇ k = F k − 1 P ^ k − 1 F k − 1 T + Q k ′ \check{\boldsymbol{P}}_{k} =\boldsymbol{F}_{k-1} \hat{\boldsymbol{P}}_{k-1} \boldsymbol{F}_{k-1}^{\mathrm{T}}+\boldsymbol{Q}_{k}^{\prime} Pˇk=Fk1P^k1Fk1T+Qk

卡尔曼增益

K k = P ˇ k G k T ( G k P ˇ k G k T + R k ′ ) − 1 \boldsymbol{K}_{k} =\check{\boldsymbol{P}}_{k} \boldsymbol{G}_{k}^{\mathrm{T}}\left(\boldsymbol{G}_{k} \check{\boldsymbol{P}}_{k} \boldsymbol{G}_{k}^{\mathrm{T}}+\boldsymbol{R}_{k}^{\prime}\right)^{-1} Kk=PˇkGkT(GkPˇkGkT+Rk)1

更新

x ^ k = x ˇ k + K k ( y k − g ( x ˇ k , 0 ) ) ⏟ 更新量  \hat{\boldsymbol{x}}_{k} =\check{\boldsymbol{x}}_{k}+\boldsymbol{K}_{k} \underbrace{\left(\boldsymbol{y}_{k}-\boldsymbol{g}\left(\check{\boldsymbol{x}}_{k}, \mathbf{0}\right)\right)}_{\text {更新量 }} x^k=xˇk+Kk更新量  (ykg(xˇk,0))

P ^ k = ( 1 − K k G k ) P ˇ k \hat{\boldsymbol{P}}_{k} =\left(\mathbf{1}-\boldsymbol{K}_{k} \boldsymbol{G}_{k}\right) \check{\boldsymbol{P}}_{k} P^k=(1KkGk)Pˇk

  • 0
    点赞
  • 3
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值