标准卡尔曼滤波

发布于:2025-03-19 ⋅ 阅读:(258) ⋅ 点赞:(0)

1.状态转移方程和观测方程

  • 状态转移方程
    x k = A x k − 1 + B u k + w k x_k = A x_{k-1} + Bu_k + w_k xk​=Axk−1​+Buk​+wk​
    • x k x_k xk​:k时刻的 状态向量,理论上的真实状态。
    • x k − 1 x_{k-1} xk−1​:k-1时刻的 状态向量,理论上的真实状态。
    • A A A:状态转移矩阵,用来决定下一时刻 状态向量 在 状态空间 中的位置
    • B , u k B, u_k B,uk​:控制矩阵 和 控制向量,用来对 状态向量 的位置进行修正或控制
    • w k w_k wk​:过程噪声,模型本身引入的噪声,假定其符合 正态分布 ,数学期望(均值)为0,协方差矩阵为 Q Q Q,
  • 观测方程
    z k = H x k + v k z_k = Hx_k + v_k zk​=Hxk​+vk​
    • z k z_k zk​:k时刻的观测值
    • H H H:观测矩阵,用于将 x k x_k xk​ 映射到观测空间
    • x k x_k xk​:与 状态转移方程 中的 x k x_k xk​ 意义相同
    • v k v_k vk​:测量噪声,测量不准确引入的噪声,假定其符合 正态分布 ,数学期望(均值)为0,协方差矩阵为 Q Q Q,
  • 注意:
    • 在上述两个公式中 x k x_k xk​,代表 k k k 时刻的 真实值,他是一个理想值,他假定预测模型和测量是完备的,误差也是已知的,但这在现实中是不可能的,卡尔曼滤波本身也是,不断通过 预测 和 修正 两个阶段交替进行,使计算结果不断逼近真实结果 x k x_k xk​ 的一种方法。
    • 也可以这么理解卡尔曼滤波:模型估计 和 测量 都有误差,误差的 概率分布 符合 正态分布(高斯分布),也就是 估计值 和 测量值都不准,卡尔曼滤波就是在 估计值 和 测量值 之间找到一个最优平衡点, 也就是找到 概率密度 最高的那个点,是一种最优估计的算法。

    如果误差都是已知的,那就直接修正了,那就等于没有误差,也不需要滤波,一个公式,直接就得到真结果了,那显然是不可能的。

2.卡尔曼增益 K k K_k Kk​

  • 依据上述的状态方程和观测方程,可以得到下面两个公式:
    x ^ k − = F x ^ k − 1 + B u k \hat{\mathbf{x}}_k^-=\mathbf{F} \hat{\mathbf{x}}_{k-1}+\mathbf{B} \mathbf{u}_k x^k−​=Fx^k−1​+Buk​
    z k = H x ^ k m e a x ^ k m e a = H − z k \mathbf{z}_k=\mathbf{H} \hat{\mathbf{x}}_{k_{mea}} \\ \hat{\mathbf{x}}_{k_{mea}} = H^-z_k zk​=Hx^kmea​​x^kmea​​=H−zk​

    • x ^ k − \hat{\mathbf{x}}_k^- x^k−​:当前状态的 先验估计 ,通过模型计算出来的值。
    • x ^ k m e a \hat{\mathbf{x}}_{k_{mea}} x^kmea​​ 测量出来的值。

    此时, x ^ k − \hat{\mathbf{x}}_k^- x^k−​ 和 x ^ k m e a \hat{\mathbf{x}}_{k_{mea}} x^kmea​​ 都是不准的,下一步就是从这两个不准的值中找到 最优估计

  • 假设存在一个系数 G G G,决定了我们应该相信 算出来的结果 还是 测出来的结果
    x ^ k = x ^ k − + G ( H − z k − x ^ k − ) \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + G(H^-z_k - \hat{\mathbf{x}}_k^-) x^k​=x^k−​+G(H−zk​−x^k−​)

    • 假设 G = 0 G = 0 G=0 , x ^ k = x ^ k − \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- x^k​=x^k−​, 这时应该相信 算出来的结果。
    • 假设 G = 1 G = 1 G=1, x ^ k = H − z k \hat{\mathbf{x}}_k = H^-z_k x^k​=H−zk​,这时更应该相信 测出来的结果

    x ^ k \hat{\mathbf{x}}_k x^k​:后验估计,也就是当前时刻的最优估计值

  • 对这个公式进行一个变换, 是 G = k k H G = k_k H G=kk​H, 带入上述公式得到
    x ^ k = x ^ k − + G ( H − z k − x ^ k − ) x ^ k = x ^ k − + k k H ( H − z k − x ^ k − ) − − − − − x ^ k = x ^ k − + k k ( z k − H x ^ k − ) \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + G(H^-z_k - \hat{\mathbf{x}}_k^-) \\ \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + k_k H(H^-z_k - \hat{\mathbf{x}}_k^-) \\ -----\\ \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + k_k (z_k - H\hat{\mathbf{x}}_k^-) \\ x^k​=x^k−​+G(H−zk​−x^k−​)x^k​=x^k−​+kk​H(H−zk​−x^k−​)−−−−−x^k​=x^k−​+kk​(zk​−Hx^k−​)

    • x ^ k \hat{\mathbf{x}}_k x^k​:后验估计
    • 此时 K k K_k Kk​ 的取值范围是 0 − H − {0 - H^-} 0−H−
  • K k K_k Kk​ 就是 卡尔曼增益, 后续主要解决 k k k_k kk​ 的取值

3. 误差的协方差矩阵 P k P_k Pk​

  • 每个时刻都会计算出一个 最优的估计(后验估计) x ^ k \hat{\mathbf{x}}_k x^k​, 通常情况下,后验估计 和 真实值 仍然存在误差,假设误差为 e k e_k ek​ :
    e k = x k − x ^ k / / 真实值 − 后验估计 e_k = x_k - \hat{\mathbf{x}}_k // 真实值 - 后验估计 ek​=xk​−x^k​//真实值−后验估计
  • P k P_k Pk​其实就是 e k e_k ek​ 的协方差矩阵
    P k = E [ e k e k T ] P_k = E[e_k {e_k}^T] Pk​=E[ek​ek​T]
  • 我们的目标是让误差 e k e_k ek​ 最小,也可以理解为方差最小,也可以说是协方差矩阵 P k P_k Pk​ 对角线之和最小,也就是协方差矩阵的 迹 最小
  • 不难理解, P k P_k Pk​也是一个理想值,无法直接获得,上面描述主要是帮助理解 P k P_k Pk​ 是什么,卡尔曼滤波中使用下面公式估计 P k P_k Pk​ 的值,下面是 P k P_k Pk​ 的先验估计公式
    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−​=Fk​Pk−1​Fk⊤​+Qk​
    • Q k Q_k Qk​:估计误差 w k w_k wk​(状态空间方程中的 w k w_k wk​) 的协方差矩阵: Q k = E [ w k w k T ] Q_k = E[w_k {w_k}^T] Qk​=E[wk​wk​T]
  • 卡尔曼增益 k k k_k kk​的公式
    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+Rk​Pk−​HT​
    • R k R_k Rk​:测量误差 v k v_k vk​ 的协方差矩阵: R k = E [ v k v k T ] R_k = E[v_k {v_k}^T] Rk​=E[vk​vk​T]
    • 当 R k R_k Rk​ 比较大时,也就是分母比较大, K k K_k Kk​ 就会比较小,当 R k R_k Rk​ 足够大, K k K_k Kk​ 就会趋近于0。
    • 当 R k R_k Rk​ 比较小时, K k ≈ P k − H T H P k − H T ≈ H − 1 K_k \approx {\frac{P_{k}^{-}H^{T}}{HP_{k}^{-}H^{T}}} \approx H^{-1} Kk​≈HPk−​HTPk−​HT​≈H−1

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

  • 卡尔曼滤波器的递推流程主要分为 预测 和 校正 两步
  • 预测部分: 包括 状态的先验估计 和 误差协方差矩阵的先验估计
    x ^ k − = F x ^ k − 1 + B u k / / 状态的先验估计 \hat{\mathbf{x}}_k^-=\mathbf{F} \hat{\mathbf{x}}_{k-1}+\mathbf{B} \mathbf{u}_k //状态的先验估计 x^k−​=Fx^k−1​+Buk​//状态的先验估计
    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−​=Fk​Pk−1​Fk⊤​+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+Rk​Pk−​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−Kk​H)Pk−​//更新误差的协方差矩阵;I:单位矩阵

微信公众号

今日签到

点亮在社区的每一天
去签到