java实现一阶卡尔曼滤波算法

目录

• 一、由来
• 二、滤波方程

$P_k$阵是病态阵的情况，利用矩阵三角分解法进行平方根滤波的技术足够解决这一问题了。可作为矩阵分解的另一项技术$UDU^T$分解法，跟三角分解法有异曲同工之妙。那么相对应的也就必然产生一种新的滤波算法，也就是$UDU^T$分解滤波算法。所以，为了知识的系统完整性，我觉得还是把这方面的内容写出来吧。

一、由来

$P_k$阵是病态阵导致计算误差增大，滤波发散的问题。
       还记得平方根滤波那篇文章里，是将$P_k$和$P_{k/k-1}$进行三角分解，分解成$\Delta_k\Delta_k^T$和$\Delta_{k/k-1}\Delta_{k/k-1}^T$，在让$\Delta_k$和$\Delta_{k/k-1}$在滤波方程中代替$P_k$和$P_{k/k-1}$的作用，从而获得平方根滤波的算法。
       其实对于正定矩阵，还有另一种矩阵分解方法可以达到类似的目的，就是$UDU^T$分解法。简单来说，就是一个正定的矩阵可以分解成$UDU^T$这样的形式，其中U是对角线上都是1的上三角矩阵，D是对角阵（除了对角线，其他元素都是0）。
所以，我们就要把$P_k$和$P_{k/k-1}$分解成$UDU^T$的形式：
$P_k=U_kD_kU_k^T$$P_{k/k-1}=U_{k/k-1}D_{k/k-1}U_{k/k-1}^T$
       为了便于描述，我们把这些符号重新定义一下：
$\hat P=P_k，\widetilde P=P_{k/k-1}$$\hat U=U_k，\widetilde U=U_{k/k-1}$$\hat D=D_k，\widetilde D=D_{k/k-1}$
       跟平方根滤波类似，$UDU^T$滤波就是把卡尔曼滤波基本方程里的$\hat P$和$\widetilde P$之间的迭代更新换成$\hat U，\hat D$ 和$\widetilde U，\widetilde D$之间的迭代更新。

二、滤波方程

       以下过程仍然限定量测为标量，如果量测是矢量，还是要用序贯处理的。
       整个过程分两个步骤，第一步叫做量测更新，也就是卡尔曼基本方程里通过当前步骤获得$P_{k/k-1}$去求$P_k$的更新步骤。在$UDU^T$滤波过程中就是根据$\widetilde U、\widetilde D$求$\hat U、\hat D$。推导过程相当麻烦。还是不写了，写了也估计没人看的，直接上结论吧。$f=\widetilde U^TH^T$$g=\widetilde Df$或者写成$g_i=\widetilde D_if_i\qquad i=1,2,...,n$$\alpha_j=R+\sum^j_{i=1}f_ig_i$或者写成递推形式$\alpha_j=\alpha_{j-1}+f_jg_j\qquad j=1,2,...,n\qquad \alpha_0=R，$$K_k=\frac{\widetilde Ug}{\alpha_n}$$\hat X_k=\hat X_{k/k-1}+K_k(Z_k-H_k\hat X_{k/k-1})$$\lambda_j=-\frac{f_j}{\alpha_{j-1}}\qquad j=1,2,...,n$$\hat D_j=\widetilde D_j\frac{\alpha_{j-1}}{\alpha_j}\qquad j=1,2,...,n$$\hat U_{ij}=\widetilde U_{ij}+\lambda_j(g_i+\sum_{k=i+1}^{j-1}\widetilde U_{ik}g_k)\qquad j=2,3,...,n\qquad i=1,2,...,j-1$$\hat U_{ii}=1\qquad i=1,2,...,n$
       如此操作，就完成了从$\widetilde U、\widetilde D$到$\hat U、\hat D$的计算。看上去好复杂。可你要知道，由于量测是标量，上面这些式子有一部分是标量计算而不是矩阵计算。比如$\alpha$不管下标是几，都是一个标量，f和g都是一个n维的列向量，所以$f_i$和$g_i$表示列向量里的元素。H是一个n维的行向量，$K_k$也是一个n维的列向量。
       一定要注意，D是对角线矩阵，也就是除了对角线上有数其他都是0，所以$D_i$就表示对角线上的第i个元素。而U是一个对角线上为1的上三角矩阵，它的下三角部分都是0。
       以上即为量测更新，接下来需要进行时间更新，在卡尔曼基本方程里，时间更新就是根据上一步获得的$P_{k-1}$来求这一步的$P_{k/k-1}$。到了$UDU^T$滤波里，就是已知$\hat U和\hat D$来求$\widetilde U和\widetilde D$。
       还是直接写公式吧。先定义两个矩阵：$D=diag[\hat D\quad Q]$$W^{(0)}=[\Phi \hat U\quad \Gamma]=\begin{pmatrix}W_1^{(0)}\\W_2^{(0)}\\\vdots\\W_n^{(0)}\end{pmatrix}$
然后就是：$\widetilde D_j=W_j^{(n-j)}D[W_j^{(n-j)}]^T\qquad j=n,n-1,...,2$$\widetilde U_{i,j}=\frac{W_i^{(n-j)}D[W_j^{(n-j)}]^T}{\widetilde D_j}\qquad j=n,n-1,...,2;\quad i=1,2,...,j-1$$W_i^{(n-j+1)}=W_i^{(n-j)}-\widetilde U_{i,j}W_j^{(n-j)}\qquad j=n,n-1,...,2;\quad i=1,2,...,j-1$$\widetilde D_1=W_1^{(n-1)}D[W_1^{(n-1)}]^T$
       这里面符号的定义我就不用讲了吧。另外要注意，这里面的$\widetilde U，\widetilde D$和$\hat U，\hat D$跟量测更新那里有些区别，应该理解为上一步到这一步的时间更新，也就是$\hat U=U_{k-1}，\widetilde U=U_{k/k-1}$$\hat D=D_{k-1}，\widetilde D=D_{k/k-1}$
       好了，交叉进行量测更新和时间更新就可以实现$UDU^T$滤波了。还有个问题，就是$\hat U，\hat D$的初值问题。很简单，还记得$P_k$的初值吗，是一个对角线矩阵，那么$\hat U$的初值就是单位矩阵I，$\hat D$的初值则就是$P_0$
       到这里，所谓的$UDU^T$滤波就是这样了，由于利用了$P$阵$UDU^T$分解形式，所以如果P阵是病态阵的时候，分解后的U阵和D阵病的程度就没那么重了。不过，说实话，这么多年，我还没见过哪个工程应用场景用到过这个滤波器。甚至连平方根滤波都很少遇到。所以，还是重点关注基本的卡尔曼滤波吧。