From 2719789b2e773f4d4dfc347649d1bb71bc690b57 Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 7 Nov 2015 11:41:15 +0100 Subject: [PATCH] use matrix lib to enable building for posix --- .../terrain_estimation/terrain_estimator.cpp | 54 +++++++++---------- .../terrain_estimation/terrain_estimator.h | 5 +- 2 files changed, 30 insertions(+), 29 deletions(-) diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp index 16ab8de4a0..a92bbb136e 100644 --- a/src/lib/terrain_estimation/terrain_estimator.cpp +++ b/src/lib/terrain_estimation/terrain_estimator.cpp @@ -46,9 +46,9 @@ TerrainEstimator::TerrainEstimator() : _time_last_distance(0), _time_last_gps(0) { - _x.zero(); + memset(&_x._data[0], 0, sizeof(_x._data)); _u_z = 0.0f; - _P.identity(); + _P.setIdentity(); } bool TerrainEstimator::is_distance_valid(float distance) { @@ -63,9 +63,9 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu const struct distance_sensor_s *distance) { if (attitude->R_valid) { - math::Matrix<3, 3> R_att(attitude->R); - math::Vector<3> a(&sensor->accelerometer_m_s2[0]); - math::Vector<3> u; + matrix::Matrix R_att(attitude->R); + matrix::Vector a(&sensor->accelerometer_m_s2[0]); + matrix::Vector u; u = R_att * a; _u_z = u(2) + 9.81f; // compensate for gravity @@ -74,32 +74,32 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu } // dynamics matrix - math::Matrix A; - A.zero(); + matrix::Matrix A; + A.setZero(); A(0,1) = 1; A(1,2) = 1; // input matrix - math::Matrix B; - B.zero(); + matrix::Matrix B; + B.setZero(); B(1,0) = 1; // input noise variance float R = 0.135f; // process noise convariance - math::Matrix Q; + matrix::Matrix Q; Q(0,0) = 0; Q(1,1) = 0; // do prediction - math::Vector dx = (A * _x) * dt; + matrix::Vector dx = (A * _x) * dt; dx(1) += B(1,0) * _u_z * dt; // propagate state and covariance matrix _x += dx; - _P += (A * _P + _P * A.transposed() + - B * R * B.transposed() + Q) * dt; + _P += (A * _P + _P * A.transpose() + + B * R * B.transpose() + Q) * dt; } void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance, @@ -114,21 +114,21 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl float d = distance->current_distance; - math::Matrix<1, n_x> C; + matrix::Matrix C; C(0, 0) = -1; // measured altitude, float R = 0.009f; - math::Vector<1> y; + matrix::Vector y; y(0) = d * cosf(attitude->roll) * cosf(attitude->pitch); // residual - math::Matrix<1, 1> S_I = (C * _P * C.transposed()); + matrix::Matrix S_I = (C * _P * C.transpose()); S_I(0,0) += R; - S_I = S_I.inversed(); - math::Vector<1> r = y - C * _x; + S_I = matrix::inv (S_I); + matrix::Vector r = y - C * _x; - math::Matrix K = _P * C.transposed() * S_I; + matrix::Matrix K = _P * C.transpose() * S_I; // some sort of outlayer rejection if (fabsf(distance->current_distance - _distance_last) < 1.0f) { @@ -149,21 +149,21 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl } if (gps->timestamp_position > _time_last_gps && gps->fix_type >= 3) { - math::Matrix<1, n_x> C; + matrix::Matrix C; C(0,1) = 1; float R = 0.056f; - math::Vector<1> y; + matrix::Vector y; y(0) = gps->vel_d_m_s; // residual - math::Matrix<1, 1> S_I = (C * _P * C.transposed()); + matrix::Matrix S_I = (C * _P * C.transpose()); S_I(0,0) += R; - S_I = S_I.inversed(); - math::Vector<1> r = y - C * _x; + S_I = matrix::inv(S_I); + matrix::Vector r = y - C * _x; - math::Matrix K = _P * C.transposed() * S_I; + matrix::Matrix K = _P * C.transpose() * S_I; _x += K * r; _P -= K * C * _P; @@ -187,8 +187,8 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl } if (reinit) { - _x.zero(); - _P.zero(); + memset(&_x._data[0], 0, sizeof(_x._data)); + _P.setZero(); _P(0,0) = _P(1,1) = _P(2,2) = 0.1f; } diff --git a/src/lib/terrain_estimation/terrain_estimator.h b/src/lib/terrain_estimation/terrain_estimator.h index fc10cb8d63..697a053b88 100644 --- a/src/lib/terrain_estimation/terrain_estimator.h +++ b/src/lib/terrain_estimation/terrain_estimator.h @@ -36,6 +36,7 @@ */ #include +#include "matrix/Matrix.hpp" #include #include #include @@ -75,9 +76,9 @@ private: bool _terrain_valid; // kalman filter variables - math::Vector _x; // state: ground distance, velocity, accel bias in z direction + matrix::Vector _x; // state: ground distance, velocity, accel bias in z direction float _u_z; // acceleration in earth z direction - math::Matrix<3, 3> _P; // covariance matrix + matrix::Matrix _P; // covariance matrix // timestamps uint64_t _time_last_distance;