/* * 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/matSup.cpp,v 1.9.2.2 2004/07/18 18:38:04 mistevens Exp $ * $NoKeywords: $ *//* * Matrix support functions for filter classes * Relies on Bayesian_filter::Bayes_filter for exception * thrown by internal matrix checks */#include "bayesFlt.hpp"		// Exceptions required#include "matSup.hpp"#include <cassert>#ifndef NDEBUG#include "boost/numeric/ublas/io.hpp"#endifnamespace {template <class scalar>inline scalar sqr(scalar x)// Square {	return x*x;}};//namespace/* Filter Matrix Namespace */namespace Bayesian_filter_matrix{#ifndef NDEBUGvoid assert_isPSD (const SymMatrix &M)/* * Assert a Matrix is Positive Semi Definate via the alogrithm in isPSD *  Requires 'cerr' and 'assert' to about execution */{	bool bPSD = isPSD(M);	if (!bPSD) {		// Display Non PSD		std::cerr.flags(std::ios::scientific); std::cerr.precision(17);		std::cerr << M;	}	assert(bPSD);}#endifbool isPSD (const SymMatrix &M)/* * Check a Matrix is both Symertic and Positive Semi Definate *  Creates a temporary copy *   * Numerics of Algorithm: *  Use UdUfactor and checks reciprocal condition number >=0 *  Algorithms using UdUfactor will will therefore succeed if isPSD(M) is true *  Do NOT assume because isPSD is true that M will appear to be PSD to other numerical algorithms * Return: *  true iff M isSymetric and is PSD by the above algorithm */{	RowMatrix UD(M.size1(),M.size1());	RowMatrix::value_type rcond = UdUfactor(UD, M);	return rcond >= 0.;}bool isSymmetric (const Matrix &M)/* * Check a Symmetric Matrix really is Square and Symmetric * The later may be implied by the SymMatrix type * Implictly also checks matrix is without IEC 559 NaN values as they are always != * Return: *  true iff M is Square and Symmetric and without NaN  */{	// Check Square	if (M.size1() != M.size2() ) {		return false;	}	// Check equality of upper and lower	bool bSym = true;	std::size_t size = M.size1();	for (std::size_t r = 0; r < size; ++r) {		for (std::size_t c = 0; c <= r; ++c) {			if( M(r,c) != M(c,r) ) {				bSym = false;			}		}	}	return bSym;}void forceSymmetric (Matrix &M, bool bUpperToLower)/* * Force Matrix Symmetry *	Normally Copies ower triangle to upper or *   Upper to Lower triangle is specifed */{	// Check Square	if (M.size1() != M.size2() ) {		using namespace Bayesian_filter;		Bayes_base::error (Logic_exception ("Matrix is not square"));	}	std::size_t size = M.size1();	if (bUpperToLower)	{		// Copy Lower to Upper		for (std::size_t r = 1; r < size; ++r) {			for (std::size_t c = 0; c < r; ++c) {				M(c,r) = M(r,c);			}		}	}	else	{		// Copy Upper to Lower		for (std::size_t r = 1; r < size; ++r) {			for (std::size_t c = 0; c < r; ++c) {				M(r,c) = M(c,r);			}		}	}}}//namespace