UKF-协方差矩阵分解

UKF-协方差矩阵分解

在SUT变化中，需要对协方差矩阵进行分解。而matlab自带的chol函数只能分解正定矩阵。为保证求解不被中断，并能提示协方差矩阵的正定、半正定以及负定情况，对常规的Cholesky简单修改。

---

一、Cholesky分解原理

$$\begin{array}{rl}& =\left[\begin{array}{ccc}{a}_{11}& a_{21}& a_{31}\\ a_{21}& a_{22}& a_{23}\\ a_{31}& a_{32}& a_{33}\end{array}\right]\\ & =\left(\begin{array}{ccc}{l}_{11}& 0& 0\\ l_{21}& l_{22}& 0\\ l_{31}& l_{32}& l_{33}\end{array}\right)\left(\begin{array}{ccc}{l}_{11}& l_{21}& l_{31}\\ 0& l_{22}& l_{32}\\ 0& 0& l_{33}\end{array}\right)=L{L}^T\\ & =\left(\begin{array}{ccc}{l}_{11}^2& l_{21}{l}_{11}& l_{31}{l}_{11}\\ l_{21}{l}_{11}& l_{21}^2+l_{22}^2& l_{31}{l}_{21}+l_{32}{l}_{22}\\ l_{31}{l}_{11}& l_{31}{l}_{21}+l_{32}{l}_{22}& l_{31}^2+l_{32}^2+l_{33}^2\end{array}\right)\end{array}$$
矩阵 $L$ 对角线上元素计算有如下规律：
$$\begin{array}{rl}{l}_{11}& =\sqrt{a_{11}}\\ l_{22}& =\sqrt{a_{22}-l_{21}^2}\\ l_{33}& =\sqrt{a_{33}-(l_{31}^2+l_{32}^2)}\end{array}$$
一般规律：
$$l_{jj}=\sqrt{a_{jj}-\sum _{k=1}^{j-1}{l}_{jk}^2}$$
对于非对角线元素 $l_{ij}$，其中 $i>j$ 计算规律如下：
$$\begin{array}{rl}{l}_{21}& =\frac{1}{l_{11}}{a}_{21}\\ l_{31}& =\frac{1}{l_{11}}{a}_{31}\\ l_{32}& =\frac{1}{l_{22}}(a_{22}-l_{31}{l}_{21})\end{array}$$
一般规律：
$$l_{ij}=\frac{1}{l_{jj}}\left(a_{ij}-\sum _{k=1}^{j-1}{l}_{ik}{l}_{jk}\right)$$

二、代码如下（示例）：

function [L,flag] = utchol(M)
    L = zeros(size(M));
    flag = 1;
    for i = 1:size(M,1)
        for j = 1:i
            s = M(i,j);
            for k = 1:j-1
                s = s - L(i,k)*L(j,k);
            end
            if j < i
                if L(j,j) > eps
                    L(i,j) = s / L(j,j);
                else
                    L(i,j) = 0;
                end
            else
                if (s < - eps)
                    s = 0;
                    flag = -1;
                elseif (s < eps)
                    s = 0;
                    flag = min(0,flag);
                end
                L(j,j) = sqrt(s);
            end
        end
    end
        if (nargout < 2) && (flag < 0)
            warning('Matrix is negative flaginite')
        end   
end

三、参考博客

参考博客
[1]: https://baimafujinji.blog.csdn.net/article/details/6512460
[2]: https://blog.csdn.net/billbliss/article/details/78559387