Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 04:03:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: added two new vector/matrix ops for kalman airspeed filter

Browse Source
This commit is contained in:
Andrew Tridgell 2013-07-20 08:13:34 +10:00
parent 2d5deddd30
commit 468e55d425
2 changed files with 31 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_Math/vector3.cpp
View File

@ -272,11 +272,32 @@ float Vector3<T>::angle(const Vector3<T> &v2) const
return acosf(((*this)*v2) / (this->length()*v2.length())); return acosf(((*this)*v2) / (this->length()*v2.length()));
} }
// multiplication of transpose by a vector
template <typename T>
Vector3<T> Vector3<T>::operator *(const Matrix3<T> &m) const
{
return Vector3<T>(*this * m.colx(),
*this * m.coly(),
*this * m.colz());
}
// multiply a column vector by a row vector, returning a 3x3 matrix
template <typename T>
Matrix3<T> Vector3<T>::mul_rowcol(const Vector3<T> &v2) const
{
const Vector3<T> v1 = *this;
return Matrix3<T>(v1.x * v2.x, v1.x * v2.y, v1.x * v2.z,
v1.y * v2.x, v1.y * v2.y, v1.y * v2.z,
v1.z * v2.x, v1.z * v2.y, v1.z * v2.z);
}
// only define for float // only define for float
template void Vector3<float>::rotate(enum Rotation); template void Vector3<float>::rotate(enum Rotation);
template float Vector3<float>::length(void) const; template float Vector3<float>::length(void) const;
template Vector3<float> Vector3<float>::operator %(const Vector3<float> &v) const; template Vector3<float> Vector3<float>::operator %(const Vector3<float> &v) const;
template float Vector3<float>::operator *(const Vector3<float> &v) const; template float Vector3<float>::operator *(const Vector3<float> &v) const;
template Vector3<float> Vector3<float>::operator *(const Matrix3<float> &m) const;
template Matrix3<float> Vector3<float>::mul_rowcol(const Vector3<float> &v) const;
template Vector3<float> &Vector3<float>::operator *=(const float num); template Vector3<float> &Vector3<float>::operator *=(const float num);
template Vector3<float> &Vector3<float>::operator /=(const float num); template Vector3<float> &Vector3<float>::operator /=(const float num);
template Vector3<float> &Vector3<float>::operator -=(const Vector3<float> &v); template Vector3<float> &Vector3<float>::operator -=(const Vector3<float> &v);

10
libraries/AP_Math/vector3.h
View File

@ -44,9 +44,13 @@
#include <math.h> #include <math.h>
#include <string.h> #include <string.h>
template <typename T>
class Matrix3;
template <typename T> template <typename T>
class Vector3 class Vector3
{ {
public: public:
T x, y, z; T x, y, z;
@ -101,6 +105,12 @@ public:
// dot product // dot product
T operator *(const Vector3<T> &v) const; T operator *(const Vector3<T> &v) const;
// multiply a row vector by a matrix, to give a row vector
Vector3<T> operator *(const Matrix3<T> &m) const;
// multiply a column vector by a row vector, returning a 3x3 matrix
Matrix3<T> mul_rowcol(const Vector3<T> &v) const;
// cross product // cross product
Vector3<T> operator %(const Vector3<T> &v) const; Vector3<T> operator %(const Vector3<T> &v) const;