欢迎来到尧图网

客户服务 关于我们

您的位置:首页 > 汽车 > 新车 > 扩展卡尔曼滤波

扩展卡尔曼滤波

2025/3/22 11:57:19 来源:https://blog.csdn.net/dmf_fff/article/details/146407873  浏览:    关键词:扩展卡尔曼滤波

1.非线性系统的线性化

  • 标准卡尔曼滤波 适用于线性化系统,扩展卡尔曼滤波 则扩展到了非线性系统,核心原理就是将非线性系统线性化,主要用的的知识点是 泰勒展开(我另外一篇文章的链接),如下是泰勒展开的公式:
    P n ( x ) = f ( a ) + f ′ ( a ) ( x − a ) + f ′ ′ ( a ) 2 ! ( x − a ) 2 + ⋯ + f ( n ) ( a ) n ! ( x − a ) n P_n(x)=f(a)+f'(a)(x-a)+\frac{f''(a)}{2!}(x-a)^2+\cdots+\frac{f^{(n)}(a)}{n!}(x-a)^n Pn(x)=f(a)+f(a)(xa)+2!f′′(a)(xa)2++n!f(n)(a)(xa)n
  • 如下图所示 泰勒展开 公式所示,只看红框部分(不考虑高阶),其本身就是一个线性化的公式。
    在这里插入图片描述
  • 以下是 y = sin ⁡ ( x ) y\:=\:\sin(x) y=sin(x) 和他的一阶 泰勒展开,显然,他是线性的。
    在这里插入图片描述
  • 结合 泰勒展开 中的描述,一阶泰勒公式只有 在他附近的点上是相对准确的,应注意。

2. 扩展卡尔曼滤波的状态空间方程

2.1 标准卡尔曼滤波的状态空间方程

  • 关于标准卡尔曼滤波可以参考问哦的另外一篇文章 标准卡尔曼滤波 。
    x k = A x k − 1 + B u k − 1 + w k − 1 / / 状态转移方程 − − − − z k = H x k + v k / / 观测方程 x_k = A x_{k-1} + Bu_{k-1} + w_{k-1} // 状态转移方程\\ ----\\ z_k = Hx_k + v_k //观测方程 xk=Axk1+Buk1+wk1//状态转移方程zk=Hxk+vk//观测方程

2.1 扩展卡尔曼滤波的状态空间方程

x k = f ( x k − 1 , u k − 1 , w k − 1 ) / / 状态方程 − − − − z k = h ( x k , v k ) / / 观测方程 x_k=f(x_{k-1},u_{k-1}, w_{k-1}) // 状态方程\\ ----\\ z_k=h(x_k, v_k) //观测方程 xk=f(xk1,uk1,wk1)//状态方程zk=h(xk,vk)//观测方程

  • f f f h h h 为非线性函数;
  • w k w_k wk: 过程噪声, 假设符合正态分布, w k ∼ N ( 0 , Q ) w_k\sim\mathcal{N}(0,Q) wkN(0,Q), 数学期望为0,误差的协方差矩阵为 Q Q Q
  • v k v_k vk: 测量噪声, 假设概率分布符合正态分布, v k ∼ N ( 0 , R ) v_k\sim\mathcal{N}(0,R) vkN(0,R),数学期望为0,误差的协方差矩阵为 R R R

3. 状态方程和观测方程的线性化

  • 使用 泰勒展开 进行线性化,即在一个点处展开,理想情况是在真实点处展开,但我们并不知道真实点在哪(如果知道就不用卡尔曼滤波了),退而求其次,此时的最优估计点就是 当前时刻的 后验估计,通过在 k − 1 k-1 k1 时刻的后验估计点展开, 得到 k k k 时刻的状态。
  • 状态方程 的线性化
    x k = f ( x ^ k − 1 , u k − 1 , 0 ) + A ( x k − x ^ k − 1 ) + W k w k − 1 x_k=f( \hat{\mathbf{x}}_{k-1},u_{k-1}, 0) + A(x_k - \hat{\mathbf{x}}_{k-1}) + W_k w_{k-1} xk=f(x^k1,uk1,0)+A(xkx^k1)+Wkwk1
    • 在原本非线性化的方程( x k = f ( x k − 1 , u k − 1 , w k − 1 ) x_k=f(x_{k-1},u_{k-1}, w_{k-1}) xk=f(xk1,uk1,wk1) )中, 是由上一时刻的真实值 x k − 1 x_{k-1} xk1 得到下一时刻的真实值 x k x_k xk, 但在进行线性化是是没有办法得到真实值的,所以就使用 x k − 1 x_{k-1} xk1 时刻的 后验估计 处展开。
    • A A A:状态转移函数 f f f 对状态 x ^ k − 1 \hat{\mathbf{x}}_{k-1} x^k1 的雅可比矩阵, 也就是 f f f x ^ k − 1 \hat{\mathbf{x}}_{k-1} x^k1 这个点求偏导 A = ∂ f ( x ^ k − 1 , u k − 1 , 0 ) ∂ x A=\frac{\partial f(\hat{\mathbf{x}}_{k-1},u_{k-1}, 0)}{\partial x} A=xf(x^k1,uk1,0)
    • W k W_k Wk f f f w k − 1 w_{k-1} wk1 的偏导, W k = ∂ f ( x ^ k − 1 , u k − 1 , 0 ) ∂ w W_k=\frac{\partial f(\hat{\mathbf{x}}_{k-1},u_{k-1}, 0)}{\partial w} Wk=wf(x^k1,uk1,0)
    • W k w k − 1 W_k w_{k-1} Wkwk1 符合正态分布数学期望为 0 0 0,协方差矩阵为 W Q W T WQW^T WQWT
      W k w k − 1 ∼ N ( 0 , W Q W T ) W_k w_{k-1}\sim\mathcal{N}(0,WQW^T) Wkwk1N(0,WQWT)
  • 观测方程的线性化
    z k = h ( f ( x ^ k − 1 , u k − 1 , 0 ) , 0 ) + H ( x k − f ( x ^ k − 1 , u k − 1 , 0 ) ) + V v k z_k = h(f( \hat{\mathbf{x}}_{k-1},u_{k-1}, 0),0) + H(x_k - f( \hat{\mathbf{x}}_{k-1},u_{k-1}, 0)) + Vv_k zk=h(f(x^k1,uk1,0),0)+H(xkf(x^k1,uk1,0))+Vvk
    • 同样的在观测方程中也是找不到真实点的,同样是在 k − 1 k-1 k1 时刻的 后验估计 处展开,得到近似的 真实点 的位置。
    • H = ∂ h ( f ( x ^ k − 1 , u k − 1 , 0 ) , 0 ) ∂ x H =\frac{\partial h(f( \hat{\mathbf{x}}_{k-1},u_{k-1}, 0),0) }{\partial x} H=xh(f(x^k1,uk1,0),0)
    • V k = ∂ h ( f ( x ^ k − 1 , u k − 1 , 0 ) , 0 ) ∂ v V_k =\frac{\partial h(f( \hat{\mathbf{x}}_{k-1},u_{k-1}, 0),0) }{\partial v} Vk=vh(f(x^k1,uk1,0),0)
    • V v k Vv_k Vvk 符合正态分布数学期望为 0 0 0,协方差矩阵为 V R V T VRV^T VRVT
      V v k ∼ N ( 0 , V R V T ) Vv_k\sim\mathcal{N}(0,VRV^T) VvkN(0,VRVT)
  • 为了方便理解,上面线性化后的公式并没有化简,通常们会写成如下形式:
    • x ~ k = f ( x ^ k − 1 , u k − 1 , 0 ) \tilde{x}_k = f( \hat{\mathbf{x}}_{k-1},u_{k-1}, 0) x~k=f(x^k1,uk1,0) 状态方程和观测方程就成为如下形式:
      x k = x ~ k + A ( x k − x ^ k − 1 ) + W k w k − 1 x_k=\tilde{x}_k + A(x_k - \hat{\mathbf{x}}_{k-1}) + W_k w_{k-1} xk=x~k+A(xkx^k1)+Wkwk1
      z k = h ( x ~ k , 0 ) + H ( x k − x ~ k ) + V v k z_k = h(\tilde{x}_k,0) + H(x_k - \tilde{x}_k) + Vv_k zk=h(x~k,0)+H(xkx~k)+Vvk
    • z ~ k = h ( x ~ k , 0 ) \tilde{z}_k =h(\tilde{x}_k,0) z~k=h(x~k,0)状态方程和观测方程就成为如下形式: x k = x ~ k + A ( x k − x ^ k − 1 ) + W k w k − 1 x_k=\tilde{x}_k + A(x_k - \hat{\mathbf{x}}_{k-1}) + W_k w_{k-1} xk=x~k+A(xkx^k1)+Wkwk1
      z k = z ~ k + H ( x k − x ~ k ) + V v k z_k = \tilde{z}_k + H(x_k - \tilde{x}_k) + Vv_k zk=z~k+H(xkx~k)+Vvk

4.扩展卡尔曼滤波的递推流程

  • 扩展卡尔曼滤波的递推流程在标准卡尔曼滤波的基础上进行修改我把标准卡尔曼滤波的递推流程也写在此处,方便对比

4.1 标准卡尔曼滤波的递推流程

  • 先验估计
    x ^ k − = F x ^ k − 1 + B u k − 1 / / 状态的先验估计 \hat{\mathbf{x}}_k^-=\mathbf{F} \hat{\mathbf{x}}_{k-1}+\mathbf{B} \mathbf{u}_{k-1} //状态的先验估计 x^k=Fx^k1+Buk1//状态的先验估计
    P k − = F k P k − 1 F k ⊤ + Q k / / 误差协方差矩阵的先验估计 \mathbf{P}_k^-=\mathbf{F}_k\mathbf{P}_{k-1}\mathbf{F}_k^\top+\mathbf{Q}_k // 误差协方差矩阵的先验估计 Pk=FkPk1Fk+Qk//误差协方差矩阵的先验估计
  • 后验估计
    K k = P k − H T H P k − H T + R k / / 卡尔曼增益 K_k=\frac{P_{k}^{-}H^{T}}{HP_{k}^{-}H^{T}+R_k} //卡尔曼增益 Kk=HPkHT+RkPkHT//卡尔曼增益
    x ^ k = x ^ k − + k k ( z k − H x ^ k − ) / / 后验估计 ( 当前时刻的最优估计 ) \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + k_k (z_k - H\hat{\mathbf{x}}_k^-) //后验估计(当前时刻的最优估计) x^k=x^k+kk(zkHx^k)//后验估计(当前时刻的最优估计)
    P k = ( I − K k H ) P k − / / 更新误差的协方差矩阵 ; I : 单位矩阵 P_{k}=(I-K_{k}H)P_{k}^{-} // 更新误差的协方差矩阵 ; I:单位矩阵 Pk=(IKkH)Pk//更新误差的协方差矩阵;I:单位矩阵

4.2 扩展卡尔曼滤波的递推流程

  • 先验估计
    x ^ k − = f ( x ^ k − 1 , u k − 1 , 0 ) / / 直接使用非线性的公式进行先验估计 \hat{\mathbf{x}}_k^- = f( \hat{\mathbf{x}}_{k-1},u_{k-1}, 0) //直接使用非线性的公式进行先验估计 x^k=f(x^k1,uk1,0)//直接使用非线性的公式进行先验估计
    P k − = F k P k − 1 F k ⊤ + W Q W T / / 误差协方差矩阵的先验估计 \mathbf{P}_k^-=\mathbf{F}_k\mathbf{P}_{k-1}\mathbf{F}_k^\top+WQW^T // 误差协方差矩阵的先验估计 Pk=FkPk1Fk+WQWT//误差协方差矩阵的先验估计
  • 后验估计
    K k = P k − H T H P k − H T + V R V T / / 卡尔曼增益 K_k=\frac{P_{k}^{-}H^{T}}{HP_{k}^{-}H^{T}+VRV^T} //卡尔曼增益 Kk=HPkHT+VRVTPkHT//卡尔曼增益
    x ^ k = x ^ k − + k k ( z k − h ( x ^ k − , 0 ) ) / / 后验估计 ( 当前时刻的最优估计 ) \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + k_k (z_k -h(\hat{\mathbf{x}}_k^-,0) ) //后验估计(当前时刻的最优估计) x^k=x^k+kk(zkh(x^k,0))//后验估计(当前时刻的最优估计)
    P k = ( I − K k H ) P k − / / 更新误差的协方差矩阵 ; I : 单位矩阵 P_{k}=(I-K_{k}H)P_{k}^{-} // 更新误差的协方差矩阵 ; I:单位矩阵 Pk=(IKkH)Pk//更新误差的协方差矩阵;I:单位矩阵

版权声明:

本网仅为发布的内容提供存储空间,不对发表、转载的内容提供任何形式的保证。凡本网注明“来源:XXX网络”的作品,均转载自其它媒体,著作权归作者所有,商业转载请联系作者获得授权,非商业转载请注明出处。

我们尊重并感谢每一位作者,均已注明文章来源和作者。如因作品内容、版权或其它问题,请及时与我们联系,联系邮箱:809451989@qq.com,投稿邮箱:809451989@qq.com

热搜词