扩展卡尔曼滤波

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)(x−a)+2!f′′(a)(x−a)2+⋯+n!f(n)(a)(x−a)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=Axk−1+Buk−1+wk−1//状态转移方程−−−−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(xk−1,uk−1,wk−1)//状态方程−−−−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) wk∼N(0,Q)， 数学期望为0，误差的协方差矩阵为 Q Q Q
• v k v_k vk: 测量噪声, 假设概率分布符合正态分布， v k ∼ N ( 0 , R ) v_k\sim\mathcal{N}(0,R) vk∼N(0,R),数学期望为0，误差的协方差矩阵为 R R R

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

• 使用 泰勒展开 进行线性化，即在一个点处展开，理想情况是在真实点处展开，但我们并不知道真实点在哪（如果知道就不用卡尔曼滤波了），退而求其次，此时的最优估计点就是 当前时刻的 后验估计 ，通过在 k − 1 k-1 k−1 时刻的后验估计点展开， 得到 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^k−1,uk−1,0)+A(xk−x^k−1)+Wkwk−1
  • 在原本非线性化的方程（ 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(xk−1,uk−1,wk−1) ）中, 是由上一时刻的真实值 x k − 1 x_{k-1} xk−1 得到下一时刻的真实值 x k x_k xk, 但在进行线性化是是没有办法得到真实值的，所以就使用 x k − 1 x_{k-1} xk−1 时刻的 后验估计 处展开。
  • A A A：状态转移函数 f f f 对状态 x ^ k − 1 \hat{\mathbf{x}}{k-1} x^k−1 的雅可比矩阵, 也就是 f f f 在 x ^ k − 1 \hat{\mathbf{x}}{k-1} x^k−1 这个点求偏导 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=∂x∂f(x^k−1,uk−1,0)，
  • W k W_k Wk： f f f 对 w k − 1 w_{k-1} wk−1 的偏导， 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=∂w∂f(x^k−1,uk−1,0)
  • W k w k − 1 W_k w_{k-1} Wkwk−1 符合正态分布数学期望为 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) Wkwk−1∼N(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^k−1,uk−1,0),0)+H(xk−f(x^k−1,uk−1,0))+Vvk
  • 同样的在观测方程中也是找不到真实点的，同样是在 k − 1 k-1 k−1 时刻的 后验估计 处展开，得到近似的 真实点 的位置。
  • 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=∂x∂h(f(x^k−1,uk−1,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=∂v∂h(f(x^k−1,uk−1,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) Vvk∼N(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^k−1,uk−1,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(xk−x^k−1)+Wkwk−1
    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(xk−x~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(xk−x^k−1)+Wkwk−1
    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(xk−x~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^k−1+Buk−1//状态的先验估计
  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−=FkPk−1Fk⊤+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=HPk−HT+RkPk−HT//卡尔曼增益
  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(zk−Hx^k−)//后验估计(当前时刻的最优估计)
  P k = ( I − K k H ) P k − / / 更新误差的协方差矩阵 ; I : 单位矩阵 P{k}=(I-K{k}H)P{k}^{-} // 更新误差的协方差矩阵 ; I:单位矩阵 Pk=(I−KkH)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^k−1,uk−1,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−=FkPk−1Fk⊤+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=HPk−HT+VRVTPk−HT//卡尔曼增益
  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(zk−h(x^k−,0))//后验估计(当前时刻的最优估计)
  P k = ( I − K k H ) P k − / / 更新误差的协方差矩阵 ; I : 单位矩阵 P{k}=(I-K{k}H)P{k}^{-} // 更新误差的协方差矩阵 ; I:单位矩阵 Pk=(I−KkH)Pk−//更新误差的协方差矩阵;I:单位矩阵