fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 05:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add implementation of pseudo-inverse (#102)

Browse Source 
* Fix compilation error

* Add implementation of pseudo-inverse

The implementation is based on this publication:
Courrieu, P. (2008). Fast Computation of Moore-Penrose Inverse Matrices, 8(2), 25–29. http://arxiv.org/abs/0804.4809
It is a fully templated implementation to guaranty type correctness.

* Add tests for pseudoinverse

* Apply suggestions from code review

Co-Authored-By: Mathieu Bresciani <brescianimathieu@gmail.com>

* Adapt fullRankCholesky tolerance to type

* Add pseudoinverse test with effectiveness matrix

* Fix coverage

* Fix rebase issue

* Fix SquareMatrix test, add null Matrix test
This commit is contained in:
Julien Lecoeur
2019-11-18 14:36:30 -08:00
committed by Julian Kent
co-authored by Mathieu Bresciani
parent cd185c995b
commit a172c3cdac
7 changed files with 338 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/Dcm.hpp
+1 -1
View File
@@ -65,7 +65,7 @@ public:
*
* @param _data pointer to array
*/
explicit Dcm(const Type data_[3*3]) : SquareMatrix<Type, 3>(data_)
explicit Dcm(const Type data_[9]) : SquareMatrix<Type, 3>(data_)
{
}
matrix/PseudoInverse.hpp
+56
View File
@@ -0,0 +1,56 @@
/**
* @file PseudoInverse.hpp
*
* Implementation of matrix pseudo inverse
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
#pragma once
#include "math.hpp"
namespace matrix
{
/**
* Full rank Cholesky factorization of A
*/
template<typename Type, size_t N>
SquareMatrix<Type, N> fullRankCholesky(const SquareMatrix<Type, N> & A, size_t& rank);
/**
* Geninv implementation detail
*/
template<typename Type, size_t M, size_t N, size_t R> class GeninvImpl;
/**
* Geninv
* Fast pseudoinverse based on full rank cholesky factorisation
*
* Courrieu, P. (2008). Fast Computation of Moore-Penrose Inverse Matrices, 8(2), 25–29. http://arxiv.org/abs/0804.4809
*/
template<typename Type, size_t M, size_t N>
Matrix<Type, N, M> geninv(const Matrix<Type, M, N> & G)
{
size_t rank;
if (M <= N) {
SquareMatrix<Type, M> A = G * G.transpose();
SquareMatrix<Type, M> L = fullRankCholesky(A, rank);
return GeninvImpl<Type, M, N, M>::genInvUnderdetermined(G, L, rank);
} else {
SquareMatrix<Type, N> A = G.transpose() * G;
SquareMatrix<Type, N> L = fullRankCholesky(A, rank);
return GeninvImpl<Type, M, N, N>::genInvOverdetermined(G, L, rank);
}
}
#include "PseudoInverse.hxx"
} // namespace matrix
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
matrix/PseudoInverse.hxx
+139
View File
@@ -0,0 +1,139 @@
/**
* @file pseudoinverse.hxx
*
* Implementation of matrix pseudo inverse
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
/**
* Full rank Cholesky factorization of A
*/
template<typename Type>
void fullRankCholeskyTolerance(Type &tol)
{
tol /= 1000000000;
}
template<> inline
void fullRankCholeskyTolerance<double>(double &tol)
{
tol /= 1000000000000000000.0;
}
template<typename Type, size_t N>
SquareMatrix<Type, N> fullRankCholesky(const SquareMatrix<Type, N> & A,
size_t& rank)
{
// Compute
// dA = np.diag(A)
// tol = np.min(dA[dA > 0]) * 1e-9
Vector<Type, N> d = A.diag();
Type tol = d.max();
for (size_t k = 0; k < N; k++) {
if ((d(k) > 0) && (d(k) < tol)) {
tol = d(k);
}
}
fullRankCholeskyTolerance<Type>(tol);
Matrix<Type, N, N> L;
size_t r = 0;
for (size_t k = 0; k < N; k++) {
if (r == 0) {
for (size_t i = k; i < N; i++) {
L(i, r) = A(i, k);
}
} else {
for (size_t i = k; i < N; i++) {
// Compute LL = L[k:n, :r] * L[k, :r].T
Type LL = Type();
for (size_t j = 0; j < r; j++) {
LL += L(i, j) * L(k, j);
}
L(i, r) = A(i, k) - LL;
}
}
if (L(k, r) > tol) {
L(k, r) = sqrt(L(k, r));
if (k < N - 1) {
for (size_t i = k + 1; i < N; i++) {
L(i, r) = L(i, r) / L(k, r);
}
}
r = r + 1;
}
}
// Return rank
rank = r;
return L;
}
template< typename Type, size_t M, size_t N, size_t R>
class GeninvImpl
{
public:
static Matrix<Type, N, M> genInvUnderdetermined(const Matrix<Type, M, N> & G, const Matrix<Type, M, M> & L, size_t rank)
{
if (rank < R) {
// Recursive call
return GeninvImpl<Type, M, N, R - 1>::genInvUnderdetermined(G, L, rank);
} else if (rank > R) {
// Error
return Matrix<Type, N, M>();
} else {
// R == rank
Matrix<Type, M, R> LL = L. template slice<M, R>(0, 0);
SquareMatrix<Type, R> X = inv(SquareMatrix<Type, R>(LL.transpose() * LL));
return G.transpose() * LL * X * X * LL.transpose();
}
}
static Matrix<Type, N, M> genInvOverdetermined(const Matrix<Type, M, N> & G, const Matrix<Type, N, N> & L, size_t rank)
{
if (rank < R) {
// Recursive call
return GeninvImpl<Type, M, N, R - 1>::genInvOverdetermined(G, L, rank);
} else if (rank > R) {
// Error
return Matrix<Type, N, M>();
} else {
// R == rank
Matrix<Type, N, R> LL = L. template slice<N, R>(0, 0);
SquareMatrix<Type, R> X = inv(SquareMatrix<Type, R>(LL.transpose() * LL));
return LL * X * X * LL.transpose() * G.transpose();
}
}
};
// Partial template specialisation for R==0, allows to stop recursion in genInvUnderdetermined and genInvOverdetermined
template< typename Type, size_t M, size_t N>
class GeninvImpl<Type, M, N, 0>
{
public:
static Matrix<Type, N, M> genInvUnderdetermined(const Matrix<Type, M, N> & G, const Matrix<Type, M, M> & L, size_t rank)
{
return Matrix<Type, N, M>();
}
static Matrix<Type, N, M> genInvOverdetermined(const Matrix<Type, M, N> & G, const Matrix<Type, N, N> & L, size_t rank)
{
return Matrix<Type, N, M>();
}
};
matrix/math.hpp
+1
View File
@@ -18,3 +18,4 @@
#include "AxisAngle.hpp"
#include "LeastSquaresSolver.hpp"
#include "Dual.hpp"
#include "PseudoInverse.hpp"