udu.cpp

良好的代码实现

/*
 * Bayes++ the Bayesian Filtering Library
 * Copyright (c) 2002 Michael Stevens
 * See accompanying Bayes++.htm for terms and conditions of use.
 *
 * $Header: /cvsroot/bayesclasses/Bayes++/BayesFilter/UdU.cpp,v 1.24.2.4 2005/01/17 14:00:16 mistevens Exp $
 * $NoKeywords: $
 */

/*
 * Linear algebra support functions for filter classes
 * Cholesky and Modified Cholesky factorisations
 *
 * UdU' and LdL' factorisations of Positive semi-definate matrices. Where
 *  U is unit upper triangular
 *  d is diagonal
 *  L is unit lower triangular
 * Storage
 *  UD(RowMatrix) format of UdU' factor
 *   strict_upper_triangle(UD) = strict_upper_triangle(U), diagonal(UD) = d, strict_lower_triangle(UD) ignored or zeroed
 *  LD(LTriMatrix) format of LdL' factor 
 *   strict_lower_triangle(LD) = strict_lower_triangle(L), diagonal(LD) = d, strict_upper_triangle(LD) ignored or zeroed
 */
#include "bayesFlt.hpp"
#include "matSup.hpp"
#include <cassert>
#include <cmath>

/* Filter Matrix Namespace */
namespace Bayesian_filter_matrix
{


template <class V>
inline typename V::value_type rcond_internal (const V& D)
/*
 * Estimate the reciprocal condition number of a Diagonal Matrix for inversion.
 * D represents a diagonal matrix, the parameter is actually passed as a vector
 *
 * The Condition Number is defined from a matrix norm.
 *  Choose max element of D as the norm of the original matrix.
 *  Assume this norm for inverse matrix is min element D.
 *  Therefore rcond = min/max
 *
 * Note:
 *  Defined to be 0 for semi-definate and 0 for an empty matrix
 *  Defined to be 0 for max and min infinite
 *  Defined to be <0 for negative matrix (D element a value  < 0)
 *  Defined to be <0 with any NaN element
 *
 *  A negative matrix may be due to errors in the original matrix resulting in
 *   a factorisation producing special values in D (e.g. -infinity,NaN etc)
 *  By definition rcond <= 1 as min<=max
 */
{
	// Special case an empty matrix
	const std::size_t n = D.size();
	if (n == 0)
		return 0;

	Vec::value_type rcond, mind = D[0], maxd = 0;

	for (std::size_t i = 0; i < n; ++i)	{
		Vec::value_type d = D[i];
		if (d != d)				// NaN
			return -1;
		if (d < mind) mind = d;
		if (d > maxd) maxd = d;
	}

	if (mind < 0)				// matrix is negative
		return -1;
								// ISSUE mind may still be -0, this is progated into rcond
	assert (mind <= maxd);		// check sanity
	
	rcond = mind / maxd; 		// rcond from min/max norm
	if (rcond != rcond)			// NaN, singular due to (mind == maxd) == (zero or infinity)
		rcond = 0;
	assert (rcond <= 1);
	return rcond;
}

template <class V>
inline typename V::value_type rcond_ignore_infinity_internal (const V& D)
/*
 * Estimate the reciprocal condition number of a Diagonal Matrix for inversion.
 * Same as rcond_internal except that elements are infinity are ignored
 * when determining the maximum element.
 */
{
	// Special case an empty matrix
	const std::size_t n = D.size();
	if (n == 0)
		return 0;

	Vec::value_type rcond, mind = D[0], maxd = 0;

	for (std::size_t i = 0; i < n; ++i)	{
		Vec::value_type d = D[i];
		if (d != d)				// NaN
			return -1;
		if (d < mind) mind = d;
		if (d > maxd && 1/d != 0)	// ignore infinity for maxd
			maxd = d;
	}

	if (mind < 0)				// matrix is negative
		return -1;
								// ISSUE mind may still be -0, this is progated into rcond
    if (maxd == 0)				// singular due to maxd == zero (elements all zero or infinity)
		return 0;
	assert (mind <= maxd);		// check sanity

	rcond = mind / maxd; 		// rcond from min/max norm
								// CRITICAL CHECK requires NaN != NaN
	if (rcond != rcond)			// NaN, singular due to (mind == maxd) == infinity
		rcond = 0;
	assert (rcond <= 1);
	return rcond;
}

Vec::value_type UdUrcond (const Vec& d)
/*
 * Estimate the reciprocal condition number for inversion of the original PSD
 * matrix for which d is the factor UdU' or LdL'.
 * The original matrix must therefore be diagonal
 */
{
	return rcond_internal (d);
}

RowMatrix::value_type UdUrcond (const RowMatrix& UD)
/*
 * Estimate the reciprocal condition number for inversion of the original PSD
 * matrix for which UD is the factor UdU' or LdL'
 *
 * The rcond of the original matrix is simply the rcond of its d factor
 * Using the d factor is fast and simple, and avoids computing any squares.
 */
{
	assert (UD.size1() == UD.size2());
	return rcond_internal (diag(UD));
}

RowMatrix::value_type UdUrcond (const RowMatrix& UD, std::size_t n)
/*
 * As above but only first n elements are used
 */
{
	return rcond_internal (diag(UD,n));
}

UTriMatrix::value_type UCrcond (const UTriMatrix& UC)
/*
 * Estimate the reciprocal condition number for inversion of the original PSD
 * matrix for which U is the factor UU'
 *
 * The rcond of the original matrix is simply the square of the rcond of diagonal(UC)
 */
{
	assert (UC.size1() == UC.size2());
	Float rcond = rcond_internal (diag(UC));
	// Square to get rcond of original matrix, take care to propogate rcond's sign!
	if (rcond < 0)
		return -(rcond*rcond);
	else
		return rcond*rcond;
}

inline UTriMatrix::value_type UCrcond (const UTriMatrix& UC, std::size_t n)
/*
 * As above but for use by UCfactor functions where only first n elements are used
 */
{
	Float rcond = rcond_internal (diag(UC,n));
	// Square to get rcond of original matrix, take care to propogate rcond's sign!
	if (rcond < 0)
		return -(rcond*rcond);
	else
		return rcond*rcond;
}


RowMatrix::value_type UdUdet (const RowMatrix& UD)
/*
 * Compute the determinant of the original PSD
 * matrix for which UD is the factor UdU' or LdL'
 * Result comes directly from determinant of diagonal in triangular matrices
 *  Defined to be 1 for 0 size UD
 */
{
	const std::size_t n = UD.size1();
	assert (n == UD.size2());
	RowMatrix::value_type det = 1;
	for (std::size_t i = 0; i < n; ++i)	{
		det *= UD(i,i);
	}
	return det;
}


RowMatrix::value_type UdUfactor_variant1 (RowMatrix& M, std::size_t n)
/*
 * In place Modified upper triangular Cholesky factor of a
 *  Positive definate or semi-definate matrix M
 * Reference: A+G p.218 Upper cholesky algorithm modified for UdU'
 *  Numerical stability may not be as good as M(k,i) is updated from previous results
 *  Algorithm has poor locality of reference and avoided for large matrices
 *  Infinity values on the diagonal can be factorised
 *
 * Strict lower triangle of M is ignored in computation
 *
 * Input: M, n=last std::size_t to be included in factorisation
 * Output: M as UdU' factor
 *    strict_upper_triangle(M) = strict_upper_triangle(U)
 *    diagonal(M) = d
 *    strict_lower_triangle(M) is unmodifed
 * Return:
 *    reciprocal condition number, -1 if negative, 0 if semi-definate (including zero)
 */
{
	std::size_t i,j,k;
	RowMatrix::value_type e, d;

	if (n > 0)
	{
		j = n-1;
		do {
			d = M(j,j);

			// Diagonal element
			if (d > 0)
			{	// Positive definate
				d = 1 / d;

				for (i = 0; i < j; ++i)
				{
					e = M(i,j);
					M(i,j) = d*e;
					for (k = 0; k <= i; ++k)
					{
						RowMatrix::Row Mk(M,k);
						Mk[i] -= e*Mk[j];
					}
				}
			}
			else if (d == 0)
			{	// Possibly Semidefinate, check not negative
				for (i = 0; i < j; ++i)
				{
					if (M(i,j) != 0)
						goto Negative;
				}
			}
			else
			{	// Negative
				goto Negative;
			}
		} while (j-- > 0);
	}

	// Estimate the reciprocal condition number
	return rcond_internal (diag(M,n));

Negative:
   return -1;
}


RowMatrix::value_type UdUfactor_variant2 (RowMatrix& M, std::size_t n)
/*
 * In place Modified upper triangular Cholesky factor of a
 *  Positive definate or semi-definate matrix M
 * Reference: A+G p.219 right side of table
 *  Algorithm has good locality of reference and preferable for large matrices
 *  Infinity values on the diagonal cannot be factorised
 *
 * Strict lower triangle of M is ignored in computation
 *
 * Input: M, n=last std::size_t to be included in factorisation
 * Output: M as UdU' factor
 *    strict_upper_triangle(M) = strict_upper_triangle(U)
 *    diagonal(M) = d
 *    strict_lower_triangle(M) is unmodifed
 * Return:
 *    reciprocal condition number, -1 if negative, 0 if semi-definate (including zero)
 */
{
	std::size_t i,j,k;
	RowMatrix::value_type e, d;
	if (n > 0)
	{
		j = n-1;
		do {
			RowMatrix::Row Mj(M,j);
			d = Mj[j];

			// Diagonal element
			if (d > 0)
			{	// Positive definate
				i = j;
				do
				{
					RowMatrix::Row Mi(M,i);
					e = Mi[j];
					for (k = j+1; k < n; ++k)
					{
						e -= Mi[k]*M(k,k)*Mj[k];
					}
					if (i == j) {
						Mi[j] = d = e;		// Diagonal element
					}
					else {
						Mi[j] = e / d;
					}
				} while (i-- > 0);
			}
			else if (d == 0)
			{	// Possibly Semidefinate, check not negative, whole row must be identically zero
				for (k = j+1; k < n; ++k)
				{
					if (Mj[k] != 0)
						goto Negative;
				}
			}
			else
			{	// Negative
				goto Negative;
			}
		} while (j-- > 0);
	}

	// Estimate the reciprocal condition number
	return rcond_internal (diag(M,n));

Negative:
   return -1;
}


LTriMatrix::value_type LdLfactor (LTriMatrix& M, std::size_t n)
/*
 * In place Modified lower triangular Cholesky factor of a
 *  Positive definate or semi-definate matrix M
 * Reference: A+G p.218 Lower cholesky algorithm modified for LdL'
 *
 * Input: M, n=last std::size_t to be included in factorisation
 * Output: M as LdL' factor
 *    strict_lower_triangle(M) = strict_lower_triangle(L)
 *    diagonal(M) = d
 * Return:
 *    reciprocal condition number, -1 if negative, 0 if semi-definate (including zero)
 * ISSUE: This could change to equivilient of UdUfactor_varient2
 */
{
	std::size_t i,j,k;
	LTriMatrix::value_type e, d;

	for (j = 0; j < n; ++j)
	{
		d = M(j,j);

		// Diagonal element
		if (d > 0)
		{
			// Positive definate
			d = 1 / d;

			for (i = j+1; i < n; ++i)
			{
				e = M(i,j);
				M(i,j) = d*e;
				for (k = i; k < n; ++k)
				{
					LTriMatrix::Row Mk(M,k);
					Mk[i] -= e*Mk[j];
				}
			}
		}
		else if (d == 0)
		{
			// Possibly Semidefinate, check not negative
			for (i = j+1; i < n; ++i)
			{
				if (M(i,j) != 0)
					goto Negative;
			}
		}
		else
		{
			// Negative
			goto Negative;
		}
	}

	// Estimate the reciprocal condition number
	return rcond_internal (diag(M,n));

Negative:
	return -1;
}


UTriMatrix::value_type UCfactor (UTriMatrix& M, std::size_t n)
/*
 * In place Upper triangular Cholesky factor of a
 *  Positive definate or semi-definate matrix M
 * Reference: A+G p.218
 * Strict lower triangle of M is ignored in computation
 *
 * Input: M, n=last std::size_t to be included in factorisation
 * Output: M as UC*UC' factor
 *    upper_triangle(M) = UC
 * Return:
 *    reciprocal condition number, -1 if negative, 0 if semi-definate (including zero)
 */
{
	std::size_t i,j,k;
	UTriMatrix::value_type e, d;

	if (n > 0)
	{
		j = n-1;
		do {
			d = M(j,j);

			// Diagonal element
			if (d > 0)
			{
				// Positive definate
				d = std::sqrt(d);
				M(j,j) = d;
				d = 1 / d;

				for (i = 0; i < j; ++i)
				{
					e = d*M(i,j);
					M(i,j) = e;
					for (k = 0; k <= i; ++k)
					{
						UTriMatrix::Row Mk(M,k);
						Mk[i] -= e*Mk[j];
					}
				}
			}
			else if (d == 0)
			{
				// Possibly Semidefinate, check not negative
				for (i = 0; i < j; ++i)
				{
					if (M(i,j) != 0)
						goto Negative;
				}
			}
			else
			{
				// Negative
				goto Negative;
			}
		} while (j-- > 0);
	}

	// Estimate the reciprocal condition number
	return UCrcond (M,n);

Negative:
   return -1;