/* * 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;