Implement evaluation of a Gaussian density in case of special factorization of the covariance matrix

[xEnVrE]

This issue is for the implementation of a utility function to evaluate a multivariate Gaussian density in those cases in which the covariance matrix can be written in the form S = UV + R with R an invertible block diagonal matrix.

This is useful when the covariance matrix of the measurement model is such that

• the evaluation of its inverse and
• the evaluation of its determinant,

that are required to evaluate the Gaussian density, are computationally demanding because rows(S) is a large number. However, if cols(U) << rows(U), then it possible to use the Sherman-Morrison formula and the fact that R is block diagonal to reduce the problem to the inversion (and the evaluation of the determinant) of a matrix of size cols(U) x cols(U)

Once this is implemented, we will be able to implement bfl::SUKFCorrection::getLikelihood.

[claudiofantacci]

Thanks for the digression! Looking forward to it 👍

[claudiofantacci]

Closed after #74.

[traversaro]

@claudiofantacci @xEnVrE I am relative sure it does not apply to your case, but it may be interesting for you to know that in some context (where you have a lot of low-rank updates) the computation of the inverse matrix via repeated uses of the Sherman–Morrison–Woodbury identity is numerically noisy/unstable ( https://epubs.siam.org/doi/abs/10.1137/0907034?journalCode=sijcd4 ). For Symmetric Positive Definite matrices it is sometimes preferred to use the Cholesky Update, see for example https://www.semanticscholar.org/paper/Low-Rank-Updates-for-the-Cholesky-Decomposition-Seeger/8e22b71338d20c884bbb904155f12227781eb750 .

[claudiofantacci]

@traversaro thanks for the comment. I think we may want to consider it anyway 👍

[xEnVrE]

Thank you @traversaro for your point. I wasn't aware of this. If I am not wrong, Cholesky Update is also used in the square root form of the Unscented Kalman Filter exactly for this reason.

In the end, the paper from which we are taking this kind of usage of the SMW identity, rewrites the covariance matrix using a sort of Cholesky factorization but the inner structure of the factors is exactly known. Don't know if, by using Cholesky updates, it is possible to obtain the same speedup as we are getting with SMW. I'll read the references you pointed out.

[traversaro]

If you are interested in this, we can also check out Section 3.3 of http://pasa.lira.dist.unige.it/pasapdf/1228_Gijsberts+Metta2012.pdf (that is actually the reason I am aware of this stuff).

[traversaro]

We actually still have the code from the Cholesky update in icub-main, see:

• https://github.com/robotology/icub-main/blob/4f5881acafab693f763dd277f87a8996fdea01a3/src/libraries/learningMachine/include/iCub/learningMachine/Math.h#L100
• https://github.com/robotology/icub-main/blob/4f5881acafab693f763dd277f87a8996fdea01a3/src/libraries/learningMachine/src/Math.cpp#L136

[claudiofantacci]

Let's move the discussion in #76!