forked from Archive/PX4-Autopilot
lpe: add vehicle_odometry and data validation handlers; improve inout interface
This commit is contained in:
TSC21
Lorenz Meier
parent
7303005373
commit
048ff56890
|
|
@ -1,5 +1,4 @@
|
|||
#include "BlockLocalPositionEstimator.hpp"
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <systemlib/mavlink_log.h>
|
||||
#include <fcntl.h>
|
||||
#include <systemlib/err.h>
|
||||
|
|
@ -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<float, n_x> &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<float>(q);
|
||||
_R_att = matrix::Dcm<float>(q);
|
||||
_q = matrix::Quatf(&_sub_att.get().q[0]);
|
||||
_R_att = matrix::Dcm<float>(_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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,6 +1,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <px4_posix.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <px4_module_params.h>
|
||||
#include <controllib/blocks.hpp>
|
||||
#include <mathlib/mathlib.h>
|
||||
|
|
@ -21,9 +22,9 @@
|
|||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/att_pos_mocap.h>
|
||||
#include <uORB/topics/landing_target_pose.h>
|
||||
#include <uORB/topics/vehicle_air_data.h>
|
||||
#include <uORB/topics/vehicle_odometry.h>
|
||||
|
||||
// uORB Publications
|
||||
#include <uORB/Publication.hpp>
|
||||
|
|
@ -251,8 +252,8 @@ private:
|
|||
uORB::Subscription<sensor_combined_s> _sub_sensor;
|
||||
uORB::Subscription<parameter_update_s> _sub_param_update;
|
||||
uORB::Subscription<vehicle_gps_position_s> _sub_gps;
|
||||
uORB::Subscription<vehicle_local_position_s> _sub_vision_pos;
|
||||
uORB::Subscription<att_pos_mocap_s> _sub_mocap;
|
||||
uORB::Subscription<vehicle_odometry_s> _sub_visual_odom;
|
||||
uORB::Subscription<vehicle_odometry_s> _sub_mocap_odom;
|
||||
uORB::Subscription<distance_sensor_s> _sub_dist0;
|
||||
uORB::Subscription<distance_sensor_s> _sub_dist1;
|
||||
uORB::Subscription<distance_sensor_s> _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<float, n_x> _x; // state vector
|
||||
Vector<float, n_u> _u; // input vector
|
||||
Matrix<float, n_x, n_x> _P; // state covariance matrix
|
||||
|
||||
matrix::Quatf _q;
|
||||
matrix::Dcm<float> _R_att;
|
||||
Vector3f _eul;
|
||||
|
||||
Matrix<float, n_x, n_x> _A; // dynamics matrix
|
||||
Matrix<float, n_x, n_u> _B; // input matrix
|
||||
|
|
|
|||
|
|
@ -36,8 +36,10 @@ void BlockLocalPositionEstimator::flowInit()
|
|||
|
||||
int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &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<float, n_y_flow> &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 +
|
||||
|
|
|
|||
|
|
@ -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<float, n_y_gps, n_y_gps> 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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -52,9 +52,10 @@ int BlockLocalPositionEstimator::lidarMeasure(Vector<float, n_y_lidar> &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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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<float, n_y_mocap> &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<float, n_y_mocap> 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<float, n_y_mocap, n_x> C;
|
||||
|
|
@ -68,11 +107,23 @@ void BlockLocalPositionEstimator::mocapCorrect()
|
|||
// noise matrix
|
||||
Matrix<float, n_y_mocap, n_y_mocap> 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<float, n_y_mocap> r = y - C * _x;
|
||||
|
|
@ -80,12 +131,12 @@ void BlockLocalPositionEstimator::mocapCorrect()
|
|||
Matrix<float, n_y_mocap, n_y_mocap> 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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -66,9 +66,10 @@ int BlockLocalPositionEstimator::sonarMeasure(Vector<float, n_y_sonar> &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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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<float, n_y_vision> &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<float, n_y_vision> 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<float, n_y_vision, n_x> C;
|
||||
|
|
@ -83,18 +113,18 @@ void BlockLocalPositionEstimator::visionCorrect()
|
|||
Matrix<float, n_y_vision, n_y_vision> 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<float, n_x> x0 = _xDelay.get(i_hist);
|
||||
|
||||
// residual
|
||||
Matrix<float, n_y_vision, n_y_vision> S_I = inv<float, n_y_vision>((C * _P * C.transpose()) + R);
|
||||
Matrix<float, n_y_vision, 1> r = y - C * x0;
|
||||
// residual covariance
|
||||
Matrix<float, n_y_vision, n_y_vision> 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<float, n_y_vision, n_y_vision> S_I = inv<float, n_y_vision>(S);
|
||||
|
||||
// fault detection
|
||||
float beta = (r.transpose() * (S_I * r))(0, 0);
|
||||
|
|
|
|||
Loading…
Reference in New Issue