From 048ff568906bebc068fd24707233ec3d253ffdff Mon Sep 17 00:00:00 2001 From: TSC21 Date: Thu, 12 Jul 2018 17:50:32 +0100 Subject: [PATCH] lpe: add vehicle_odometry and data validation handlers; improve inout interface --- .../BlockLocalPositionEstimator.cpp | 121 +++++++++++++----- .../BlockLocalPositionEstimator.hpp | 28 +++- .../local_position_estimator/sensors/flow.cpp | 9 +- .../local_position_estimator/sensors/gps.cpp | 8 +- .../sensors/lidar.cpp | 5 +- .../sensors/mocap.cpp | 85 +++++++++--- .../sensors/sonar.cpp | 5 +- .../sensors/vision.cpp | 89 +++++++++---- 8 files changed, 260 insertions(+), 90 deletions(-) diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 77bba1df30..d14777b048 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -1,5 +1,4 @@ #include "BlockLocalPositionEstimator.hpp" -#include #include #include #include @@ -35,9 +34,9 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : // gps 10 hz _sub_gps(ORB_ID(vehicle_gps_position), 1000 / 10, 0, &getSubscriptions()), // vision 50 hz - _sub_vision_pos(ORB_ID(vehicle_vision_position), 1000 / 50, 0, &getSubscriptions()), + _sub_visual_odom(ORB_ID(vehicle_visual_odometry), 1000 / 50, 0, &getSubscriptions()), // mocap 50 hz - _sub_mocap(ORB_ID(att_pos_mocap), 1000 / 50, 0, &getSubscriptions()), + _sub_mocap_odom(ORB_ID(vehicle_mocap_odometry), 1000 / 50, 0, &getSubscriptions()), // all distance sensors, 10 hz _sub_dist0(ORB_ID(distance_sensor), 1000 / 10, 0, &getSubscriptions()), _sub_dist1(ORB_ID(distance_sensor), 1000 / 10, 1, &getSubscriptions()), @@ -121,7 +120,26 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _lidarUpdated(false), _sonarUpdated(false), _landUpdated(false), - _baroUpdated(false) + _baroUpdated(false), + + // sensor validation flags + _vision_xy_valid(false), + _vision_z_valid(false), + _mocap_xy_valid(false), + _mocap_z_valid(false), + + // sensor std deviations + _vision_eph(0.0), + _vision_epv(0.0), + _mocap_eph(0.0), + _mocap_epv(0.0), + + // local to global coversion related variables + _is_global_cov_init(false), + _global_ref_timestamp(0.0), + _ref_lat(0.0), + _ref_lon(0.0), + _ref_alt(0.0) { // assign distance subs to array _dist_subs[0] = &_sub_dist0; @@ -257,8 +275,8 @@ void BlockLocalPositionEstimator::update() _flowUpdated = (_fusion.get() & FUSE_FLOW) && _sub_flow.updated(); _gpsUpdated = (_fusion.get() & FUSE_GPS) && _sub_gps.updated(); - _visionUpdated = (_fusion.get() & FUSE_VIS_POS) && _sub_vision_pos.updated(); - _mocapUpdated = _sub_mocap.updated(); + _visionUpdated = (_fusion.get() & FUSE_VIS_POS) && _sub_visual_odom.updated(); + _mocapUpdated = _sub_mocap_odom.updated(); _lidarUpdated = (_sub_lidar != nullptr) && _sub_lidar->updated(); _sonarUpdated = (_sub_sonar != nullptr) && _sub_sonar->updated(); _landUpdated = landed() && ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE);// throttle rate @@ -349,7 +367,7 @@ void BlockLocalPositionEstimator::update() // reinitialize x if necessary bool reinit_x = false; - for (int i = 0; i < n_x; i++) { + for (size_t i = 0; i < n_x; i++) { // should we do a reinit // of sensors here? // don't want it to take too long @@ -361,7 +379,7 @@ void BlockLocalPositionEstimator::update() } if (reinit_x) { - for (int i = 0; i < n_x; i++) { + for (size_t i = 0; i < n_x; i++) { _x(i) = 0; } @@ -371,8 +389,8 @@ void BlockLocalPositionEstimator::update() // force P symmetry and reinitialize P if necessary bool reinit_P = false; - for (int i = 0; i < n_x; i++) { - for (int j = 0; j <= i; j++) { + for (size_t i = 0; i < n_x; i++) { + for (size_t j = 0; j <= i; j++) { if (!PX4_ISFINITE(_P(i, j))) { mavlink_and_console_log_info(&mavlink_log_pub, "%sreinit P (%d, %d) not finite", msg_label, i, j); @@ -542,13 +560,15 @@ void BlockLocalPositionEstimator::publishLocalPos() const Vector &xLP = _xLowPass.getState(); // lie about eph/epv to allow visual odometry only navigation when velocity est. good - float vxy_stddev = sqrtf(_P(X_vx, X_vx) + _P(X_vy, X_vy)); - float epv = sqrtf(_P(X_z, X_z)); + float evh = sqrtf(_P(X_vx, X_vx) + _P(X_vy, X_vy)); + float evv = sqrtf(_P(X_vz, X_vz)); float eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y)); + float epv = sqrtf(_P(X_z, X_z)); + float eph_thresh = 3.0f; float epv_thresh = 3.0f; - if (vxy_stddev < _vxy_pub_thresh.get()) { + if (evh < _vxy_pub_thresh.get()) { if (eph > eph_thresh) { eph = eph_thresh; } @@ -563,10 +583,12 @@ void BlockLocalPositionEstimator::publishLocalPos() PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy)) && PX4_ISFINITE(_x(X_vz))) { _pub_lpos.get().timestamp = _timeStamp; + _pub_lpos.get().xy_valid = _estimatorInitialized & EST_XY; _pub_lpos.get().z_valid = _estimatorInitialized & EST_Z; _pub_lpos.get().v_xy_valid = _estimatorInitialized & EST_XY; _pub_lpos.get().v_z_valid = _estimatorInitialized & EST_Z; + _pub_lpos.get().x = xLP(X_x); // north _pub_lpos.get().y = xLP(X_y); // east @@ -577,14 +599,19 @@ void BlockLocalPositionEstimator::publishLocalPos() _pub_lpos.get().z = xLP(X_z); // down } - _pub_lpos.get().vx = xLP(X_vx); // north - _pub_lpos.get().vy = xLP(X_vy); // east - _pub_lpos.get().vz = xLP(X_vz); // down + _pub_gpos.get().yaw = matrix::Eulerf(_q).psi(); + + _pub_lpos.get().vx = xLP(X_vx); // north + _pub_lpos.get().vy = xLP(X_vy); // east + _pub_lpos.get().vz = xLP(X_vz); // down // this estimator does not provide a separate vertical position time derivative estimate, so use the vertical velocity _pub_lpos.get().z_deriv = xLP(X_vz); - _pub_lpos.get().yaw = _eul(2); + _pub_lpos.get().ax = _u(U_ax); // north + _pub_lpos.get().ay = _u(U_ay); // east + _pub_lpos.get().az = _u(U_az); // down + _pub_lpos.get().xy_global = _estimatorInitialized & EST_XY; _pub_lpos.get().z_global = !(_sensorTimeout & SENSOR_BARO) && _altOriginGlobal; _pub_lpos.get().ref_timestamp = _time_origin; @@ -600,15 +627,13 @@ void BlockLocalPositionEstimator::publishLocalPos() _pub_lpos.get().dist_bottom_valid = _estimatorInitialized & EST_Z; _pub_lpos.get().eph = eph; _pub_lpos.get().epv = epv; - _pub_lpos.update(); - //TODO provide calculated values for these - _pub_lpos.get().evh = 0.0f; - _pub_lpos.get().evv = 0.0f; + _pub_lpos.get().evh = evh; + _pub_lpos.get().evv = evv; _pub_lpos.get().vxy_max = INFINITY; _pub_lpos.get().vz_max = INFINITY; _pub_lpos.get().hagl_min = INFINITY; _pub_lpos.get().hagl_max = INFINITY; - + _pub_lpos.update(); } } @@ -616,11 +641,40 @@ void BlockLocalPositionEstimator::publishEstimatorStatus() { _pub_est_status.get().timestamp = _timeStamp; - for (int i = 0; i < n_x; i++) { + for (size_t i = 0; i < n_x; i++) { _pub_est_status.get().states[i] = _x(i); - _pub_est_status.get().covariances[i] = _P(i, i); } + // matching EKF2 covariances indexing + // quaternion - not determined, as it is a position estimator + _pub_est_status.get().covariances[0] = NAN; + _pub_est_status.get().covariances[1] = NAN; + _pub_est_status.get().covariances[2] = NAN; + _pub_est_status.get().covariances[3] = NAN; + // linear velocity + _pub_est_status.get().covariances[4] = _P(X_vx, X_vx); + _pub_est_status.get().covariances[5] = _P(X_vy, X_vy); + _pub_est_status.get().covariances[6] = _P(X_vz, X_vz); + // position + _pub_est_status.get().covariances[7] = _P(X_x, X_x); + _pub_est_status.get().covariances[8] = _P(X_y, X_y); + _pub_est_status.get().covariances[9] = _P(X_z, X_z); + // gyro bias - not determined + _pub_est_status.get().covariances[10] = NAN; + _pub_est_status.get().covariances[11] = NAN; + _pub_est_status.get().covariances[12] = NAN; + // accel bias + _pub_est_status.get().covariances[13] = _P(X_bx, X_bx); + _pub_est_status.get().covariances[14] = _P(X_by, X_by); + _pub_est_status.get().covariances[15] = _P(X_bz, X_bz); + // mag - not determined + for (size_t i = 16; i <= 21; i++) { + _pub_est_status.get().covariances[i] = NAN; + } + // replacing the hor wind cov with terrain altitude covariance + _pub_est_status.get().covariances[22] = _P(X_tz, X_tz); + _pub_est_status.get().covariances[23] = NAN; + _pub_est_status.get().n_states = n_x; _pub_est_status.get().health_flags = _sensorFault; _pub_est_status.get().timeout_flags = _sensorTimeout; @@ -640,13 +694,14 @@ void BlockLocalPositionEstimator::publishGlobalPos() float alt = -xLP(X_z) + _altOrigin; // lie about eph/epv to allow visual odometry only navigation when velocity est. good - float vxy_stddev = sqrtf(_P(X_vx, X_vx) + _P(X_vy, X_vy)); - float epv = sqrtf(_P(X_z, X_z)); + float evh = sqrtf(_P(X_vx, X_vx) + _P(X_vy, X_vy)); float eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y)); + float epv = sqrtf(_P(X_z, X_z)); + float eph_thresh = 3.0f; float epv_thresh = 3.0f; - if (vxy_stddev < _vxy_pub_thresh.get()) { + if (evh < _vxy_pub_thresh.get()) { if (eph > eph_thresh) { eph = eph_thresh; } @@ -666,8 +721,7 @@ void BlockLocalPositionEstimator::publishGlobalPos() _pub_gpos.get().vel_n = xLP(X_vx); _pub_gpos.get().vel_e = xLP(X_vy); _pub_gpos.get().vel_d = xLP(X_vz); - - _pub_gpos.get().yaw = _eul(2); + _pub_gpos.get().yaw = matrix::Eulerf(_q).psi(); _pub_gpos.get().eph = eph; _pub_gpos.get().epv = epv; _pub_gpos.get().terrain_alt = _altOrigin - xLP(X_tz); @@ -771,9 +825,8 @@ void BlockLocalPositionEstimator::updateSSParams() void BlockLocalPositionEstimator::predict() { // get acceleration - matrix::Quatf q(&_sub_att.get().q[0]); - _eul = matrix::Euler(q); - _R_att = matrix::Dcm(q); + _q = matrix::Quatf(&_sub_att.get().q[0]); + _R_att = matrix::Dcm(_q); Vector3f a(_sub_sensor.get().accelerometer_m_s2); // note, bias is removed in dynamics function _u = _R_att * a; @@ -838,12 +891,12 @@ void BlockLocalPositionEstimator::predict() _B * _R * _B.transpose() + _Q) * getDt(); // covariance propagation logic - for (int i = 0; i < n_x; i++) { + for (size_t i = 0; i < n_x; i++) { if (_P(i, i) > P_MAX) { // if diagonal element greater than max, stop propagating dP(i, i) = 0; - for (int j = 0; j < n_x; j++) { + for (size_t j = 0; j < n_x; j++) { dP(i, j) = 0; dP(j, i) = 0; } diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 3d4f797040..7cdd7ed298 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include #include @@ -21,9 +22,9 @@ #include #include #include -#include #include #include +#include // uORB Publications #include @@ -251,8 +252,8 @@ private: uORB::Subscription _sub_sensor; uORB::Subscription _sub_param_update; uORB::Subscription _sub_gps; - uORB::Subscription _sub_vision_pos; - uORB::Subscription _sub_mocap; + uORB::Subscription _sub_visual_odom; + uORB::Subscription _sub_mocap_odom; uORB::Subscription _sub_dist0; uORB::Subscription _sub_dist1; uORB::Subscription _sub_dist2; @@ -411,13 +412,32 @@ private: bool _landUpdated; bool _baroUpdated; + // sensor validation flags + bool _vision_xy_valid; + bool _vision_z_valid; + bool _mocap_xy_valid; + bool _mocap_z_valid; + + // sensor std deviations + float _vision_eph; + float _vision_epv; + float _mocap_eph; + float _mocap_epv; + + // local to global coversion related variables + bool _is_global_cov_init; + uint64_t _global_ref_timestamp; + double _ref_lat; + double _ref_lon; + float _ref_alt; + // state space Vector _x; // state vector Vector _u; // input vector Matrix _P; // state covariance matrix + matrix::Quatf _q; matrix::Dcm _R_att; - Vector3f _eul; Matrix _A; // dynamics matrix Matrix _B; // input matrix diff --git a/src/modules/local_position_estimator/sensors/flow.cpp b/src/modules/local_position_estimator/sensors/flow.cpp index 80588e18fd..2475e22f07 100644 --- a/src/modules/local_position_estimator/sensors/flow.cpp +++ b/src/modules/local_position_estimator/sensors/flow.cpp @@ -36,8 +36,10 @@ void BlockLocalPositionEstimator::flowInit() int BlockLocalPositionEstimator::flowMeasure(Vector &y) { + matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q)); + // check for sane pitch/roll - if (_eul(0) > 0.5f || _eul(1) > 0.5f) { + if (euler.phi() > 0.5f || euler.theta() > 0.5f) { return -1; } @@ -58,8 +60,6 @@ int BlockLocalPositionEstimator::flowMeasure(Vector &y) return -1; } - matrix::Eulerf euler = matrix::Quatf(_sub_att.get().q); - float d = agl() * cosf(euler.phi()) * cosf(euler.theta()); // optical flow in x, y axis @@ -159,7 +159,8 @@ void BlockLocalPositionEstimator::flowCorrect() + _sub_att.get().pitchspeed * _sub_att.get().pitchspeed + _sub_att.get().yawspeed * _sub_att.get().yawspeed; - float rot_sq = _eul(0) * _eul(0) + _eul(1) * _eul(1); + matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q)); + float rot_sq = euler.phi() * euler.phi() + euler.theta() * euler.theta(); R(Y_flow_vx, Y_flow_vx) = flow_vxy_stddev * flow_vxy_stddev + _flow_r.get() * _flow_r.get() * rot_sq + diff --git a/src/modules/local_position_estimator/sensors/gps.cpp b/src/modules/local_position_estimator/sensors/gps.cpp index 985b4728f7..12eb23d4b8 100644 --- a/src/modules/local_position_estimator/sensors/gps.cpp +++ b/src/modules/local_position_estimator/sensors/gps.cpp @@ -113,9 +113,9 @@ void BlockLocalPositionEstimator::gpsCorrect() if (gpsMeasure(y_global) != OK) { return; } // gps measurement in local frame - double lat = y_global(Y_gps_x); - double lon = y_global(Y_gps_y); - float alt = y_global(Y_gps_z); + double lat = y_global(Y_gps_x); + double lon = y_global(Y_gps_y); + float alt = y_global(Y_gps_z); float px = 0; float py = 0; float pz = -(alt - _gpsAltOrigin); @@ -189,7 +189,7 @@ void BlockLocalPositionEstimator::gpsCorrect() Matrix S = C * _P * C.transpose() + R; // publish innovations - for (int i = 0; i < 6; i++) { + for (size_t i = 0; i < 6; i++) { _pub_innov.get().vel_pos_innov[i] = r(i); _pub_innov.get().vel_pos_innov_var[i] = S(i, i); } diff --git a/src/modules/local_position_estimator/sensors/lidar.cpp b/src/modules/local_position_estimator/sensors/lidar.cpp index f455cded0b..a1478bde25 100644 --- a/src/modules/local_position_estimator/sensors/lidar.cpp +++ b/src/modules/local_position_estimator/sensors/lidar.cpp @@ -52,9 +52,10 @@ int BlockLocalPositionEstimator::lidarMeasure(Vector &y) _lidarStats.update(Scalarf(d)); _time_last_lidar = _timeStamp; y.setZero(); + matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q)); y(0) = (d + _lidar_z_offset.get()) * - cosf(_eul(0)) * - cosf(_eul(1)); + cosf(euler.phi()) * + cosf(euler.theta()); return OK; } diff --git a/src/modules/local_position_estimator/sensors/mocap.cpp b/src/modules/local_position_estimator/sensors/mocap.cpp index af5fd9137c..98f80f5b81 100644 --- a/src/modules/local_position_estimator/sensors/mocap.cpp +++ b/src/modules/local_position_estimator/sensors/mocap.cpp @@ -9,6 +9,10 @@ extern orb_advert_t mavlink_log_pub; static const uint32_t REQ_MOCAP_INIT_COUNT = 20; static const uint32_t MOCAP_TIMEOUT = 200000; // 0.2 s +// set pose/velocity as invalid if standard deviation is bigger than EP_MAX_STD_DEV +// TODO: the user should be allowed to set these values by a parameter +static constexpr float EP_MAX_STD_DEV = 100.0f; + void BlockLocalPositionEstimator::mocapInit() { // measure @@ -32,23 +36,55 @@ void BlockLocalPositionEstimator::mocapInit() _sensorTimeout &= ~SENSOR_MOCAP; _sensorFault &= ~SENSOR_MOCAP; + // get reference for global position + globallocalconverter_getref(&_ref_lat, &_ref_lon, &_ref_alt); + _global_ref_timestamp = hrt_absolute_time(); + _is_global_cov_init = globallocalconverter_initialized(); + + if (!_map_ref.init_done && _is_global_cov_init && !FUSE_VIS_POS) { + // initialize global origin using the mocap estimator reference (only if the vision estimation is not being fused as well) + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (mocap) : lat %6.2f lon %6.2f alt %5.1f m", + double(_ref_lat), double(_ref_lon), double(_ref_alt)); + map_projection_init(&_map_ref, _ref_lat, _ref_lon); + // set timestamp when origin was set to current time + _time_origin = _timeStamp; + } + if (!_altOriginInitialized) { _altOriginInitialized = true; - _altOriginGlobal = false; - _altOrigin = 0; + _altOriginGlobal = true; + _altOrigin = globallocalconverter_initialized() ? _ref_alt : 0.0f; } } } int BlockLocalPositionEstimator::mocapMeasure(Vector &y) { - y.setZero(); - y(Y_mocap_x) = _sub_mocap.get().x; - y(Y_mocap_y) = _sub_mocap.get().y; - y(Y_mocap_z) = _sub_mocap.get().z; - _mocapStats.update(y); - _time_last_mocap = _sub_mocap.get().timestamp; - return OK; + if (!PX4_ISFINITE(_sub_mocap_odom.get().pose_covariance[0])) { + // check if the mocap data is valid based on the covariances + _mocap_eph = sqrtf(fmaxf(_sub_mocap_odom.get().pose_covariance[0], _sub_mocap_odom.get().pose_covariance[6])); + _mocap_epv = sqrtf(_sub_mocap_odom.get().pose_covariance[11]); + _mocap_xy_valid = _mocap_eph <= EP_MAX_STD_DEV; + _mocap_z_valid = _mocap_epv <= EP_MAX_STD_DEV; + } else { + // if we don't have covariances, assume every reading + _mocap_xy_valid = true; + _mocap_z_valid = true; + } + + if (!_mocap_xy_valid || !_mocap_z_valid) { + _time_last_mocap = _sub_mocap_odom.get().timestamp; + return !OK; + + } else { + y.setZero(); + y(Y_mocap_x) = _sub_mocap_odom.get().x; + y(Y_mocap_y) = _sub_mocap_odom.get().y; + y(Y_mocap_z) = _sub_mocap_odom.get().z; + _mocapStats.update(y); + _time_last_mocap = _sub_mocap_odom.get().timestamp; + return OK; + } } void BlockLocalPositionEstimator::mocapCorrect() @@ -56,7 +92,10 @@ void BlockLocalPositionEstimator::mocapCorrect() // measure Vector y; - if (mocapMeasure(y) != OK) { return; } + if (mocapMeasure(y) != OK) { + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap data invalid. eph: %f epv: %f", _mocap_eph, _mocap_epv); + return; + } // mocap measurement matrix, measures position Matrix C; @@ -68,11 +107,23 @@ void BlockLocalPositionEstimator::mocapCorrect() // noise matrix Matrix R; R.setZero(); - float mocap_p_var = _mocap_p_stddev.get() * \ - _mocap_p_stddev.get(); - R(Y_mocap_x, Y_mocap_x) = mocap_p_var; - R(Y_mocap_y, Y_mocap_y) = mocap_p_var; - R(Y_mocap_z, Y_mocap_z) = mocap_p_var; + + // use std dev from mocap data if available + if (_mocap_eph > _mocap_p_stddev.get()) { + R(Y_mocap_x, Y_mocap_x) = _mocap_eph * _mocap_eph; + R(Y_mocap_y, Y_mocap_y) = _mocap_eph * _mocap_eph; + + } else { + R(Y_mocap_x, Y_mocap_x) = _mocap_p_stddev.get() * _mocap_p_stddev.get(); + R(Y_mocap_y, Y_mocap_y) = _mocap_p_stddev.get() * _mocap_p_stddev.get(); + } + + if (_mocap_epv > _mocap_p_stddev.get()) { + R(Y_mocap_z, Y_mocap_z) = _mocap_epv * _mocap_epv; + + } else { + R(Y_mocap_z, Y_mocap_z) = _mocap_p_stddev.get() * _mocap_p_stddev.get(); + } // residual Vector r = y - C * _x; @@ -80,12 +131,12 @@ void BlockLocalPositionEstimator::mocapCorrect() Matrix S = C * _P * C.transpose() + R; // publish innovations - for (int i = 0; i < 3; i++) { + for (size_t i = 0; i < 3; i++) { _pub_innov.get().vel_pos_innov[i] = r(i); _pub_innov.get().vel_pos_innov_var[i] = S(i, i); } - for (int i = 3; i < 6; i++) { + for (size_t i = 3; i < 6; i++) { _pub_innov.get().vel_pos_innov[i] = 0; _pub_innov.get().vel_pos_innov_var[i] = 1; } diff --git a/src/modules/local_position_estimator/sensors/sonar.cpp b/src/modules/local_position_estimator/sensors/sonar.cpp index 04bd043611..79c46f72e1 100644 --- a/src/modules/local_position_estimator/sensors/sonar.cpp +++ b/src/modules/local_position_estimator/sensors/sonar.cpp @@ -66,9 +66,10 @@ int BlockLocalPositionEstimator::sonarMeasure(Vector &y) _sonarStats.update(Scalarf(d)); _time_last_sonar = _timeStamp; y.setZero(); + matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q)); y(0) = (d + _sonar_z_offset.get()) * - cosf(_eul(0)) * - cosf(_eul(1)); + cosf(euler.phi()) * + cosf(euler.theta()); return OK; } diff --git a/src/modules/local_position_estimator/sensors/vision.cpp b/src/modules/local_position_estimator/sensors/vision.cpp index 7299499f91..cdc84280e2 100644 --- a/src/modules/local_position_estimator/sensors/vision.cpp +++ b/src/modules/local_position_estimator/sensors/vision.cpp @@ -14,6 +14,10 @@ static const uint32_t REQ_VISION_INIT_COUNT = 1; // set the timeout to 0.5 seconds static const uint32_t VISION_TIMEOUT = 500000; // 0.5 s +// set pose/velocity as invalid if standard deviation is bigger than EP_MAX_STD_DEV +// TODO: the user should be allowed to set these values by a parameter +static constexpr float EP_MAX_STD_DEV = 100.0f; + void BlockLocalPositionEstimator::visionInit() { // measure @@ -37,11 +41,16 @@ void BlockLocalPositionEstimator::visionInit() _sensorTimeout &= ~SENSOR_VISION; _sensorFault &= ~SENSOR_VISION; - if (!_map_ref.init_done && _sub_vision_pos.get().xy_global) { + // get reference for global position + globallocalconverter_getref(&_ref_lat, &_ref_lon, &_ref_alt); + _global_ref_timestamp = hrt_absolute_time(); + _is_global_cov_init = globallocalconverter_initialized(); + + if (!_map_ref.init_done && _is_global_cov_init) { // initialize global origin using the visual estimator reference mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (vision) : lat %6.2f lon %6.2f alt %5.1f m", - double(_sub_vision_pos.get().ref_lat), double(_sub_vision_pos.get().ref_lon), double(_sub_vision_pos.get().ref_alt)); - map_projection_init(&_map_ref, _sub_vision_pos.get().ref_lat, _sub_vision_pos.get().ref_lon); + double(_ref_lat), double(_ref_lon), double(_ref_alt)); + map_projection_init(&_map_ref, _ref_lat, _ref_lon); // set timestamp when origin was set to current time _time_origin = _timeStamp; } @@ -49,20 +58,38 @@ void BlockLocalPositionEstimator::visionInit() if (!_altOriginInitialized) { _altOriginInitialized = true; _altOriginGlobal = true; - _altOrigin = _sub_vision_pos.get().z_global ? _sub_vision_pos.get().ref_alt : 0.0f; + _altOrigin = globallocalconverter_initialized() ? _ref_alt : 0.0f; } } } int BlockLocalPositionEstimator::visionMeasure(Vector &y) { - y.setZero(); - y(Y_vision_x) = _sub_vision_pos.get().x; - y(Y_vision_y) = _sub_vision_pos.get().y; - y(Y_vision_z) = _sub_vision_pos.get().z; - _visionStats.update(y); - _time_last_vision_p = _sub_vision_pos.get().timestamp; - return OK; + if (!PX4_ISFINITE(_sub_visual_odom.get().pose_covariance[0])) { + // check if the vision data is valid based on the covariances + _vision_eph = sqrtf(fmaxf(_sub_visual_odom.get().pose_covariance[0], _sub_visual_odom.get().pose_covariance[6])); + _vision_epv = sqrtf(_sub_visual_odom.get().pose_covariance[11]); + _vision_xy_valid = _vision_eph <= EP_MAX_STD_DEV; + _vision_z_valid = _vision_epv <= EP_MAX_STD_DEV; + } else { + // if we don't have covariances, assume every reading + _vision_xy_valid = true; + _vision_z_valid = true; + } + + if (!_vision_xy_valid || !_vision_z_valid) { + _time_last_vision_p = _sub_visual_odom.get().timestamp; + return !OK; + + } else { + y.setZero(); + y(Y_vision_x) = _sub_visual_odom.get().x; + y(Y_vision_y) = _sub_visual_odom.get().y; + y(Y_vision_z) = _sub_visual_odom.get().z; + _visionStats.update(y); + _time_last_vision_p = _sub_visual_odom.get().timestamp; + return OK; + } } void BlockLocalPositionEstimator::visionCorrect() @@ -70,7 +97,10 @@ void BlockLocalPositionEstimator::visionCorrect() // measure Vector y; - if (visionMeasure(y) != OK) { return; } + if (visionMeasure(y) != OK) { + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision data invalid. eph: %f epv: %f", _vision_eph, _vision_epv); + return; + } // vision measurement matrix, measures position Matrix C; @@ -83,18 +113,18 @@ void BlockLocalPositionEstimator::visionCorrect() Matrix R; R.setZero(); - // use error estimates from vision topic if available - if (_sub_vision_pos.get().eph > _vision_xy_stddev.get()) { - R(Y_vision_x, Y_vision_x) = _sub_vision_pos.get().eph * _sub_vision_pos.get().eph; - R(Y_vision_y, Y_vision_y) = _sub_vision_pos.get().eph * _sub_vision_pos.get().eph; + // use std dev from vision data if available + if (_vision_eph > _vision_xy_stddev.get()) { + R(Y_vision_x, Y_vision_x) = _vision_eph * _vision_eph; + R(Y_vision_y, Y_vision_y) = _vision_eph * _vision_eph; } else { R(Y_vision_x, Y_vision_x) = _vision_xy_stddev.get() * _vision_xy_stddev.get(); R(Y_vision_y, Y_vision_y) = _vision_xy_stddev.get() * _vision_xy_stddev.get(); } - if (_sub_vision_pos.get().epv > _vision_z_stddev.get()) { - R(Y_vision_z, Y_vision_z) = _sub_vision_pos.get().epv * _sub_vision_pos.get().epv; + if (_vision_epv > _vision_z_stddev.get()) { + R(Y_vision_z, Y_vision_z) = _vision_epv * _vision_epv; } else { R(Y_vision_z, Y_vision_z) = _vision_z_stddev.get() * _vision_z_stddev.get(); @@ -103,20 +133,33 @@ void BlockLocalPositionEstimator::visionCorrect() // vision delayed x uint8_t i_hist = 0; - float vision_delay = (_timeStamp - _sub_vision_pos.get().timestamp) * 1e-6f; // measurement delay in seconds + float vision_delay = (_timeStamp - _sub_visual_odom.get().timestamp) * 1e-6f; // measurement delay in seconds if (vision_delay < 0.0f) { vision_delay = 0.0f; } // use auto-calculated delay from measurement if parameter is set to zero - if (getDelayPeriods(_vision_delay.get() > 0.0f ? _vision_delay.get() : vision_delay, &i_hist) < 0) { return; } - - //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision delay : %0.2f ms", double(vision_delay * 1000.0f)); + if (getDelayPeriods(_vision_delay.get() > 0.0f ? _vision_delay.get() : vision_delay, &i_hist) < 0){ return; } Vector x0 = _xDelay.get(i_hist); // residual - Matrix S_I = inv((C * _P * C.transpose()) + R); Matrix r = y - C * x0; + // residual covariance + Matrix S = C * _P * C.transpose() + R; + + // publish innovations + for (size_t i = 0; i < 3; i++) { + _pub_innov.get().vel_pos_innov[i] = r(i, 0); + _pub_innov.get().vel_pos_innov_var[i] = S(i, i); + } + + for (size_t i = 3; i < 6; i++) { + _pub_innov.get().vel_pos_innov[i] = 0; + _pub_innov.get().vel_pos_innov_var[i] = 1; + } + + // residual covariance, (inverse) + Matrix S_I = inv(S); // fault detection float beta = (r.transpose() * (S_I * r))(0, 0);