fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 04:00:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

sparse quadratic form

Browse Source
This commit is contained in:
Kamil Ritz
2020-08-26 18:23:07 +02:00
committed by Julian Kent
parent 3a5bfb2bd1
commit 13f092a30f
2 changed files with 25 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/SparseVector.hpp
+14
View File
@@ -166,6 +166,20 @@ matrix::Vector<Type, Q> operator*(const matrix::Matrix<Type, Q, M>& mat, const m
return res;
}
// returns x.T * A * x
template<typename Type, size_t M, size_t ... Idxs>
Type quadraticForm(const matrix::SquareMatrix<Type, M>& A, const matrix::SparseVector<Type, M, Idxs...>& x) {
Type res = Type(0);
for (size_t i = 0; i < x.non_zeros(); i++) {
Type tmp = Type(0);
for (size_t j = 0; j < x.non_zeros(); j++) {
tmp += A(x.index(i), x.index(j)) * x.atCompressedIndex(j);
}
res += x.atCompressedIndex(i) * tmp;
}
return res;
}
template<typename Type,size_t M, size_t... Idxs>
constexpr size_t SparseVector<Type, M, Idxs...>::_indices[SparseVector<Type, M, Idxs...>::N];