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

Added runge kutta integration.

Browse Source
This commit is contained in:
jgoppert
2015-11-11 23:31:43 -05:00
parent 2c7a375e3d
commit 42f2e60b24
11 changed files with 112 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/Matrix.hpp
+39 -1
View File
@@ -152,6 +152,21 @@ public:
return res;
}
// unary minus
Matrix<Type, M, N> operator-() const
{
Matrix<Type, M, N> res;
const Matrix<Type, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(i , j) = -self(i, j);
}
}
return res;
}
void operator+=(const Matrix<Type, M, N> &other)
{
Matrix<Type, M, N> &self = *this;
@@ -287,6 +302,22 @@ public:
memset(_data, 0, sizeof(_data));
}
void setAll(Type val)
{
Matrix<Type, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
self(i, j) = val;
}
}
}
inline void setOne()
{
setAll(1);
}
void setIdentity()
{
setZero();
@@ -369,12 +400,19 @@ public:
};
template<typename Type, size_t M, size_t N>
Matrix<Type, M, N> zero() {
Matrix<Type, M, N> zeros() {
Matrix<Type, M, N> m;
m.setZero();
return m;
}
template<typename Type, size_t M, size_t N>
Matrix<Type, M, N> ones() {
Matrix<Type, M, N> m;
m.setOne();
return m;
}
typedef Matrix<float, 3, 3> Matrix3f;
}; // namespace matrix