--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/toolbox/cholp.m	Thu May 07 18:36:24 2015 +0200
@@ -0,0 +1,69 @@
+function [R, P, I] = cholp(A, piv)
+%CHOLP  Cholesky factorization with pivoting of a pos. semidefinite matrix.
+%       [R, P] = CHOLP(A) returns R and a permutation matrix P such that
+%       R'*R = P'*A*P.  Only the upper triangular part of A is used.
+%       If A is not positive definite, an error message is printed.
+%
+%       [R, P, I] = CHOLP(A) never produces an error message.
+%       If A is positive definite then I = 0 and R is the Cholesky factor.
+%       If A is not positive definite then I is positive and
+%       R is (I-1)-by-N with P'*A*P - R'*R zeros in columns 1:I-1 and
+%       rows 1:I-1.
+%       [R, I] = CHOLP(A, 0) forces P = EYE(SIZE(A)), and therefore behaves
+%       like [R, I] = CHOL(A).
+
+%       This routine is based on the LINPACK routine CCHDC.  It works
+%       for both real and complex matrices.
+%
+%       Reference:
+%       G.H. Golub and C.F. Van Loan, Matrix Computations, Second
+%       Edition, Johns Hopkins University Press, Baltimore, Maryland,
+%       1989, sec. 4.2.9.
+
+if nargin == 1, piv = 1; end
+
+n = length(A);
+pp = 1:n;
+I = 0;
+
+for k = 1:n
+
+    if piv
+       d = diag(A);
+       [big, m] = max( d(k:n) );
+       m = m+k-1;
+    else
+       big = A(k,k);  m = k;
+    end
+    if big <= 0, I = k; break, end
+
+%   Symmetric row/column permutations.
+    if m ~= k
+       A(:, [k m]) = A(:, [m k]);
+       A([k m], :) = A([m k], :);
+       pp( [k m] ) = pp( [m k] );
+    end
+
+    A(k,k) = sqrt( A(k,k) );
+    if k == n, break, end
+    A(k, k+1:n) = A(k, k+1:n) / A(k,k);
+
+%   For simplicity update the whole of the remaining submatrix (rather
+%   than just the upper triangle).
+
+    j = k+1:n;
+    A(j,j) = A(j,j) - A(k,j)'*A(k,j);
+
+end
+
+R = triu(A);
+if I > 0
+    if nargout < 3, error('Matrix must be positive definite.'), end
+    R = R(1:I-1,:);
+end
+
+if piv == 0
+   P = I;
+else
+   P = eye(n); P = P(:,pp);
+end