卡尔曼滤波

• 卡尔曼滤波

  • 便于理解的几个例子

    • 例1

      • 团战：A说张良没大，B说谁去骗大。此时该怎么选？
      • 那就看更信任两个人中的哪个（谁的权重大）
      • 但是现在只有观测值（A说张良没大）和一个估计值（B数学模型预测张良CD）
      • 此时应该怎么选？怎么选才能得到更优结果？

    • 例2

      • 火箭的尾焰温度测量，传感器只能放在内部，基于内部结果进行外部估计

    • 例3

      • 开车时如何准备定位自己的位置呢？

        1. 提供加速度信息
        2. 里程表等信息
        3. GPS信息

        但是以上三种方式都有误差，所以应该采用什么方法来解决问题？

  • 定义

  • 结合已知信息估计最优位置

  • 本质是优化估计算法

  • 小车位置更新的例子

    • 状态向量：（位置和速度） (X_{t} =egin{bmatrix} x_{t}\ar{x_{t}}end{bmatrix}) (x_{t})是位置，(ar{x}_{t})是速度

    • 小车的加速度：(U_{t}=frac{f_{t}}{m})

    • 下一时刻位置：(egin{cases}x_{t}=x_{t+1}+ar{x}_{t-1}igtriangleup t+frac{1}{2}frac{f_{t}}{m}igtriangleup t^{2}\ar{x}_{t}=ar{x}_{t-1}+frac{f_{t}}{m}igtriangleup t end{cases})

    • 矩阵表达式：(egin{bmatrix}x_{t}\ ar{x}_{t}end{bmatrix}=egin{bmatrix}1&igtriangleup t\0 &1end{bmatrix}egin{bmatrix}x_{t-1}\ ar{x}_{t-1}end{bmatrix}+egin{bmatrix}frac{igtriangleup t^{2}}{2}\igtriangleup tend{bmatrix}frac{f_{t}}{m})

    • 其中(F_{t}=egin{bmatrix}1&igtriangleup t\0 &1end{bmatrix}, B_{t}=egin{bmatrix}frac{igtriangleup t^{2}}{2} \igtriangleup tend{bmatrix})

    • 任何状态都会受外部环境的影响（例如行车过程中遇到了减速带，影响了速度），通常呈现正态分布

      • (u_{k})------->(egin{cases}x_{k}=Ax_{k-1}+Bu_{k}+w_{k} \y_{k}=Cx_{k}+v_{k}end{cases})------->(y_{k})

      • 由此产生观测值(y_{k})

      • (u_{k})------->(egin{cases}hat{x}_{k}=Ahat{x}_{k-1}+Bu_{k}+w_{k} \hat{y}_{k}=Chat{x}_{k}end{cases})------->(hat{x}_{k})

      • 此处为小车运行建立了一个模型，并由此计算出一个预测值(hat{x}_{k})

• 卡尔曼滤波本质

  • 基于估计值（predicted state estimate）和观测值（measurement）进行综合（如下一帧预测值和下一帧检测值）得到最优估计值(optimal state estimate),相当于取两个值分布的交集以得到最准确估计
  • (hat{x}_k=Ahat{x}_{k-1}+Bu_{k}+K_{k}(y_{k}-C(Ahat{x}_{k-1}+Bu_{k})))
  • (hat{sideset{}{_k^-}x}=Ahat{x}_{k-1}+Bu_{k})
  • (hat{x})为后验估计，需要用观测值进行修正
  • (hat{sideset{}{_k^-}x})为先验估计，由当前的状态估计出之后的状态
  • (K_{k})$传感器的权重值

• 两大核心模块：

  • prediction
    • 预测状态估计值及其协方差，单状态中协方差矩阵就是其方差，其即为预测状态中不确定性的度量（噪音导致）
    • 估计值(hat{sideset{}{_k^-}x}=Ahat{x}_{k-1}+Bu_{k})
    • 协方差(sideset{}{_k^-}p=AP_{k-1}A^{T}+Q)
    • Q为噪音矩阵
  • update
    • 这一步要基于预测值更新参数
    • 例如追踪每一帧的状态肯定要变
    • 预测完需要根据观测值来修正，再用修正后的状态值去估计下一帧
    • (K_{k}=frac{sideset{}{_k^-}p C^{T}}{Csideset{}{_k^-}pC^{T}+R})
    • (hat{x}_k=hat{sideset{}{_k^-}x}+K_{k}(y_{k}-Chat{sideset{}{_k^-}x}))
    • (p_{k}=(I-k_{k}C)sideset{}{_k^-}p)

• 卡尔曼增益K

  • (K_{k}=frac{sideset{}{_k^-}p C^{T}}{Csideset{}{_k^-}pC^{T}+R})
  • 目的是为了让最优估计值的方差更小
  • 其相当于一个权重项，该怎么利用估计与观测
  • 两个极端环境
    • 当观测没有噪音时
      • 增益K化简后结果：(lim_{R o 0}K_{k} =lim_{R o 0}frac{sideset{}{_k^-}p C^{T}}{Csideset{}{_k^-}pC^{T}+R}=c^{-1})
      • 相当于最优估计值等于观测值（(hat{x}_{k}=y_{k})），其中C表示转换矩阵（单状态，匀速）
    • 当状态估计没有噪音时
      • 增益K化简后结果：(lim_{sideset{}{_k^-}p o 0}K_{k} =lim_{sideset{}{_k^-}p o 0}frac{sideset{}{_k^-}p C^{T}}{Csideset{}{_k^-}pC^{T}+R}=0)
      • 相当于最优估计值等于预测值（(hat{x}_{k}=hat{sideset{}{_k^-}x})），其中C表示转换矩阵（单状态，匀速）

• 追踪问题需要考虑的状态

  • 均值（mean）：8维向量表示为(x=[c_{x},c_{y},r,h,v_{x},v_{y},v_{r},v_{h}])
    • 目标追踪框的中心点坐标((c_{x},c_{y}))，追踪框宽高比(r)，高(h)，以及各自速度变化值组成
    • 协方差矩阵：表示目标位置信息的不确定性，由8*8的矩阵表示

• 追踪问题需要分两个阶段：

  • 每一个track都要预测下一时刻的状态，并基于检测到的结果来修正（匀速，线性，常规追踪都是一帧一帧处理的）
  • (egin{bmatrix}c_{x} \c_{y} \w \h \v_{x} \v_{y} \v_{w} \v_{h} end{bmatrix}_{t+1}=egin{pmatrix} 1& 0& 0& 0& dt& 0& 0& 0\ 0& 1& 0& 0& 0& dt&0&0\ 0& 0& 1& 0& 0& 0& dt&0 \ 0& 0& 0& 1& 0& 0& 0&dt \ 0& 0& 0& 0& 1& 0 &0 & 0\ 0& 0& 0& 0& 0& 1&0 &0 \ 0& 0& 0& 0& 0& 0& 1& 0\ 0& 0& 0& 0& 0& 0& 0&1 end{pmatrix}egin{bmatrix}c_{x} \c_{y} \w \h \v_{x} \v_{y} \v_{w} \v_{h} end{bmatrix}_{t})