Unscented Kalman Filter(无迹卡尔曼滤波）

最近读了一篇文献，里面用到了无迹卡尔曼滤波(Unscented Kalman Filter)。这里写一下我对这种方法的理解。卡尔曼滤波的理解部分可以参考我之前的文章。

我的一点点理解

无迹卡尔曼滤波是对卡尔曼滤波的一种改进。这种改进主要是针对非线性的信号。因为在卡尔曼滤波中，预测模型以及测量空间对应的转换矩阵都是都是线性转换。但是在面对非线性信号时，会出现无法拟合的情况。所以就有了无迹卡尔曼滤波。这种方法的主要改进在于，不再用线性的模型去计算预测模型以及转换矩阵，而是通过采样和计算均值方法的方式，去估计样本的方差和均值。

计算过程

无迹卡尔曼滤波的计算方式和卡尔曼滤波比较类似，只是讲线性转换模型换成了采样的方式。具体的原理推导比较复杂，所以这里只写一下无迹卡尔曼滤波的计算过程：

无迹卡尔曼的计算步骤和卡尔曼滤波基本是一致的，只是对其中的一些步骤进行了修改，首先，我们看一下Kalman Filter的计算过程：

1. 建立编码模型和转换模型， 假设观测变量是$z$， 测量变量是$x$， 那么首先我们假设：

   1. 当前时刻的测量变量是可以根据上一时刻的测量变量估计：
      $$x_t=F{x}_{t-1}+w_t,(w_t-N(0,Q))$$

2. 当前时刻的观测变量可以根据测量变量估计：
   $$z_t=H{x}_t+r_t,(r_t-N(0,R))$$

3. 根据以上的编码模型和转换模型，Kalman Filter的计算流程如下：

   1. 首先，根据已知的模型，以及上一时刻的卡尔曼估计值，计算当前时刻的模型预测值

   $$x_t^{\prime }=F{x}_{t-1}$$

   2. 根据当前的模型预测值，计算对应的协方差

   $$P(x_t|x_t^{\prime })=FP(x_t|X_t)F^T$$

   3. 根据当前的协方差和测量空间的转换矩阵，计算当前时刻的卡尔曼增益

   $$K_t=P(x_t|x_t^{\prime })H^T(HP(x_t|x_t^{\prime })H^T+R{)}^{-1}$$

   4. 根据卡尔曼增益和测量值，计算当前时刻的卡尔曼估计值

   $$x_t=x_t^{\prime }+K_t(z_t-H{x}_t^{\prime })$$

   5. 计算了当前时刻的卡尔曼估计值之后，还需要计算当前的估计值和真实值的协方差矩阵，方便下一次计算

   $$P(x_t|X_t)=(I-H{K}_t)P(x_t|x_t^{\prime })$$

   作为线性的解码器，Kalman Filter确实能找到观测变量和测量变量之间的关系，并用观测变量去纠正当前测量变量中的误差。但是涉及到非线性关系的时候，Kalman Filter的线性假设就不成立了。这时有两种优化的方法：

   1. 如果已知这种非线性关系的公式，例如加速度和位置的关系等，那么可以把上述转换模型和观测模型换成已知的非线性模型，增加解码准确率。这种方法就是扩展卡尔曼滤波(Extend Kalman Filter)。这种方法的优点在于拟合更加准确，但是缺点也很明显。首先是计算量增加，如果非线性拟合涉及很复杂的模型，那么计算量比Kalman Filter增加很多。然后是非线性模型，并不是任何时候，这种模型都是已知的，如果不是已知的，那就需要进行非线性拟合，找到最合适的拟合模型，例如指数模型，高阶模型等，再次增加计算量。
   2. 如果不知道这种非线性关系的公式，那么我们可以进行非线性拟合或者直接假设一个公式。但是我们观察Kalman Filter的计算过程，整个估计过程中，用到了当前时刻的值，以及协方差。而这两个量，我们是能通过采样的方式得到的，即，可以不需要直接计算非线性模型的协方差矩阵，直接通过采样估计，类似蒙特卡洛的方法。但是采样的计算量会更大，因为需要大样本才能得到准确的估计。目前有另外一种办法，能够用很少的采样点(几个)就得到准确的估计，这种方法是无迹变换(Unscented Transform)，结合到Kalman Filter中，就是无迹卡尔曼滤波(Unscented Kalman Filter)

   所以无迹卡尔曼滤波的主要流程如下：

   1. 计算转换模型和编码模型

      1. 建立转换模型，可以是非线性也可以是线性，这里用线性模型：
         $$x_t=F{x}_{t-1}+w_t,(w_t-N(0,Q))$$

      2. 建立编码模型，也可以是线性或非线性模型：

$$z_t=H{x}_t+r_t,(r_t-N(0,R))$$

2. 根据上述模型和训练集数据，用最小二乘法或其他的拟合方法，得到模型参数，然后开始无迹卡尔曼的预测和更新阶段

   1. 根据模型预测$x_t$
      $$x_t^{\prime }=F{x}_{t-1}$$

   2. 预测$x_t$的协方差
      $$P(x_t|x_t^{\prime })=FP(x_t|X_t)F^T+Q$$

   3. 用采样点估计当前协方差矩阵，先采样$2d+1$个点，并保证中心点的值为$x_t^{\prime }$
      $$X_0=x_t^{\prime }$$

      $$X_i=x_t^{\prime }+(\sqrt{(d+k)P(x_t|x_t^{\prime })}{)}_i,i=1,...,d$$

      $$X_i=x_t^{\prime }-(\sqrt{(d+k)P(x_t|x_t^{\prime })}{)}_i,i=d+1,...,2d$$

   4. 计算采样点的权重值
      $$w_0=\frac{k}{d+k},w_i=\frac{1}{2d+k},i=1,...2d$$

   5. 根据转换矩阵，采样点，计算观测值和测量值的关系
      $$Z_i=h(X_i),i=0,...2d$$

      $$z_t=\sum _{i=0,...2d}{w}_i{Z}_i$$

   6. 根据采样点估计的观测值，计算观测值$z$的方差，以及观测值$z$和测量值$x$的协方差
      $$P_{zz,t}=w_0(Z_0-z_t)(Z_0-z_t{)}^T+(\sum _{i=1,...,2d}{w}_i(Z_i-Z_0)(Z_i-z_0{)}^T)+R$$

      $$P_{xz,t}=w_0(Z_0-z_t)(Z_0-z_t{)}^T+(\sum _{i=1,...,2d}{w}_i(X_i-X_0)(Z_i-Z0{)}^T)$$

   7. 根据计算的协方差，可以计算Kalman增益
      $$K=P_{xz,t}{P}_{zz,t}^{-1}$$

   8. 用Kalman增益计算最有估计值
      $$x_t=x_t^{\prime }+K_t(h(x_t^{\prime })-z_t)$$

      $$P(x_t|X_t)=P(x_t|x_t^{\prime })-P_{xz,t}(P_{zz,t}^{-1}{)}^T{P}_{xz,t}^T$$

      以上就是无迹卡尔曼滤波的主要步骤。是对Kalman Filter的一种改进，通过引入UT变换，用采样的方式得到非线性模型的均值和方差。是一种很巧妙的计算方法。
      代码链接
      Unscented Kalman Filter