8.卡尔曼滤波之平方根滤波

       上一篇文章，我们提到了卡尔曼滤波的发散问题，并说明了，导致卡尔曼滤波发散的原因一般有两种，一种是由于模型建立不够准确，导致的发散，另一种是计算过程由于累计误差效应，导致增益矩阵P失去对称性或正定性而造成的发散。
       而且，上一篇文章对第一种情况提出了一些解决措施，主要思路就是认为近期的观测量更可靠一些，并逐渐忘记之间的观测对滤波的影响，也就是衰减记忆法和限定记忆法。
       在这篇文章，我们讨论针对滤波器发散另一种原因可以采取的抑制措施。这里先介绍一种，也就是平方根滤波。不过在介绍这种滤波之前，我们需要先介绍一种对滤波器的处理方法：序贯处理。

一. 序贯处理

       先解释一下为什么要有序贯处理。还记得卡尔曼滤波的基本方程吧。当中求解增益矩阵的那个方程长这个样子：$$K_k=P_{k/k-1}{H}_k^T(H_k{P}_{k/k-1}{H}_k^T+R_k{)}^{-1}$$
       里面涉及了矩阵求逆的计算，要知道矩阵求逆的计算量是相当大的，观察这个方程，当$Z_k$的维数很大时，矩阵求逆的计算量会极具增加，一般来讲，求逆的计算量与矩阵阶数的三次方成正比。而序贯处理就可以理解将高阶矩阵求逆转化为低阶矩阵求逆的算法。
       当然，这是有条件的。序贯处理的条件是观测量$Z_k$的方差矩阵$R_k$是对角矩阵。而这一条件在几乎绝大多数情况下卡尔曼滤波都是满足的。
       下面可以说明一下什么是序贯处理了。首先想一下，为什么绝大多数系统的$R_k$是对角矩阵呢，那是因为$Z_k$里的量测值之间都是不相关的。那么好了，如果我们把每个量测值单独看成一次量测，那么，对应的$K_k=P_{k/k-1}{H}_k^T(H_k{P}_{k/k-1}{H}_k^T+R_k{)}^{-1}$这个方程里的求逆就会变成除法，这样不就避免了高阶矩阵求逆了吗。就是这个道理。于是我们就有了下面这套算法。
       先定义量测向量$Z_k$是个r维的向量，$Z_k^i$是它的第i个量测值，$H_k^i$是$H_k$的第i行，$R_k^i$是$Z_k^i$的方差。
$$K_k^i=P_k^{i-1}{H}_k^{iT}(H_k^i{P}_k^{i-1}{H}_k^{iT}+R_k^i{)}^{-1}$$$${\hat{X}}_k^i={\hat{X}}_k^{i-1}+K_k^i(Z_k^i-H_k^i{\hat{X}}_k^{i-1})$$$$P_k^i=(I-K_k^i{H}_k^i)P_k^{i-1}$$
       当然，$i=1,2,...r$。而且$${\hat{X}}_k^0={\Phi }_{k,k-1}{\hat{X}}_{k-1}$$$$P_k^0=P_{k/k-1}$$
       看到了吧，这套算法当中矩阵求逆其实是除法，因为那是一个标量。当然这套算法是有代价的，代价就是把每个滤波周期又细分成了r个滤波周期。但是当量测向量的维数比较长的时候，这种处理对降低计算量的效果是十分明显的。而如果量测向量维数很短，那也就没必要这么处理了。

二. 平方根滤波

       先解释一下为什么在介绍平方根滤波之前要介绍序贯处理。这是没办法的事，因为实用的平方根滤波是假设量测量是标量形式的。所以，对于矢量形式的量测，需要利用序贯处理，对每个量测值进行处理才能实现这套算法。
       为啥平方根滤波是针对矩阵计算累计误差导致非负定有效果呢？先来理解一下，为啥卡尔曼滤波方程的方差矩阵计算过程会有累计误差吧。还记得P阵的定义吧，它是状态量X的方差矩阵，描述状态量X的精度的。而我们知道，状态量X里面可以包含各种量，有的精度高些，有的精度低些，可能对于X中的某一个元素，差个100到200都不算大，可对于X中的另外一个元素，差个0.001都算是大的。在这种情况下，描述X精度的P阵内部就可能导致元素之间的数量级差别非常大。这种矩阵有一个名称，叫做病态矩阵。通过计算机对病态矩阵进行处理，就极可能由于舍入等因素导致的误差会越来越大。
       有什么办法没有呢。其实根治的办法没有，不过有办法可以减轻病态矩阵病的程度。就是开平方。其实道理也很简单，比如10000和0.0001之间数量级差8个，但是开平方后，100和0.01的数量级就只差4个了。数量级只差越小，舍入误差影响就会越小。这就是平方根滤波的思想来源。
       所以，我们需要对卡尔曼滤波方程种的$P_k$阵和$P_{k/k-1}$阵进行开平方。一个数的开平方好办，矩阵的开平方是怎么回事？定义是这样的，拿$P_k$为例，如果$P_k$的平方根是矩阵${\Delta }_k$，那么$P_k$就可以写成$P_k={\Delta }_k{\Delta }_k^T$。而且，当$P_k$可以写成这种形式的时候，$P_k$一定是非负定的，就像一个数的平方一定不小于零是一个道理。
       而且，矩阵理论中有一条定理，任何矩阵，如果是正定的，那么它一定可以分解成两个三角矩阵的乘积，这两个三角矩阵互为转置。也就是说对于$P_k={\Delta }_k{\Delta }_k^T$，${\Delta }_k$一定可以写成下三角矩阵的形式。于是，对$P_k$矩阵开平方的问题，就变成了对它进行三角分解的问题了。这种矩阵分解算法很多人都研究过，最常用的是乔莱斯基分解法。有机会我把这些通用算法单独写一篇文章来介绍，总之这里，大家记住：$P_k$可以分解成${\Delta }_k{\Delta }_k^T$，而${\Delta }_k$是一个下三角矩阵。同理，$P_{k/k-1}={\Delta }_{k/k-1}{\Delta }_{k/k-1}^T$，这里的${\Delta }_{k/k-1}$也是下三角矩阵。
       那么好了，我们的目的就是把卡尔曼滤波方程里的$P_k$用${\Delta }_k$代替。也就是，我们不想知道$P_k$的递推方程了，而是想知道${\Delta }_k$的递推方程是什么样的。这就要用到之前说的假设了，量测是标量。当量测是标量时：$$a_k={\Delta }_{k/k-1}^T{H}_k^T$$$$b_k=(a_k^T{a}_k+R_k{)}^{-1}$$$${\gamma }_k=(1+\sqrt{b_k{R}_k}{)}^{-1}$$$$K_k=b_k{\Delta }_{k/k-1}{a}_k$$$${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})$$$${\Delta }_k={\Delta }_{k/k-1}-{\gamma }_k{K}_k{a}_k^T$$
       注意哦，由于量测$Z_k$是标量，所以上面方程里的$b_k,{\gamma }_k,R_k$可都是标量。如果量测是矢量，就要用到序贯处理了。比如量测矢量是m维的，
       取$${\hat{X}}_k^0={\hat{X}}_{k/k-1}$$$${\Delta }_k^0={\Delta }_{k/k-1}$$
       对于m维的量测，则有如下递推：$$a_k^j=(H_k^j{\Delta }_k^{j-1}{)}^T$$$$b_k^j=(a_k^{jT}{a}_k^j+R_k^j{)}^{-1}$$$${\gamma }_k^j=(1+\sqrt{b_k^j{R}_k^j}{)}^{-1}$$$$K_k^j=b_k^j{\Delta }_{k/k-1}^{j-1}{a}_k^j$$$${\hat{X}}_k^j={\hat{X}}_k^{j-1}+K_k^j(Z_k^j-H_k^j{\hat{X}}_k^{j-1})$$$${\Delta }_k^j={\Delta }_k^{j-1}-{\gamma }_k^j{K}_k^j{a}_k^{jT}$$$j=1,2,...m$
       当计算到$j=m$时$${\hat{X}}_k={\hat{X}}_k^m$$$${\Delta }_k={\Delta }_k^m$$
       以上方程的获得都是基于卡尔曼滤波基本方程中的$$P_k=(I-K_k{H}_k)P_{k/k-1}$$进行推导获得的，是卡尔曼滤波方程的量测更新部分，所以上面这些也叫做平方根滤波的量测更新步骤。
       来看这些方程，我们发现，只要知道${\Delta }_{k/k-1}$就可以计算${\Delta }_k$，进而计算出${\hat{X}}_k$，就像卡尔曼滤波方程中，知道$P_{k/k-1}$就可以知道$P_k$一样。
       但是${\Delta }_{k/k-1}$是怎么知道的呢？有同学说是把$P_{k/k-1}$进行三角分解得到的。那$P_{k/k-1}$又怎么才能知道呢。哦，根据卡尔曼滤波基本方程中的$$P_{k/k-1}={\Phi }_{k,k-1}{P}_{k-1}{\Phi }_{k,k-1}^T+{\Gamma }_{k-1}{Q}_{k-1}{\Gamma }_{k-1}^T$$由$P_{k-1}$计算得到，继续提问，$P_{k-1}$怎么知道的？由上一步平方根滤波中的${\Delta }_{k-1}$跟自己的转置相乘得到。大家有没有发现，我们转了一圈还是要计算$P_k$，因为下一步平方根滤波需要用。有没有方法可以直接从${\Delta }_{k-1}$计算到${\Delta }_{k/k-1}$的方法呢，就像从$P_{k-1}$计算到$P_{k/k-1}$一样？有的。这就是平方根滤波的时间更新步骤。
       我们需要构造一个矩阵A。$$A={\left(\begin{array}{c}{\Delta }_{k-1}^T{\Phi }_{k,k-1}^T\\ ({\Gamma }_{k-1}{Q}_{k-1}^{\frac{1}{2}})\end{array}\right)}_{(n+l)\ast n}$$
       这个A阵可不是方的哦。把这个A阵变换成上三角矩阵，也就是通过变换A阵变换成$${\left(\begin{array}{c}D\\ 0\end{array}\right)}_{(n+l)\ast n}$$       D是上三角矩阵，则${\Delta }_{k/k-1}^T=D$，常用的变换方法有两种，Householder变换法或者改进的Gram-Schmidt(MGS)法。具体方法有机会讲，总之这里只需要明白，平方根的时间更新就是通过${\Delta }_{k-1}$计算出${\Delta }_{k/k-1}$。
       重新审视上面这些公式，我们发现，已经看不到$P_k$和$P_{k/k-1}$的影子了，全程都是这两个矩阵的平方根矩阵，也就是两个下三角矩阵在来回折腾。而如果$P_k$和$P_{k/k-1}$是病的很严重的病态矩阵，那么${\Delta }_{k-1}$计算出${\Delta }_{k/k-1}$病的程度就明显减轻了很多，也就减少了由于计算舍入误差造成发散的可能了。
       上面这套平方根算法是基于下三角分解后的$\Delta$进行的。当然也可以采用同样的思路使用上三角分解进行。有些文献中描述，采用下三角分解进行的平方根滤波叫做平方根滤波的Potter算法，而采用上三角分解的平方根滤波叫做平方根滤波的Carlson算法。其实本质没有区别的。