Bayesian Filter.贝叶斯（Bayesian）滤波器的C++类库。包括卡尔曼滤波（kalman filter）、粒子

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