mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-27 08:43:33 +08:00
113 lines
2.8 KiB
C++
113 lines
2.8 KiB
C++
/**
|
||
* @file PseudoInverse.hpp
|
||
*
|
||
* Implementation of matrix pseudo inverse
|
||
*
|
||
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
|
||
* @author Julian Kent <julian@auterion.com>
|
||
*/
|
||
|
||
#pragma once
|
||
|
||
#include "math.hpp"
|
||
|
||
namespace matrix
|
||
{
|
||
|
||
/**
|
||
* 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);
|
||
|
||
SquareMatrix<Type, M> LtL = L.transpose() * L;
|
||
SquareMatrix<Type, M> X;
|
||
if (!inv(LtL, X, rank)) {
|
||
return Matrix<Type, N, M>(); // LCOV_EXCL_LINE -- this can only be hit from numerical issues
|
||
}
|
||
return G.transpose() * L * X * X * L.transpose();
|
||
|
||
} else {
|
||
SquareMatrix<Type, N> A = G.transpose() * G;
|
||
SquareMatrix<Type, N> L = fullRankCholesky(A, rank);
|
||
|
||
SquareMatrix<Type, N> LtL = L.transpose() * L;
|
||
SquareMatrix<Type, N> X;
|
||
if(!inv(LtL, X, rank)) {
|
||
return Matrix<Type, N, M>(); // LCOV_EXCL_LINE -- this can only be hit from numerical issues
|
||
}
|
||
return L * X * X * L.transpose() * G.transpose();
|
||
}
|
||
}
|
||
|
||
|
||
template<typename Type>
|
||
Type typeEpsilon();
|
||
|
||
template<> inline
|
||
float typeEpsilon<float>()
|
||
{
|
||
return FLT_EPSILON;
|
||
}
|
||
|
||
/**
|
||
* Full rank Cholesky factorization of A
|
||
*/
|
||
template<typename Type, size_t N>
|
||
SquareMatrix<Type, N> fullRankCholesky(const SquareMatrix<Type, N> & A,
|
||
size_t& rank)
|
||
{
|
||
// Loses one ulp accuracy per row of diag, relative to largest magnitude
|
||
const Type tol = N * typeEpsilon<Type>() * A.diag().max();
|
||
|
||
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;
|
||
}
|
||
|
||
} // namespace matrix
|
||
|
||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|