[Algorithm] Cholesky 正定矩陣分解

最近在搞 UKF，需要求解 P 矩陣的開跟號，所以找了一篇文章線代啟示錄 - Cholesky 分解參考，賺寫一個 Cholesky 矩陣分解的程式，暫時先用 MATLAB 實現，方便之後再移植到單晶片上做處理。

簡單講一下原理，假定有一個正定矩陣 A，我們希望他可以被分解成一個 P 矩陣乘上自己的轉置，而這個 P 矩陣是一個下三角矩陣，也就是我們想要求得的分解結果，如下圖所示，

如果 A 矩陣是一個正定矩陣，則可以表示成下面形式，L 和 U 分別為上三角和下三角矩陣（U 和 L 打反了，將就下），X 為對角矩陣，並且 A 矩陣的轉置會等於 A 矩陣，

藉由 A 矩陣的轉置會等於 A 矩陣的條件，可以得到下列關係，並且帶回前式整理，

可以得到 A 矩陣和 P 矩陣的關係，這裡以 3x3 的矩陣做展開，再過歸納，

上式反推得到下列關係，

透過上述關係可歸納出在 n x n 的正定矩陣分解出來的 P 矩陣如下。

最後透過 MATLAB 來做驗證，MATLAB 有一個 Chol() 的指令，用來做 cholesky 分解的，下面自己撰寫好 cholesky 分解後，透過 A = P P' 來驗證結果，當然也可以透過 Chol() 來判斷是否正確，程式沒有很完善，在正定矩陣的判斷等等沒有處理，如果要實際應用，建議需要另外加入一些條件處理特殊狀況。

cholesky 分解, cholesky.m

function sqrt_matrix = cholesky( pdMatrix )

    [n, m] = size(pdMatrix);
 sqrt_matrix = zeros(n);
 for j = 1 : n
        for i = j : n
            if i == j
                tempSum = 0;
                for k = 1 : i - 1
                    tempSum = tempSum + sqrt_matrix(i, k)^2;
                end
                sqrt_matrix(i, j) = sqrt(pdMatrix(i, j) - tempSum);
            else
                tempSum = 0;
                for k = 1 : j - 1
                    tempSum = tempSum + sqrt_matrix(i, k) * sqrt_matrix(j, k);
                end
                sqrt_matrix(i, j) = (pdMatrix(i, j) - tempSum) / sqrt_matrix(j, j);
            end
        end
    end

end

cholesky 驗證, testCholesky.m

n = 10;
X = diag(rand(n, 1));
U = orth(rand(n, n));

pdMatrix = U' * X * U;

P_U = chol(pdMatrix, 'upper');
P_L = chol(pdMatrix, 'lower');

P   = cholesky(pdMatrix);

check = roundn(pdMatrix, -10) == roundn(P * P', -10);
if sum(sum(check)) == n * n
    sprintf('Equal')
else
    sprintf('Not Equal')
end

Github → https://github.com/Hom-Wang/MATLAB/tree/master/cholesky