fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 01:01:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Work on kalman filter interface.

Browse Source
This commit is contained in:
jgoppert
2015-11-12 16:41:07 -05:00
parent 0a110a1b02
commit b9924820e1
3 changed files with 11 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/Quaternion.hpp
+6
View File
@@ -98,6 +98,12 @@ public:
return r; return r;
} }
void operator*=(const Quaternion & other)
{
Quaternion &self = *this;
self = self * other;
}
Matrix41 derivative(const Matrix31 & w) const { Matrix41 derivative(const Matrix31 & w) const {
const Quaternion &q = *this; const Quaternion &q = *this;
Type dataQ[] = { Type dataQ[] = {
matrix/filter.hpp
+5 -5
View File
@@ -6,12 +6,12 @@ namespace matrix {
template<typename Type, size_t M, size_t N> template<typename Type, size_t M, size_t N>
int kalman_correct( int kalman_correct(
const SquareMatrix<Type, M> & P, const Matrix<Type, M, M> & P,
const Matrix<Type, N, M> & C, const Matrix<Type, N, M> & C,
const SquareMatrix<Type, N> & R, const Matrix<Type, N, N> & R,
const Vector<Type, N> &r, const Matrix<Type, N, 1> &r,
Vector<Type, M> & dx, Matrix<Type, M, 1> & dx,
SquareMatrix<Type, M> & dP, Matrix<Type, M, M> & dP,
float & beta float & beta
) )
{ {
test/filter.cpp
-10
View File
@@ -5,16 +5,6 @@
using namespace matrix; using namespace matrix;
template int kalman_correct<float, 6, 5>(
const SquareMatrix<float, 6> & P,
const Matrix<float, 5, 6> & C,
const SquareMatrix<float, 5> & R,
const Vector<float, 5> &r,
Vector<float, 6> & dx,
SquareMatrix<float, 6> & dP,
float & beta
);
int main() int main()
{ {
const size_t n_x = 6; const size_t n_x = 6;