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

一. 序贯处理

先解释一下为什么要有序贯处理。还记得卡尔曼滤波的基本方程吧。当中求解增益矩阵的那个方程长这个样子：Kk=Pk/k−1HkT(HkPk/k−1HkT+Rk)−1K_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
       里面涉及了矩阵求逆的计算，要知道矩阵求逆的计算量是相当大的，观察这个方程，当ZkZ_kZk的维数很大时，矩阵求逆的计算量会极具增加，一般来讲，求逆的计算量与矩阵阶数的三次方成正比。而序贯处理就可以理解将高阶矩阵求逆转化为低阶矩阵求逆的算法。
       当然，这是有条件的。序贯处理的条件是观测量ZkZ_kZk的方差矩阵RkR_kRk是对角矩阵。而这一条件在几乎绝大多数情况下卡尔曼滤波都是满足的。
       下面可以说明一下什么是序贯处理了。首先想一下，为什么绝大多数系统的RkR_kRk是对角矩阵呢，那是因为ZkZ_kZk里的量测值之间都是不相关的。那么好了，如果我们把每个量测值单独看成一次量测，那么，对应的Kk=Pk/k−1HkT(HkPk/k−1HkT+Rk)−1K_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这个方程里的求逆就会变成除法，这样不就避免了高阶矩阵求逆了吗。就是这个道理。于是我们就有了下面这套算法。
       先定义量测向量ZkZ_kZk是个r维的向量，ZkiZ_k^iZki是它的第i个量测值，HkiH_k^iHki是HkH_kHk的第i行，RkiR_k^iRki是ZkiZ_k^iZki的方差。
Kki=Pki−1HkiT(HkiPki−1HkiT+Rki)−1K_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)−1X^ki=X^ki−1+Kki(Zki−HkiX^ki−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)Pki=(I−KkiHki)Pki−1P_k^i=(I-K_k^iH_k^i)P_k^{i-1}Pki=(I−KkiHki)Pki−1
       当然，i=1,2,...ri=1,2,...ri=1,2,...r。而且X^k0=Φk,k−1X^k−1\hat X_k^0=\Phi_{k,k-1}\hat X_{k-1}X^k0=Φk,k−1X^k−1Pk0=Pk/k−1P_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个了。数量级只差越小，舍入误差影响就会越小。这就是平方根滤波的思想来源。
       所以，我们需要对卡尔曼滤波方程种的PkP_kPk阵和Pk/k−1P_{k/k-1}Pk/k−1阵进行开平方。一个数的开平方好办，矩阵的开平方是怎么回事？定义是这样的，拿PkP_kPk为例，如果PkP_kPk的平方根是矩阵Δk\Delta_kΔk，那么PkP_kPk就可以写成Pk=ΔkΔkTP_k=\Delta_k\Delta_k^TPk=ΔkΔkT。而且，当PkP_kPk可以写成这种形式的时候，PkP_kPk一定是非负定的，就像一个数的平方一定不小于零是一个道理。
       而且，矩阵理论中有一条定理，任何矩阵，如果是正定的，那么它一定可以分解成两个三角矩阵的乘积，这两个三角矩阵互为转置。也就是说对于Pk=ΔkΔkTP_k=\Delta_k\Delta_k^TPk=ΔkΔkT，Δk\Delta_kΔk一定可以写成下三角矩阵的形式。于是，对PkP_kPk矩阵开平方的问题，就变成了对它进行三角分解的问题了。这种矩阵分解算法很多人都研究过，最常用的是乔莱斯基分解法。有机会我把这些通用算法单独写一篇文章来介绍，总之这里，大家记住：PkP_kPk可以分解成ΔkΔkT\Delta_k\Delta_k^TΔkΔkT，而Δk\Delta_kΔk是一个下三角矩阵。同理，Pk/k−1=Δk/k−1Δk/k−1TP_{k/k-1}=\Delta_{k/k-1}\Delta_{k/k-1}^TPk/k−1=Δk/k−1Δk/k−1T，这里的Δk/k−1\Delta_{k/k-1}Δk/k−1也是下三角矩阵。
       那么好了，我们的目的就是把卡尔曼滤波方程里的PkP_kPk用Δk\Delta_kΔk代替。也就是，我们不想知道PkP_kPk的递推方程了，而是想知道Δk\Delta_kΔk的递推方程是什么样的。这就要用到之前说的假设了，量测是标量。当量测是标量时：ak=Δk/k−1THkTa_k=\Delta_{k/k-1}^TH_k^Tak=Δk/k−1THkTbk=(akTak+Rk)−1b_k=(a_k^Ta_k+R_k)^{-1}bk=(akTak+Rk)−1γk=(1+bkRk)−1\gamma_k=(1+\sqrt {b_kR_k})^{-1}γk=(1+bkRk)−1Kk=bkΔk/k−1akK_k=b_k\Delta_{k/k-1}a_kKk=bkΔk/k−1akX^k=X^k/k−1+Kk(Zk−HkX^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−γkKkakT\Delta_k=\Delta_{k/k-1}-\gamma_kK_ka_k^TΔk=Δk/k−1−γkKkakT
       注意哦，由于量测ZkZ_kZk是标量，所以上面方程里的bk,γk,Rkb_k,\gamma_k,R_kbk,γk,Rk可都是标量。如果量测是矢量，就要用到序贯处理了。比如量测矢量是m维的，
       取X^k0=X^k/k−1\hat X_k^0=\hat X_{k/k-1}X^k0=X^k/k−1Δk0=Δk/k−1\Delta_k^0=\Delta_{k/k-1}Δk0=Δk/k−1
       对于m维的量测，则有如下递推：akj=(HkjΔkj−1)Ta_k^j=(H_k^j\Delta_k^{j-1})^Takj=(HkjΔkj−1)Tbkj=(akjTakj+Rkj)−1b_k^j=(a_k^{jT}a_k^j+R_k^j)^{-1}bkj=(akjTakj+Rkj)−1γkj=(1+bkjRkj)−1\gamma_k^j=(1+\sqrt {b_k^jR_k^j})^{-1}γkj=(1+bkjRkj)−1Kkj=bkjΔk/k−1j−1akjK_k^j=b_k^j\Delta_{k/k-1}^{j-1}a_k^jKkj=bkjΔk/k−1j−1akjX^kj=X^kj−1+Kkj(Zkj−HkjX^kj−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)Δkj=Δkj−1−γkjKkjakjT\Delta_k^j=\Delta_k^{j-1}-\gamma_k^jK_k^ja_k^{jT}Δkj=Δkj−1−γkjKkjakjTj=1,2,...mj=1,2,...mj=1,2,...m
       当计算到j=mj=mj=m时X^k=X^km\hat X_k=\hat X_k^mX^k=X^kmΔk=Δkm\Delta_k=\Delta_k^mΔk=Δkm
       以上方程的获得都是基于卡尔曼滤波基本方程中的Pk=(I−KkHk)Pk/k−1P_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_kX^k，就像卡尔曼滤波方程中，知道Pk/k−1P_{k/k-1}Pk/k−1就可以知道PkP_kPk一样。
       但是Δk/k−1\Delta_{k/k-1}Δk/k−1是怎么知道的呢？有同学说是把Pk/k−1P_{k/k-1}Pk/k−1进行三角分解得到的。那Pk/k−1P_{k/k-1}Pk/k−1又怎么才能知道呢。哦，根据卡尔曼滤波基本方程中的Pk/k−1=Φk,k−1Pk−1Φk,k−1T+Γk−1Qk−1Γk−1TP_{k/k-1}=\Phi_{k,k-1}P_{k-1}\Phi_{k,k-1}^T+\Gamma_{k-1}Q_{k-1}\Gamma_{k-1}^TPk/k−1=Φk,k−1Pk−1Φk,k−1T+Γk−1Qk−1Γk−1T由Pk−1P_{k-1}Pk−1计算得到，继续提问，Pk−1P_{k-1}Pk−1怎么知道的？由上一步平方根滤波中的Δk−1\Delta_{k-1}Δk−1跟自己的转置相乘得到。大家有没有发现，我们转了一圈还是要计算PkP_kPk，因为下一步平方根滤波需要用。有没有方法可以直接从Δk−1\Delta_{k-1}Δk−1计算到Δk/k−1\Delta_{k/k-1}Δk/k−1的方法呢，就像从Pk−1P_{k-1}Pk−1计算到Pk/k−1P_{k/k-1}Pk/k−1一样？有的。这就是平方根滤波的时间更新步骤。
       我们需要构造一个矩阵A。A=(Δk−1TΦk,k−1T(Γk−1Qk−112))(n+l)∗nA=\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阵变换成(D0)(n+l)∗n\begin{pmatrix}D\\0\end{pmatrix}_{(n+l)*n}(D0)(n+l)∗n       D是上三角矩阵，则Δk/k−1T=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。
       重新审视上面这些公式，我们发现，已经看不到PkP_kPk和Pk/k−1P_{k/k-1}Pk/k−1的影子了，全程都是这两个矩阵的平方根矩阵，也就是两个下三角矩阵在来回折腾。而如果PkP_kPk和Pk/k−1P_{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算法。其实本质没有区别的。

8.卡尔曼滤波之平方根滤波相关推荐

卡尔曼滤波原理（七）平方根Kalman滤波：Potter平方根滤波、SVD分解滤波、UD分解滤波、平方根信息滤波SRIKF
学习总结分享导航定位知识,希望能帮助到大家,欢迎关注我的公众号:导航定位源码阅读文章目录一.平方根滤波基本形式二.Potter平方根滤波 1.方差阵的量测更新 2.方差阵的时间更新 3.Pott ...
关于卡尔曼滤波和粒子滤波最直白的解释
卡尔曼滤波本来是控制系统课上学的,当时就没学明白,也蒙混过关了,以为以后也不用再见到它了,可惜没这么容易,后来学计算机视觉和图像处理,发现用它的地方更多了,没办法的时候只好耐心学习和理解了.一直很想把 ...
卡尔曼滤波、粒子滤波【通俗解释】
卡尔曼滤波本来是控制系统课上学的,当时就没学明白,也蒙混过关了,以为以后也不用再见到它了,可惜没这么容易,后来学计算机视觉和图像处理,发现用它的地方更多了,没办法的时候只好耐心学习和理解了.一直很想把 ...
卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波以及粒子滤波原理
所有滤波问题其实都是求感兴趣的状态的后验概率分布,只是由于针对特定条件的不同,可通过求解递推贝叶斯公式获得后验概率的解析解(KF.EKF.UKF),也可通过大数统计平均求期望的方法来获得后验概率(PF ...
卡尔曼滤波和粒子滤波
整篇转自:https://blog.csdn.net/zkl99999/article/details/46619771/ 转自http://blog.csdn.net/karen99/article ...
【多源融合】滤波融合之卡尔曼滤波与粒子滤波
多源融合之滤波对比最近在看CSDN滤波方面的知识,但是很多都是激光雷达.视觉.信号处理等方面的大佬们总结的,我也做过卡尔曼滤波和粒子滤波.博主大地测量学与测量工程专业,主要做定位方面的内容,现在从我 ...
【SLAM基础入门】贝叶斯滤波、卡尔曼滤波、粒子滤波笔记（2）
基于B站老王的贝叶斯滤波.卡尔曼滤波.粒子滤波 Bilibili 文章目录第三部分:随机过程的贝叶斯滤波BF 第四部分:卡尔曼滤波KF 第三部分:随机过程的贝叶斯滤波BF 随机过程包含一系列随机变量 ...
从卡尔曼滤波到粒子滤波很详细，很明了。。
转自http://blog.csdn.net/karen99/article/details/7771743 卡尔曼滤波本来是控制系统课上学的,当时就没学明白,也蒙混过关了,以为以后也不用再见到它了, ...
【SLAM基础入门】贝叶斯滤波、卡尔曼滤波、粒子滤波笔记（1）
贝叶斯滤波.卡尔曼滤波.粒子滤波 (https://www.bilibili.com/video/BV1HT4y1577g?spm_id_from=333.999.header_right.histo ...

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

目录

一. 序贯处理

二. 平方根滤波

8.卡尔曼滤波之平方根滤波相关推荐

最新文章

热门文章