1 files changed, 73 insertions, 0 deletions
diff --git a/williamson.m b/williamson.m
new file mode 100644
index 0000000..f969ab1
--- /dev/null
+++ b/williamson.m
@@ -0,0 +1,73 @@
+function [Db, S] = williamson(V, tol)
+    % Williamson decomposition of positive-definite (real) symmetric matrix.
+    %
+    % Args:
+    %   V (matrix): positive definite symmetric (real) matrix
+    %   tol (scalar): tolerance for symmetry check (default: 1e-11)
+    %
+    % Returns:
+    %   Db (matrix): diagonal matrix
+    %   S (matrix): symplectic matrix such that V = S' * Db * S
+
+    if nargin < 2
+        tol = 1e-11;
+    end
+
+    [n, m] = size(V);
+
+    if n ~= m
+        error('The input matrix is not square');
+    end
+
+    diffn = norm(V - V');
+    if diffn >= tol
+        error('The input matrix is not symmetric');
+    end
+
+    if mod(n, 2) ~= 0
+        error('The input matrix must have an even number of rows/columns');
+    end
+
+    n = n / 2;
+
+    omega = sympmat(n);
+
+    vals = eig(V);
+    if any(vals <= 0)
+        error('Input matrix is not positive definite');
+    end
+
+    Mm12 = real(sqrtm(inv(V)));
+    r1 = Mm12 * omega * Mm12;
+    [K, s1] = schur(r1);
+
+    X = [0 1; 1 0];
+    I = eye(2);
+    seq = cell(1, n);
+
+    % Construct a permutation matrix p
+    for i = 1:n
+        if s1(2*i-1, 2*i) > 0
+            seq{i} = I;
+        else
+            seq{i} = X;
+        end
+    end
+    p = blkdiag(seq{:});
+
+    Kt = K * p;
+    s1t = p * s1 * p;
+    s1t
+
+    dd = xpxp_to_xxpp(s1t);
+    perm_indices = xpxp_to_xxpp(1:2*n);
+    Ktt = Kt(:, perm_indices);
+
+    Db_diag = [1 ./ diag(dd(1:n, n+1:end)); 1 ./ diag(dd(1:n, n+1:end))];
+    Db = diag(Db_diag);
+
+    S = Mm12 * Ktt * sqrtm(Db);
+    S = inv(S)';
+end
+
+