8.卡尔曼滤波之平方根滤波
目录
- 一. 序贯处理
- 二. 平方根滤波
上一篇文章，我们提到了卡尔曼滤波的发散问题，并说明了，导致卡尔曼滤波发散的原因一般有两种，一种是由于模型建立不够准确，导致的发散，另一种是计算过程由于累计误差效应，导致增益矩阵P失去对称性或正定性而造成的发散。
而且，上一篇文章对第一种情况提出了一些解决措施，主要思路就是认为近期的观测量更可靠一些，并逐渐忘记之间的观测对滤波的影响，也就是衰减记忆法和限定记忆法。
在这篇文章，我们讨论针对滤波器发散另一种原因可以采取的抑制措施。这里先介绍一种，也就是平方根滤波。不过在介绍这种滤波之前，我们需要先介绍一种对滤波器的处理方法：序贯处理。
一. 序贯处理
先解释一下为什么要有序贯处理。还记得卡尔曼滤波的基本方程吧。当中求解增益矩阵的那个方程长这个样子： K k = P k / k − 1 H k T ( H k P k / k − 1 H k T + R k ) − 1 K_k=P_{k/k-1}H_k^T(H_kP_{k/k-1}H_k^T+R_k)^{-1} Kk=Pk/k−1HkT(HkPk/k−1HkT+Rk)−1
里面涉及了矩阵求逆的计算，要知道矩阵求逆的计算量是相当大的，观察这个方程，当 Z k Z_k Zk的维数很大时，矩阵求逆的计算量会极具增加，一般来讲，求逆的计算量与矩阵阶数的三次方成正比。而序贯处理就可以理解将高阶矩阵求逆转化为低阶矩阵求逆的算法。
当然，这是有条件的。序贯处理的条件是观测量 Z k Z_k Zk的方差矩阵 R k R_k Rk是对角矩阵。而这一条件在几乎绝大多数情况下卡尔曼滤波都是满足的。
下面可以说明一下什么是序贯处理了。首先想一下，为什么绝大多数系统的 R k R_k Rk是对角矩阵呢，那是因为 Z k Z_k Zk里的量测值之间都是不相关的。那么好了，如果我们把每个量测值单独看成一次量测，那么，对应的 K k = P k / k − 1 H k T ( H k P k / k − 1 H k T + R k ) − 1 K_k=P_{k/k-1}H_k^T(H_kP_{k/k-1}H_k^T+R_k)^{-1} Kk=Pk/k−1HkT(HkPk/k−1HkT+Rk)−1这个方程里的求逆就会变成除法，这样不就避免了高阶矩阵求逆了吗。就是这个道理。于是我们就有了下面这套算法。
先定义量测向量 Z k Z_k Zk是个r维的向量， Z k i Z_k^i Zki是它的第i个量测值， H k i H_k^i Hki是 H k H_k Hk的第i行， R k i R_k^i Rki是 Z k i Z_k^i Zki的方差。
K k i = P k i − 1 H k i T ( H k i P k i − 1 H k i T + R k i ) − 1 K_k^i=P_k^{i-1}H_k^{iT}(H_k^iP_k^{i-1}H_k^{iT}+R_k^i)^{-1} Kki=Pki−1HkiT(HkiPki−1HkiT+Rki)−1 X ^ k i = X ^ k i − 1 + K k i ( Z k i − H k i X ^ 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}) X^ki=X^ki−1+Kki(Zki−HkiX^ki−1) P k i = ( I − K k i H k i ) P k i − 1 P_k^i=(I-K_k^iH_k^i)P_k^{i-1} Pki=(I−KkiHki)Pki−1
当然， i = 1 , 2 , . . . r i=1,2,...r i=1,2,...r。而且 X ^ k 0 = Φ k , k − 1 X ^ k − 1 \hat X_k^0=\Phi_{k,k-1}\hat X_{k-1} X^k0=Φk,k−1X^k−1 P k 0 = P k / k − 1 P_k^0=P_{k/k-1} Pk0=Pk/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 Pk阵和 P k / k − 1 P_{k/k-1} Pk/k−1阵进行开平方。一个数的开平方好办，矩阵的开平方是怎么回事？定义是这样的，拿 P k P_k Pk为例，如果 P k P_k Pk的平方根是矩阵 Δ k \Delta_k Δk，那么 P k P_k Pk就可以写成 P k = Δ k Δ k T P_k=\Delta_k\Delta_k^T Pk=ΔkΔkT。而且，当 P k P_k Pk可以写成这种形式的时候， P k P_k Pk一定是非负定的，就像一个数的平方一定不小于零是一个道理。
而且，矩阵理论中有一条定理，任何矩阵，如果是正定的，那么它一定可以分解成两个三角矩阵的乘积，这两个三角矩阵互为转置。也就是说对于 P k = Δ k Δ k T P_k=\Delta_k\Delta_k^T Pk=ΔkΔkT， Δ k \Delta_k Δk一定可以写成下三角矩阵的形式。于是，对 P k P_k Pk矩阵开平方的问题，就变成了对它进行三角分解的问题了。这种矩阵分解算法很多人都研究过，最常用的是乔莱斯基分解法。有机会我把这些通用算法单独写一篇文章来介绍，总之这里，大家记住： P k P_k Pk可以分解成 Δ k Δ k T \Delta_k\Delta_k^T ΔkΔkT，而 Δ k \Delta_k Δk是一个下三角矩阵。同理， P k / k − 1 = Δ k / k − 1 Δ k / k − 1 T P_{k/k-1}=\Delta_{k/k-1}\Delta_{k/k-1}^T Pk/k−1=Δk/k−1Δk/k−1T，这里的 Δ k / k − 1 \Delta_{k/k-1} Δk/k−1也是下三角矩阵。
那么好了，我们的目的就是把卡尔曼滤波方程里的 P k P_k Pk用 Δ k \Delta_k Δk代替。也就是，我们不想知道 P k P_k Pk的递推方程了，而是想知道 Δ k \Delta_k Δk的递推方程是什么样的。这就要用到之前说的假设了，量测是标量。当量测是标量时： a k = Δ k / k − 1 T H k T a_k=\Delta_{k/k-1}^TH_k^T ak=Δk/k−1THkT b k = ( a k T a k + R k ) − 1 b_k=(a_k^Ta_k+R_k)^{-1} bk=(akTak+Rk)−1 γ k = ( 1 + b k R k ) − 1 \gamma_k=(1+\sqrt {b_kR_k})^{-1} γk=(1+bkRk)−1 K k = b k Δ k / k − 1 a k K_k=b_k\Delta_{k/k-1}a_k Kk=bkΔk/k−1ak X ^ k = X ^ k / k − 1 + K k ( Z k − H k X ^ k / k − 1 ) \hat X_k=\hat X_{k/k-1}+K_k(Z_k-H_k\hat X_{k/k-1}) X^k=X^k/k−1+Kk(Zk−HkX^k/k−1) Δ k = Δ k / k − 1 − γ k K k a k T \Delta_k=\Delta_{k/k-1}-\gamma_kK_ka_k^T Δk=Δk/k−1−γkKkakT
注意哦，由于量测 Z k Z_k Zk是标量，所以上面方程里的 b k , γ k , R k b_k,\gamma_k,R_k bk,γk,Rk可都是标量。如果量测是矢量，就要用到序贯处理了。比如量测矢量是m维的，
取 X ^ k 0 = X ^ k / k − 1 \hat X_k^0=\hat X_{k/k-1} X^k0=X^k/k−1 Δ k 0 = Δ k / k − 1 \Delta_k^0=\Delta_{k/k-1} Δk0=Δk/k−1
对于m维的量测，则有如下递推： a k j = ( H k j Δ k j − 1 ) T a_k^j=(H_k^j\Delta_k^{j-1})^T akj=(HkjΔkj−1)T b k j = ( a k j T a k j + R k j ) − 1 b_k^j=(a_k^{jT}a_k^j+R_k^j)^{-1} bkj=(akjTakj+Rkj)−1 γ k j = ( 1 + b k j R k j ) − 1 \gamma_k^j=(1+\sqrt {b_k^jR_k^j})^{-1} γkj=(1+bkjRkj)−1 K k j = b k j Δ k / k − 1 j − 1 a k j K_k^j=b_k^j\Delta_{k/k-1}^{j-1}a_k^j Kkj=bkjΔk/k−1j−1akj X ^ k j = X ^ k j − 1 + K k j ( Z k j − H k j X ^ k j − 1 ) \hat X_k^j=\hat X_k^{j-1}+K_k^j(Z_k^j-H_k^j\hat X_k^{j-1}) X^kj=X^kj−1+Kkj(Zkj−HkjX^kj−1) Δ k j = Δ k j − 1 − γ k j K k j a k j T \Delta_k^j=\Delta_k^{j-1}-\gamma_k^jK_k^ja_k^{jT} Δkj=Δkj−1−γkjKkjakjT j = 1 , 2 , . . . m j=1,2,...m j=1,2,...m
当计算到 j = m j=m j=m时 X ^ k = X ^ k m \hat X_k=\hat X_k^m X^k=X^km Δ k = Δ k m \Delta_k=\Delta_k^m Δk=Δkm
以上方程的获得都是基于卡尔曼滤波基本方程中的 P k = ( I − K k H k ) P k / k − 1 P_k=(I-K_kH_k)P_{k/k-1} Pk=(I−KkHk)Pk/k−1进行推导获得的，是卡尔曼滤波方程的量测更新部分，所以上面这些也叫做平方根滤波的量测更新步骤。
来看这些方程，我们发现，只要知道 Δ k / k − 1 \Delta_{k/k-1} Δk/k−1就可以计算 Δ k \Delta_k Δk，进而计算出 X ^ k \hat X_k X^k，就像卡尔曼滤波方程中，知道 P k / k − 1 P_{k/k-1} Pk/k−1就可以知道 P k P_k Pk一样。
但是 Δ k / k − 1 \Delta_{k/k-1} Δk/k−1是怎么知道的呢？有同学说是把 P k / k − 1 P_{k/k-1} Pk/k−1进行三角分解得到的。那 P k / k − 1 P_{k/k-1} Pk/k−1又怎么才能知道呢。哦，根据卡尔曼滤波基本方程中的 P k / k − 1 = Φ k , k − 1 P k − 1 Φ k , k − 1 T + Γ k − 1 Q k − 1 Γ k − 1 T 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 Pk/k−1=Φk,k−1Pk−1Φk,k−1T+Γk−1Qk−1Γk−1T由 P k − 1 P_{k-1} Pk−1计算得到，继续提问， P k − 1 P_{k-1} Pk−1怎么知道的？由上一步平方根滤波中的 Δ k − 1 \Delta_{k-1} Δk−1跟自己的转置相乘得到。大家有没有发现，我们转了一圈还是要计算 P k P_k Pk，因为下一步平方根滤波需要用。有没有方法可以直接从 Δ k − 1 \Delta_{k-1} Δk−1计算到 Δ k / k − 1 \Delta_{k/k-1} Δk/k−1的方法呢，就像从 P k − 1 P_{k-1} Pk−1计算到 P k / k − 1 P_{k/k-1} Pk/k−1一样？有的。这就是平方根滤波的时间更新步骤。
我们需要构造一个矩阵A。 A = ( Δ k − 1 T Φ k , k − 1 T ( Γ k − 1 Q k − 1 1 2 ) ) ( n + l ) ∗ n A=\begin {pmatrix}\Delta_{k-1}^T\Phi_{k,k-1}^T\\(\Gamma_{k-1}Q_{k-1}^{\frac{1}2})\end {pmatrix}_{(n+l)*n} A=(Δk−1TΦk,k−1T(Γk−1Qk−121))(n+l)∗n
这个A阵可不是方的哦。把这个A阵变换成上三角矩阵，也就是通过变换A阵变换成 ( D 0 ) ( n + l ) ∗ n \begin{pmatrix}D\\0\end{pmatrix}_{(n+l)*n} (D0)(n+l)∗n D是上三角矩阵，则 Δ k / k − 1 T = D \Delta_{k/k-1}^T=D Δk/k−1T=D，常用的变换方法有两种，Householder变换法或者改进的Gram-Schmidt(MGS)法。具体方法有机会讲，总之这里只需要明白，平方根的时间更新就是通过 Δ k − 1 \Delta_{k-1} Δk−1计算出 Δ k / k − 1 \Delta_{k/k-1} Δk/k−1。
重新审视上面这些公式，我们发现，已经看不到 P k P_k Pk和 P k / k − 1 P_{k/k-1} Pk/k−1的影子了，全程都是这两个矩阵的平方根矩阵，也就是两个下三角矩阵在来回折腾。而如果 P k P_k Pk和 P k / k − 1 P_{k/k-1} Pk/k−1是病的很严重的病态矩阵，那么 Δ k − 1 \Delta_{k-1} Δk−1计算出 Δ k / k − 1 \Delta_{k/k-1} Δk/k−1病的程度就明显减轻了很多，也就减少了由于计算舍入误差造成发散的可能了。
上面这套平方根算法是基于下三角分解后的 Δ \Delta Δ进行的。当然也可以采用同样的思路使用上三角分解进行。有些文献中描述，采用下三角分解进行的平方根滤波叫做平方根滤波的Potter算法，而采用上三角分解的平方根滤波叫做平方根滤波的Carlson算法。其实本质没有区别的。