// $Id: CovMatrix6Invert.cc,v 1.6 2002/09/19 16:45:18 sachs Exp $ // // Invert a positive definite symmetric 6x6 matrix // David Sachs, Fermilab - April 2002 #include "CovMatrices/CovMatrix6.h" #include "CovMatrixElement.h" // Invert a positive definite symmetric 6x6 matrix - using L*D*L^T decomposition // // Base on algorithm in Golub & Van Loan "Matrix Computations" 3rd Ed. pp 138-139 // The exact formulas used were determined by hand. // bool CovMatrices::CovMatrix6::invert(void) { // Invert by // // a) decomposing M = G*D*G^T with G unit lower triangular and D positive diagonal // (if M is not positive definite this will fail, leaving this unchanged) // b) inverting G to form H // c) multiplying H^T * D^-1 * H to get M^-1. // // If the matrix is pos. def. it is inverted and 1 is returned. // If the matrix is not pos. def. it remains unaltered and 0 is returned. // NOTE: The LDl^T algoritm should always work for posive definite symmetric matrices. // It will also work for many, but not all, non-positive-definite symmetric matrice, // but may be unstable. In particular it will fail if M[0,0] is zero, which // is quite possible for a nonsingular symmetric matrix. // double h10; // below-diagonal elements of H // double h20, h21; // will be defined later // double h30, h31, h32; // double h40, h41, h42, h43; // double h50, h51, h52, h53, h54; double d00, d11, d22, d33, d44, d55; // 1/diagonal elements of D = // diagonal elements of D^-1 double g10; // below-diagonal elements of G double g20, g21; double g30, g31, g32; double g40, g41, g42, g43; double g50, g51, g52, g53, g54; double G10; // UNSCALED below-diagonal elements of G double G20, G21; double G30, G31, G32; double G40, G41, G42, G43; double G50, G51, G52, G53, G54; // Form G and D -- compute diagonal members of D^-1 directly //------- // Scale first column by 1/M00 d00 = M[M00]; if (d00 <= 0.) return false; d00 = 1.0 / d00; G10 = M[M10]; g10 = M[M10] * d00; G20 = M[M20]; g20 = M[M20] * d00; G30 = M[M30]; g30 = M[M30] * d00; G40 = M[M40]; g40 = M[M40] * d00; G50 = M[M50]; g50 = M[M50] * d00; // Form d11 d11 = M[M11] - (G10 * g10); if (d11 <= 0.) return false; d11 = 1.0 / d11; // Subtract inter-column column dot products from rest of column 1 and // scale to get column 1 of G G21 = (M[M21] - (G10 * g20)); g21 = G21 * d11; G31 = (M[M31] - (G10 * g30)); g31 = G31 * d11; G41 = (M[M41] - (G10 * g40)); g41 = G41 * d11; G51 = (M[M51] - (G10 * g50)); g51 = G51 * d11; // Form d22 d22 = M[M22] - (G20 * g20) - (G21 * g21); if (d22 <= 0.) return false; d22 = 1.0 / d22; // Subtract inter-column column dot products from rest of column 2 and // scale to get column 2 of G G32 = (M[M32] - (G20 * g30) - (G21 * g31)); g32 = G32 * d22; G42 = (M[M42] - (G20 * g40) - (G21 * g41)); g42 = G42 * d22; G52 = (M[M52] - (G20 * g50) - (G21 * g51)); g52 = G52 * d22; // Form d33 d33 = M[M33] - (G30 * g30) - (G31 * g31) - (G32 * g32); if (d33 <= 0.) return false; d33 = 1.0 / d33; // Subtract inter-column column dot products from rest of column 3 and // scale to get column 3 of G G43 = (M[M43] - (G30 * g40) - (G31 * g41) - (G32 * g42)); g43 = G43 * d33; G53 = (M[M53] - (G30 * g50) - (G31 * g51) - (G32 * g52)); g53 = G53 * d33; // Form d44 - if this is possible inversion succeeds d44 = M[M44] - (G40 * g40) - (G41 * g41) - (G42 * g42) - (G43 * g43); if (d44 <= 0.) return false; d44 = 1.0 / d44; // Subtract inter-column column dot product from A54 and scale to get G54 G54 = (M[M54] - (G40 * g50) - (G41 * g51) - (G42 * g52) - (G43 * g53)); g54 = G54 * d44; // Finally form d55 - if this is possible inversion succeeds d55 = M[M55] - (G50 * g50) - (G51 * g51) - (G52 * g52) - (G53 * g53) - (G54 * g54); if (d55 <= 0.) return false; d55 = 1.0 / d55; // Form H = 1/G -- Diagonal elements of G and H are 1.0 //------------- // Because the unscaled matrix elements of G are no longer needed // define the elements of H to occupy the same memory // Because of their simple forms, h10, h21, h32 h43 and h54 are defined by their formulas #define h10 (-g10) #define h20 G20 #define h30 G30 #define h40 G40 #define h50 G50 #define h21 (-g21) #define h31 G31 #define h41 G41 #define h51 G51 #define h32 (-g32) #define h42 G42 #define h52 G52 #define h43 (-g43) #define h53 G53 #define h54 (-g54) // h10 = -g10; // h21 = -g21; h20 = g10 * g21 - g20; // h32 = -g32; h31 = g21 * g32 - g31; h30 = g32 * g20 - h31 * g10 - g30; // h43 = -g43; h42 = g32 * g43 - g42; h41 = g31 * g43 - g21 * h42 - g41; h40 = g30 * g43 - g20 * h42 - g10 * h41 - g40; // h54 = -g54; h53 = g43 * g54 - g53; h52 = g42 * g54 - g32 * h53 - g52; h51 = g41 * g54 - g31 * h53 - g21 * h52 - g51; h50 = g40 * g54 - g30 * h53 - g20 * h52 - g10 * h51 - g50; // Change this to its inverse = H^T * D^-1 * H //------------------------------------ // NOTE: Common subexpressions are parenthesized to provide proper ordering // for optimizing C++ compilers M[M00] = d00 + h10 * (h10*d11) + h20 * (h20*d22) + h30 * (h30*d33) + h40 * (h40*d44) + h50 * (h50*d55); M[M01] = (h10*d11) + h20 * (h21*d22) + h30 * (h31*d33) + h40 * (h41*d44) + h50 * (h51*d55);; M[M11] = d11 + h21 * (h21*d22) + h31 * (h31*d33) + h41 * (h41*d44) + h51 * (h51*d55); M[M02] = (h20*d22) + h30 * (h32*d33) + h40 * (h42*d44) + h50 * (h52*d55); M[M12] = (h21*d22) + h31 * (h32*d33) + h41 * (h42*d44) + h51 * (h52*d55); M[M22] = d22 + h32 * (h32*d33) + h42 * (h42*d44) + h52 * (h52*d55); M[M03] = (h30*d33) + h40 * (h43*d44) + h50 * (h53*d55); M[M13] = (h31*d33) + h41 * (h43*d44) + h51 * (h53*d55); M[M23] = (h32*d33) + h42 * (h43*d44) + h52 * (h53*d55); M[M33] = d33 + h43 * (h43*d44) + h53 * (h53*d55); M[M04] = (h40*d44) + h50 * (h54*d55); M[M14] = (h41*d44) + h51 * (h54*d55); M[M24] = (h42*d44) + h52 * (h54*d55); M[M34] = (h43*d44) + h53 * (h54*d55); M[M44] = d44 + h54 * (h54*d55); M[M05] = (h50*d55); M[M15] = (h51*d55); M[M25] = (h52*d55); M[M35] = (h53*d55); M[M45] = (h54*d55); M[M55] = d55; return true; }