卡尔曼滤波

• 卡尔曼滤波

  • 便于理解的几个例子

    • 例1

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

    • 例2

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

    • 例3

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

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

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

  • 定义

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

  • 本质是优化估计算法

  • 小车位置更新的例子

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

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

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

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

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

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

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

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

      • \(u_{k}\)------->\(\begin{cases}\hat{x}_{k}=A\hat{x}_{k-1}+Bu_{k}+w_{k} \\\hat{y}_{k}=C\hat{x}_{k}\end{cases}\)------->\(\hat{x}_{k}\)

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

• 卡尔曼滤波本质

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

• 两大核心模块：

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

• 卡尔曼增益K

  • \(K_{k}=\frac{\sideset{}{_k^-}p C^{T}}{C\sideset{}{_k^-}pC^{T}+R}\)
  • 目的是为了让最优估计值的方差更小
  • 其相当于一个权重项，该怎么利用估计与观测
  • 两个极端环境
    • 当观测没有噪音时
      • 增益K化简后结果：\(\lim_{R \to 0}K_{k} =\lim_{R \to 0}\frac{\sideset{}{_k^-}p C^{T}}{C\sideset{}{_k^-}pC^{T}+R}=c^{-1}\)
      • 相当于最优估计值等于观测值（\(\hat{x}_{k}=y_{k}\)），其中C表示转换矩阵（单状态，匀速）
    • 当状态估计没有噪音时
      • 增益K化简后结果：\(\lim_{\sideset{}{_k^-}p \to 0}K_{k} =\lim_{\sideset{}{_k^-}p \to 0}\frac{\sideset{}{_k^-}p C^{T}}{C\sideset{}{_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都要预测下一时刻的状态，并基于检测到的结果来修正（匀速，线性，常规追踪都是一帧一帧处理的）
  • \(\begin{bmatrix}c_{x} \\c_{y} \\w \\h \\v_{x} \\v_{y} \\v_{w} \\v_{h} \end{bmatrix}_{t+1}=\begin{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}\begin{bmatrix}c_{x} \\c_{y} \\w \\h \\v_{x} \\v_{y} \\v_{w} \\v_{h} \end{bmatrix}_{t}\)