Skip to content
This repository has been archived by the owner on Jan 7, 2019. It is now read-only.

Commit

Permalink
[math][fix]fixed matrix multiplication + LU-Decomp
Browse files Browse the repository at this point in the history
  • Loading branch information
@strongly-typed
Marten Junga authored and strongly-typed committed Dec 19, 2017
1 parent 827d9f4 commit c4ed672
Show file tree
Hide file tree
Showing 7 changed files with 224 additions and 51 deletions.
40 changes: 27 additions & 13 deletions src/xpcc/math/lu_decomposition.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,21 +62,35 @@ namespace xpcc
Matrix<T, N, N> *l,
Matrix<T, N, N> *u);

template <typename T, uint8_t N>
static bool
decompose(const Matrix<T, N, N> &matrix,
Matrix<T, N, N> *l,
Matrix<T, N, N> *u,
Matrix<T, N, N> *p);

template <typename T, uint8_t N>
static bool
decompose(const Matrix<T, N, N> &matrix,
Matrix<T, N, N> *l,
Matrix<T, N, N> *u,
Vector<int8_t, N> *p);


template <typename T, uint8_t N, uint8_t BXWIDTH>
static bool
solve(const Matrix<T, N, N> &l,
const Matrix<T, N, N> &u,
Matrix<T, BXWIDTH, N> *xb);
Matrix<T, N, BXWIDTH> *xb);

template <typename T, uint8_t N, uint8_t BXWIDTH>
static bool
solve(const Matrix<T, N, N> &A,
Matrix<T, N, BXWIDTH> *xb);


private:
template<typename T, uint8_t OFFSET, uint8_t WIDTH, uint8_t HEIGHT>
template<typename T, uint8_t OFFSET, uint8_t HEIGHT, uint8_t WIDTH>
class LUSubDecomposition
{
public:
Expand All @@ -95,21 +109,21 @@ namespace xpcc
template<uint8_t BXWIDTH>
static bool
solveLyEqualsB(
Matrix<T, WIDTH, HEIGHT> *l,
Matrix<T, BXWIDTH, HEIGHT> *bx);
Matrix<T, HEIGHT, WIDTH> *l,
Matrix<T, HEIGHT, BXWIDTH> *bx);

template<uint8_t BXWIDTH>
static bool
solveUxEqualsY(
Matrix<T, WIDTH, HEIGHT> *u,
Matrix<T, BXWIDTH, HEIGHT> *bx);
Matrix<T, HEIGHT, WIDTH> *u,
Matrix<T, HEIGHT, BXWIDTH> *bx);

template<uint8_t BXWIDTH>
static bool
solve(
const Matrix<T, WIDTH, HEIGHT> &l,
const Matrix<T, WIDTH, HEIGHT> &u,
Matrix<T, BXWIDTH, HEIGHT> *bx);
const Matrix<T, HEIGHT, WIDTH> &l,
const Matrix<T, HEIGHT, WIDTH> &u,
Matrix<T, HEIGHT, BXWIDTH> *bx);
};

template<typename T, uint8_t HEIGHT>
Expand Down Expand Up @@ -161,14 +175,14 @@ namespace xpcc
template<uint8_t BXWIDTH>
static bool
solveUxEqualsY(
Matrix<T, WIDTH, OFFSET> *u,
Matrix<T, BXWIDTH, OFFSET> *bx);
Matrix<T, WIDTH, OFFSET> *u,
Matrix<T, OFFSET, BXWIDTH> *bx);

template<uint8_t BXWIDTH>
static bool
solveLyEqualsB(
Matrix<T, WIDTH, OFFSET> *l,
Matrix<T, BXWIDTH, OFFSET> *bx);
Matrix<T, WIDTH, OFFSET> *l,
Matrix<T, OFFSET, BXWIDTH> *bx);
};
};
}
Expand Down
96 changes: 66 additions & 30 deletions src/xpcc/math/lu_decomposition_impl.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,18 +63,54 @@ xpcc::LUDecomposition::decompose(

return LUSubDecomposition<T, 0, SIZE, SIZE>::decomposeRecur(u->ptr(), l->ptr(), p->ptr());
}
// ----------------------------------------------------------------------------
template<typename T, uint8_t SIZE>
bool
xpcc::LUDecomposition::decompose(
const xpcc::Matrix<T, SIZE, SIZE> &matrix,
xpcc::Matrix<T, SIZE, SIZE> *l,
xpcc::Matrix<T, SIZE, SIZE> *u,
xpcc::Matrix<T, SIZE, SIZE> *p)
{
xpcc::Vector<int8_t, SIZE> pv;
if (not(decompose(matrix, l, u, &pv))){
return false;}
*p = xpcc::Matrix<T, SIZE, SIZE>::zeroMatrix();
for (int i=0; i<SIZE; i++){
(*p)[i][pv[i]] = 1;}
return true;
}

// ----------------------------------------------------------------------------
template<typename T, uint8_t SIZE, uint8_t BXWIDTH>
bool
xpcc::LUDecomposition::solve(
const xpcc::Matrix<T, SIZE, SIZE> &l,
const xpcc::Matrix<T, SIZE, SIZE> &u,
xpcc::Matrix<T, BXWIDTH, SIZE> *xb)
xpcc::Matrix<T, SIZE, BXWIDTH> *xb)
{
return LUSubDecomposition<T, 0, SIZE, SIZE>::solve(l, u, xb);
}

// ----------------------------------------------------------------------------
template<typename T, uint8_t SIZE, uint8_t BXWIDTH>
bool
xpcc::LUDecomposition::solve(
const xpcc::Matrix<T, SIZE, SIZE> &A,
xpcc::Matrix<T, SIZE, BXWIDTH> *xb)
{
xpcc::Matrix<T, SIZE, SIZE> l;
xpcc::Matrix<T, SIZE, SIZE> u;
xpcc::Matrix<T, SIZE, SIZE> p;
if(not(decompose(A, &l, &u, &p))){
return false;
}
*xb = (p * (*xb));
if(not(solve(l, u, xb))){
return false;}
return true;
}

//=============================================================================
// PRIVATE CLASS xpcc::LUDecomposition::RowOperation
//=============================================================================
Expand Down Expand Up @@ -143,20 +179,20 @@ xpcc::LUDecomposition::RowOperation<T, 0>::swap(T * /* row1 */, T * /* row2 */)
//=============================================================================

// ----------------------------------------------------------------------------
template<typename T, uint8_t OFFSET, uint8_t WIDTH, uint8_t HEIGHT>
template<typename T, uint8_t OFFSET, uint8_t HEIGHT, uint8_t WIDTH>
bool
xpcc::LUDecomposition::LUSubDecomposition<T, OFFSET, WIDTH, HEIGHT>::decomposeRecur(T * u, T * l)
xpcc::LUDecomposition::LUSubDecomposition<T, OFFSET, HEIGHT, WIDTH>::decomposeRecur(T * u, T * l)
{
if (!decompose(u, l))
return false;

return LUSubDecomposition<T, OFFSET+1, WIDTH, HEIGHT>::decomposeRecur(u, l);
return LUSubDecomposition<T, OFFSET+1, HEIGHT, WIDTH>::decomposeRecur(u, l);
}

// ----------------------------------------------------------------------------
template<typename T, uint8_t OFFSET, uint8_t WIDTH, uint8_t HEIGHT>
template<typename T, uint8_t OFFSET, uint8_t HEIGHT, uint8_t WIDTH>
bool
xpcc::LUDecomposition::LUSubDecomposition<T, OFFSET, WIDTH, HEIGHT>::decompose(T * u, T * l)
xpcc::LUDecomposition::LUSubDecomposition<T, OFFSET, HEIGHT, WIDTH>::decompose(T * u, T * l)
{
const uint8_t width = WIDTH;
const uint8_t height = HEIGHT;
Expand All @@ -180,20 +216,20 @@ xpcc::LUDecomposition::LUSubDecomposition<T, OFFSET, WIDTH, HEIGHT>::decompose(T
}

// ----------------------------------------------------------------------------
template<typename T, uint8_t OFFSET, uint8_t WIDTH, uint8_t HEIGHT>
template<typename T, uint8_t OFFSET, uint8_t HEIGHT, uint8_t WIDTH>
bool
xpcc::LUDecomposition::LUSubDecomposition<T, OFFSET, WIDTH, HEIGHT>::decomposeRecur(T *u, T *l, int8_t *p)
xpcc::LUDecomposition::LUSubDecomposition<T, OFFSET, HEIGHT, WIDTH>::decomposeRecur(T *u, T *l, int8_t *p)
{
if (!decompose(u, l, p))
return false;

return LUSubDecomposition<T, OFFSET+1, WIDTH, HEIGHT>::decomposeRecur(u, l, p);
return LUSubDecomposition<T, OFFSET+1, HEIGHT, WIDTH>::decomposeRecur(u, l, p);
}

// ----------------------------------------------------------------------------
template<typename T, uint8_t OFFSET, uint8_t WIDTH, uint8_t HEIGHT>
template<typename T, uint8_t OFFSET, uint8_t HEIGHT, uint8_t WIDTH>
bool
xpcc::LUDecomposition::LUSubDecomposition<T, OFFSET, WIDTH, HEIGHT>::decompose(T *u, T *l, int8_t *p)
xpcc::LUDecomposition::LUSubDecomposition<T, OFFSET, HEIGHT, WIDTH>::decompose(T *u, T *l, int8_t *p)
{
const uint8_t width = WIDTH;
const uint8_t height = HEIGHT;
Expand Down Expand Up @@ -226,11 +262,11 @@ xpcc::LUDecomposition::LUSubDecomposition<T, OFFSET, WIDTH, HEIGHT>::decompose(T
}

// ----------------------------------------------------------------------------
template<typename T, uint8_t OFFSET, uint8_t WIDTH, uint8_t HEIGHT> template<uint8_t BXWIDTH>
template<typename T, uint8_t OFFSET, uint8_t HEIGHT, uint8_t WIDTH> template<uint8_t BXWIDTH>
bool
xpcc::LUDecomposition::LUSubDecomposition<T, OFFSET, WIDTH, HEIGHT>::solveLyEqualsB(
xpcc::Matrix<T, WIDTH, HEIGHT> *l,
xpcc::Matrix<T, BXWIDTH, HEIGHT> *bx)
xpcc::LUDecomposition::LUSubDecomposition<T, OFFSET, HEIGHT, WIDTH>::solveLyEqualsB(
xpcc::Matrix<T, HEIGHT, WIDTH> *l,
xpcc::Matrix<T, HEIGHT, BXWIDTH> *bx)
{
T factor = (*l)[OFFSET][OFFSET];
RowOperation<T, BXWIDTH>::multiply((*bx)[OFFSET], (*bx)[OFFSET], 1.0/factor);
Expand All @@ -248,25 +284,25 @@ xpcc::LUDecomposition::LUSubDecomposition<T, OFFSET, WIDTH, HEIGHT>::solveLyEqua
(*l)[j][OFFSET] = 0;
}
// solve the problem for SIZE-1
LUSubDecomposition<T, OFFSET+1, WIDTH, HEIGHT>::solveLyEqualsB(l, bx);
LUSubDecomposition<T, OFFSET+1, HEIGHT, WIDTH>::solveLyEqualsB(l, bx);

return true;
}

// ----------------------------------------------------------------------------
template<typename T, uint8_t OFFSET, uint8_t WIDTH, uint8_t HEIGHT> template<uint8_t BXWIDTH>
template<typename T, uint8_t OFFSET, uint8_t HEIGHT, uint8_t WIDTH> template<uint8_t BXWIDTH>
bool
xpcc::LUDecomposition::LUSubDecomposition<T, OFFSET, WIDTH, HEIGHT>::solveUxEqualsY(
xpcc::Matrix<T, WIDTH, HEIGHT> *u,
xpcc::Matrix<T, BXWIDTH, HEIGHT> *bx)
xpcc::LUDecomposition::LUSubDecomposition<T, OFFSET, HEIGHT, WIDTH>::solveUxEqualsY(
xpcc::Matrix<T, HEIGHT, WIDTH> *u,
xpcc::Matrix<T, HEIGHT, BXWIDTH> *bx)
{
// make sure there is a row under us
if (OFFSET >= HEIGHT-1) {
return true;
}

// solve the problem for SIZE-1
LUSubDecomposition<T, OFFSET+1, WIDTH, HEIGHT>::solveUxEqualsY(u, bx);
LUSubDecomposition<T, OFFSET+1, HEIGHT, WIDTH>::solveUxEqualsY(u, bx);

// substract the row so that all upper rows have a 0 at OFFSET
for (uint8_t j = 0; j < OFFSET+1; ++j)
Expand All @@ -280,12 +316,12 @@ xpcc::LUDecomposition::LUSubDecomposition<T, OFFSET, WIDTH, HEIGHT>::solveUxEqua
}

// ----------------------------------------------------------------------------
template<typename T, uint8_t OFFSET, uint8_t WIDTH, uint8_t HEIGHT> template<uint8_t BXWIDTH>
template<typename T, uint8_t OFFSET, uint8_t HEIGHT, uint8_t WIDTH> template<uint8_t BXWIDTH>
bool
xpcc::LUDecomposition::LUSubDecomposition<T, OFFSET, WIDTH, HEIGHT>::solve(
const xpcc::Matrix<T, WIDTH, HEIGHT> &l,
const xpcc::Matrix<T, WIDTH, HEIGHT> &u,
xpcc::Matrix<T, BXWIDTH, HEIGHT> *bx)
xpcc::LUDecomposition::LUSubDecomposition<T, OFFSET, HEIGHT, WIDTH>::solve(
const xpcc::Matrix<T, HEIGHT, WIDTH> &l,
const xpcc::Matrix<T, HEIGHT, WIDTH> &u,
xpcc::Matrix<T, HEIGHT, BXWIDTH> *bx)
{
// we want to solve Ax = b where A = LU
// we can write LUx = b
Expand All @@ -296,12 +332,12 @@ xpcc::LUDecomposition::LUSubDecomposition<T, OFFSET, WIDTH, HEIGHT>::solve(

// start solving Ly = b

xpcc::Matrix<T, WIDTH, HEIGHT> lCopy(l);
xpcc::Matrix<T, HEIGHT, WIDTH> lCopy(l);
if (!solveLyEqualsB(&lCopy, bx)) {
return false;
}

xpcc::Matrix<T, WIDTH, HEIGHT> uCopy(u);
xpcc::Matrix<T, HEIGHT, WIDTH> uCopy(u);
if (!solveUxEqualsY(&uCopy, bx)) {
return false;
}
Expand Down Expand Up @@ -330,7 +366,7 @@ template<typename T, uint8_t OFFSET, uint8_t WIDTH> template<uint8_t BXWIDTH>
bool
xpcc::LUDecomposition::LUSubDecomposition<T, OFFSET, WIDTH, OFFSET>::solveUxEqualsY(
xpcc::Matrix<T, WIDTH, OFFSET> * /* u */,
xpcc::Matrix<T, BXWIDTH, OFFSET> * /* bx */)
xpcc::Matrix<T, OFFSET, BXWIDTH> * /* bx */)
{
return true;
}
Expand All @@ -340,7 +376,7 @@ template<typename T, uint8_t OFFSET, uint8_t WIDTH> template<uint8_t BXWIDTH>
bool
xpcc::LUDecomposition::LUSubDecomposition<T, OFFSET, WIDTH, OFFSET>::solveLyEqualsB(
xpcc::Matrix<T, WIDTH, OFFSET> * /* l */,
xpcc::Matrix<T, BXWIDTH, OFFSET> * /* bx */)
xpcc::Matrix<T, OFFSET, BXWIDTH> * /* bx */)
{
return true;
}
Expand Down
2 changes: 1 addition & 1 deletion src/xpcc/math/matrix.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -175,7 +175,7 @@ namespace xpcc

/// Matrix multiplication with different size matrices
template<uint8_t RHSCOL>
Matrix<T, ROWS, ROWS>
Matrix<T, ROWS, RHSCOL>
operator * (const Matrix<T, COLUMNS, RHSCOL> &rhs) const;

Matrix<T, COLUMNS, ROWS>
Expand Down
8 changes: 4 additions & 4 deletions src/xpcc/math/matrix_impl.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -261,19 +261,19 @@ xpcc::Matrix<T, ROWS, COLUMNS>::operator -= (const xpcc::Matrix<T, ROWS, COLUMNS
// ----------------------------------------------------------------------------
template<typename T, uint8_t ROWS, uint8_t COLUMNS>
template<uint8_t RHSCOL>
xpcc::Matrix<T, ROWS, ROWS>
xpcc::Matrix<T, ROWS, RHSCOL>
xpcc::Matrix<T, ROWS, COLUMNS>::operator * (const Matrix<T, COLUMNS, RHSCOL> &rhs) const
{
xpcc::Matrix<T, ROWS, ROWS> m;
xpcc::Matrix<T, ROWS, RHSCOL> m;

for (uint_fast8_t i = 0; i < ROWS; ++i)
{
for (uint_fast8_t j = 0; j < RHSCOL; ++j)
{
m[j][i] = element[j * COLUMNS] * rhs[0][i];
m[i][j] = element[i * COLUMNS] * rhs[0][j];
for (uint_fast8_t x = 1; x < COLUMNS; ++x)
{
m[j][i] += element[j * COLUMNS + x] * rhs[x][i];
m[i][j] += element[i * COLUMNS + x] * rhs[x][j];
}
}
}
Expand Down
66 changes: 66 additions & 0 deletions src/xpcc/math/test/LUDecomposition_test.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,66 @@
#include <xpcc/math/matrix.hpp>
#include <xpcc/math/lu_decomposition.hpp>
#include "LUDecomposition_test.hpp"

void
LUDecompositionTest::testLUD()
{
// https://www.wolframalpha.com/input/?i=LU+Decomposition+of+%7B%7B1,2,3%7D,%7B0,1,2%7D,%7B3,4,6%7D%7D

const float m[] = {
1.f, 2.f, 3.f,
0.f, 1.f, 2.f,
3.f, 4.f, 6.f
};

xpcc::Matrix<float, 3, 3> A(m);
xpcc::Matrix<float, 3, 3> l;
xpcc::Matrix<float, 3, 3> u;
TEST_ASSERT_TRUE(xpcc::LUDecomposition::decompose(A, &l, &u));
TEST_ASSERT_EQUALS(l[0][0], 1);
TEST_ASSERT_EQUALS(l[0][1], 0);
TEST_ASSERT_EQUALS(l[0][2], 0);
TEST_ASSERT_EQUALS(l[1][0], 0);
TEST_ASSERT_EQUALS(l[1][1], 1);
TEST_ASSERT_EQUALS(l[1][2], 0);
TEST_ASSERT_EQUALS(l[2][0], 3);
TEST_ASSERT_EQUALS(l[2][1], -2);
TEST_ASSERT_EQUALS(l[2][2], 1);

TEST_ASSERT_EQUALS(u[0][0], 1);
TEST_ASSERT_EQUALS(u[0][1], 2);
TEST_ASSERT_EQUALS(u[0][2], 3);
TEST_ASSERT_EQUALS(u[1][0], 0);
TEST_ASSERT_EQUALS(u[1][1], 1);
TEST_ASSERT_EQUALS(u[1][2], 2);
TEST_ASSERT_EQUALS(u[2][0], 0);
TEST_ASSERT_EQUALS(u[2][1], 0);
TEST_ASSERT_EQUALS(u[2][2], 1);

// test if the inverse is calculated
xpcc::Matrix<float, 3, 3> AI = xpcc::Matrix<float, 3, 3>::identityMatrix();
TEST_ASSERT_TRUE(xpcc::LUDecomposition::solve(l, u, &AI));

// test if (inverse of A) * (A) = E
TEST_ASSERT_TRUE(A * AI == (xpcc::Matrix<float, 3, 3>::identityMatrix()));

// test Ax=b
// https://www.wolframalpha.com/input/?i=solve%7B%7B1,2,3%7D,%7B0,1,2%7D,%7B3,4,6%7D%7D+%7B%7Bx%7D,%7By%7D,%7Bz%7D%7D%3D+%7B%7B0%7D,%7B1%7D,%7B2%7D%7D
const float n[] = {
0.f,
1.f,
2.f
};
xpcc::Matrix<float, 3, 1> b(n);
TEST_ASSERT_TRUE(xpcc::LUDecomposition::solve(l, u, &b));
TEST_ASSERT_EQUALS(b[0][0], 2);
TEST_ASSERT_EQUALS(b[1][0], -7);
TEST_ASSERT_EQUALS(b[2][0], 4);

b = xpcc::Matrix<float, 3, 1>(n);
TEST_ASSERT_TRUE(xpcc::LUDecomposition::solve(A, &b));
TEST_ASSERT_EQUALS(b[0][0], 2.f);
TEST_ASSERT_EQUALS(b[1][0], -7.f);
TEST_ASSERT_EQUALS(b[2][0], 4.f);
}

Loading

0 comments on commit c4ed672

Please sign in to comment.