diff --git a/src/modules/landing_target_estimator/CMakeLists.txt b/src/modules/landing_target_estimator/CMakeLists.txt new file mode 100644 index 0000000000..a9e13267c1 --- /dev/null +++ b/src/modules/landing_target_estimator/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 2018 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE modules__landing_target_estimator + MAIN landing_target_estimator + STACK_MAIN 1200 + COMPILE_FLAGS + SRCS + landing_target_estimator_main.cpp + LandingTargetEstimator.cpp + KalmanFilter.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/landing_target_estimator/KalmanFilter.cpp b/src/modules/landing_target_estimator/KalmanFilter.cpp new file mode 100644 index 0000000000..e4636b6e0f --- /dev/null +++ b/src/modules/landing_target_estimator/KalmanFilter.cpp @@ -0,0 +1,153 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file KalmanFilter.h + * Simple Kalman Filter for variable gain low-passing + * + * @author Nicolas de Palezieux (Sunflower Labs) + * + */ + +#include "KalmanFilter.h" + +namespace landing_target_estimator +{ +KalmanFilter::KalmanFilter(matrix::Vector &initial, matrix::Matrix &covInit) +{ + init(initial, covInit); +} + +void KalmanFilter::init(matrix::Vector &initial, matrix::Matrix &covInit) +{ + _x = initial; + _covariance = covInit; +} + +void KalmanFilter::init(float initial0, float initial1, float covInit00, float covInit11) +{ + matrix::Vector initial; + initial(0) = initial0; + initial(1) = initial1; + matrix::Matrix covInit; + covInit(0, 0) = covInit00; + covInit(1, 1) = covInit11; + + init(initial, covInit); +} + +void KalmanFilter::predict(float dt, float acc, float acc_unc) +{ + _x(0) += _x(1) * dt + dt * dt / 2 * acc; + _x(1) += acc * dt; + + matrix::Matrix A; // propagation matrix + A(0, 0) = 1; + A(1, 1) = 1; + A(0, 1) = dt; + + matrix::Matrix G; // noise model + G(0, 0) = dt * dt / 2; + G(1, 0) = dt; + + matrix::Matrix process_noise = G * G.transpose() * acc_unc; + + _covariance = A * _covariance * A.transpose() + process_noise; +} + +bool KalmanFilter::update(float meas, float measUnc) +{ + + // H = [1, 0] + _residual = meas - _x(0); + + // H * P * H^T simply selects P(0,0) + _innovCov = _covariance(0, 0) + measUnc; + + // outlier rejection + float beta = _residual / _innovCov * _residual; + + // 5% false alarm probability + if (beta > 3.84f) { + return false; + } + + matrix::Vector kalmanGain; + kalmanGain(0) = _covariance(0, 0); + kalmanGain(1) = _covariance(1, 0); + kalmanGain /= _innovCov; + + _x += kalmanGain * _residual; + + matrix::Matrix identity; + identity.identity(); + + matrix::Matrix KH; // kalmanGain * H + KH(0, 0) = kalmanGain(0); + KH(1, 0) = kalmanGain(1); + + _covariance = (identity - KH) * _covariance; + + return true; + +} + +void KalmanFilter::getState(matrix::Vector &state) +{ + state = _x; +} + +void KalmanFilter::getState(float &state0, float &state1) +{ + state0 = _x(0); + state1 = _x(1); +} + +void KalmanFilter::getCovariance(matrix::Matrix &covariance) +{ + covariance = _covariance; +} + +void KalmanFilter::getCovariance(float &cov00, float &cov11) +{ + cov00 = _covariance(0, 0); + cov11 = _covariance(1, 1); +} + +void KalmanFilter::getInnovations(float &innov, float &innovCov) +{ + innov = _residual; + innovCov = _innovCov; +} + +} // namespace landing_target_estimator diff --git a/src/modules/landing_target_estimator/KalmanFilter.h b/src/modules/landing_target_estimator/KalmanFilter.h new file mode 100644 index 0000000000..f0d194c26a --- /dev/null +++ b/src/modules/landing_target_estimator/KalmanFilter.h @@ -0,0 +1,150 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file KalmanFilter.h + * Simple Kalman Filter for variable gain low-passing + * + * Constant velocity model. Prediction according to + * x_{k+1} = A * x_{k} + * with A = [1 dt; 0 1] + * + * Update with a direct measurement of the first state: + * H = [1 0] + * + * @author Nicolas de Palezieux (Sunflower Labs) + * + */ + +#include +#include +#include +#include + +#pragma once + +namespace landing_target_estimator +{ +class KalmanFilter +{ +public: + /** + * Default constructor, state not initialized + */ + KalmanFilter() {}; + + /** + * Constructor, initialize state + */ + KalmanFilter(matrix::Vector &initial, matrix::Matrix &covInit); + + /** + * Default desctructor + */ + virtual ~KalmanFilter() {}; + + /** + * Initialize filter state + * @param initial initial state + * @param covInit initial covariance + */ + void init(matrix::Vector &initial, matrix::Matrix &covInit); + + /** + * Initialize filter state, only specifying diagonal covariance elements + * @param initial0 first initial state + * @param initial1 second initial state + * @param covInit00 initial variance of first state + * @param covinit11 initial variance of second state + */ + void init(float initial0, float initial1, float covInit00, float covInit11); + + /** + * Predict the state with an external acceleration estimate + * @param dt Time delta in seconds since last state change + * @param acc Acceleration estimate + * @param acc_unc Variance of acceleration estimate + */ + void predict(float dt, float acc, float acc_unc); + + /** + * Update the state estimate with a measurement + * @param meas state measeasurement + * @param measUnc measurement uncertainty + * @return update success (measurement not rejected) + */ + bool update(float meas, float measUnc); + + /** + * Get the current filter state + * @param x1 State + */ + void getState(matrix::Vector &state); + + /** + * Get the current filter state + * @param state0 First state + * @param state1 Second state + */ + void getState(float &state0, float &state1); + + /** + * Get state covariance + * @param covariance Covariance of the state + */ + void getCovariance(matrix::Matrix &covariance); + + /** + * Get state variances (diagonal elements) + * @param cov00 Variance of first state + * @param cov11 Variance of second state + */ + void getCovariance(float &cov00, float &cov11); + + /** + * Get measurement innovation and covariance of last update call + * @param innov Measurement innovation + * @param innovCov Measurement innovation covariance + */ + void getInnovations(float &innov, float &innovCov); + +private: + matrix::Vector _x; // state + + matrix::Matrix _covariance; // state covariance + + float _residual; // residual of last measurement update + + float _innovCov; // innovation covariance of last measurement update +}; +} // namespace landing_target_estimator diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp new file mode 100644 index 0000000000..3ce4aa38bd --- /dev/null +++ b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp @@ -0,0 +1,328 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file LandingTargetEstimator.cpp + * + * @author Nicolas de Palezieux (Sunflower Labs) + * @author Mohammed Kabir + * + */ + +#include +#include +#include + +#include "LandingTargetEstimator.h" + +#define SEC2USEC 1000000.0f + + +namespace landing_target_estimator +{ + +LandingTargetEstimator::LandingTargetEstimator() : + _targetPosePub(nullptr), + _targetInnovationsPub(nullptr), + _paramHandle(), + _vehicleLocalPosition_valid(false), + _vehicleAttitude_valid(false), + _sensorBias_valid(false), + _new_irlockReport(false), + _estimator_initialized(false), + _faulty(false), + _last_predict(0), + _last_update(0) +{ + _paramHandle.acc_unc = param_find("LTEST_ACC_UNC"); + _paramHandle.meas_unc = param_find("LTEST_MEAS_UNC"); + _paramHandle.pos_unc_init = param_find("LTEST_POS_UNC_IN"); + _paramHandle.vel_unc_init = param_find("LTEST_VEL_UNC_IN"); + _paramHandle.mode = param_find("LTEST_MODE"); + _paramHandle.scale_x = param_find("LTEST_SCALE_X"); + _paramHandle.scale_y = param_find("LTEST_SCALE_Y"); + + // Initialize uORB topics. + _initialize_topics(); + + _check_params(true); +} + +LandingTargetEstimator::~LandingTargetEstimator() +{ +} + +void LandingTargetEstimator::update() +{ + _check_params(false); + + _update_topics(); + + /* predict */ + if (_estimator_initialized) { + if (hrt_absolute_time() - _last_update > landing_target_estimator_TIMEOUT_US) { + PX4_WARN("Timeout"); + _estimator_initialized = false; + + } else if (_vehicleLocalPosition_valid + && _vehicleLocalPosition.v_xy_valid) { + float dt = (hrt_absolute_time() - _last_predict) / SEC2USEC; + + // predict target position with the help of accel data + matrix::Vector3f a; + + if (_sensorBias_valid) { + matrix::Quaternion q_att(&_vehicleAttitude.q[0]); + _R_att = matrix::Dcm(q_att); + a(0) = _sensorBias.accel_x; + a(1) = _sensorBias.accel_y; + a(2) = _sensorBias.accel_z; + a = _R_att * a; + + } else { + a.zero(); + } + + _kalman_filter_x.predict(dt, -a(0), _params.acc_unc); + _kalman_filter_y.predict(dt, -a(1), _params.acc_unc); + + _last_predict = hrt_absolute_time(); + } + } + + if (!_new_irlockReport) { + // nothing to do + return; + } + + // mark this sensor measurement as consumed + _new_irlockReport = false; + + if (!_vehicleAttitude_valid || !_vehicleLocalPosition_valid || !_vehicleLocalPosition.dist_bottom_valid) { + // don't have the data needed for an update + return; + } + + if (!PX4_ISFINITE(_irlockReport.pos_y) || !PX4_ISFINITE(_irlockReport.pos_x)) { + return; + } + + // TODO account for sensor orientation as set by parameter + // default orientation has camera x pointing in body y, camera y in body -x + + matrix::Vector sensor_ray; // ray pointing towards target in body frame + sensor_ray(0) = -_irlockReport.pos_y * _params.scale_y; // forward + sensor_ray(1) = _irlockReport.pos_x * _params.scale_x; // right + sensor_ray(2) = 1.0f; + + // rotate the unit ray into the navigation frame, assume sensor frame = body frame + matrix::Quaternion q_att(&_vehicleAttitude.q[0]); + _R_att = matrix::Dcm(q_att); + sensor_ray = _R_att * sensor_ray; + + if (fabsf(sensor_ray(2)) < 1e-6f) { + // z component of measurement unsafe, don't use this measurement + return; + } + + float dist = _vehicleLocalPosition.dist_bottom; + + // scale the ray s.t. the z component has length of dist + _rel_pos(0) = sensor_ray(0) / sensor_ray(2) * dist; + _rel_pos(1) = sensor_ray(1) / sensor_ray(2) * dist; + + if (!_estimator_initialized) { + PX4_INFO("Init"); + _kalman_filter_x.init(_rel_pos(0), 0, _params.pos_unc_init, _params.vel_unc_init); + _kalman_filter_y.init(_rel_pos(1), 0, _params.pos_unc_init, _params.vel_unc_init); + + _estimator_initialized = true; + _last_update = hrt_absolute_time(); + _last_predict = _last_update; + + } else { + // update + bool update_x = _kalman_filter_x.update(_rel_pos(0), _params.meas_unc * dist * dist); + bool update_y = _kalman_filter_y.update(_rel_pos(1), _params.meas_unc * dist * dist); + + if (!update_x || !update_y) { + if (!_faulty) { + _faulty = true; + PX4_WARN("Landing target measurement rejected:%s%s", update_x ? "" : " x", update_y ? "" : " y"); + } + + } else { + _faulty = false; + } + + if (!_faulty) { + // only publish if both measurements were good + + _target_pose.timestamp = _irlockReport.timestamp; + + float x, xvel, y, yvel, covx, covx_v, covy, covy_v; + _kalman_filter_x.getState(x, xvel); + _kalman_filter_x.getCovariance(covx, covx_v); + + _kalman_filter_y.getState(y, yvel); + _kalman_filter_y.getCovariance(covy, covy_v); + + _target_pose.is_static = (_params.mode == TargetMode::Stationary); + + _target_pose.rel_pos_valid = true; + _target_pose.rel_vel_valid = true; + _target_pose.x_rel = x; + _target_pose.y_rel = y; + _target_pose.z_rel = dist; + _target_pose.vx_rel = xvel; + _target_pose.vy_rel = yvel; + + _target_pose.cov_x_rel = covx; + _target_pose.cov_y_rel = covy; + + _target_pose.cov_vx_rel = covx_v; + _target_pose.cov_vy_rel = covy_v; + + if (_vehicleLocalPosition_valid && _vehicleLocalPosition.xy_valid) { + _target_pose.x_abs = x + _vehicleLocalPosition.x; + _target_pose.y_abs = y + _vehicleLocalPosition.y; + _target_pose.z_abs = dist + _vehicleLocalPosition.z; + _target_pose.abs_pos_valid = true; + + } else { + _target_pose.abs_pos_valid = false; + } + + if (_targetPosePub == nullptr) { + _targetPosePub = orb_advertise(ORB_ID(landing_target_pose), &_target_pose); + + } else { + orb_publish(ORB_ID(landing_target_pose), _targetPosePub, &_target_pose); + } + + _last_update = hrt_absolute_time(); + _last_predict = _last_update; + } + + float innov_x, innov_cov_x, innov_y, innov_cov_y; + _kalman_filter_x.getInnovations(innov_x, innov_cov_x); + _kalman_filter_y.getInnovations(innov_y, innov_cov_y); + + _target_innovations.timestamp = _irlockReport.timestamp; + _target_innovations.innov_x = innov_x; + _target_innovations.innov_cov_x = innov_cov_x; + _target_innovations.innov_y = innov_y; + _target_innovations.innov_cov_y = innov_cov_y; + + if (_targetInnovationsPub == nullptr) { + _targetInnovationsPub = orb_advertise(ORB_ID(landing_target_innovations), &_target_innovations); + + } else { + orb_publish(ORB_ID(landing_target_innovations), _targetInnovationsPub, &_target_innovations); + } + } + +} + +void LandingTargetEstimator::_check_params(const bool force) +{ + bool updated; + parameter_update_s paramUpdate; + + orb_check(_parameterSub, &updated); + + if (updated) { + orb_copy(ORB_ID(parameter_update), _parameterSub, ¶mUpdate); + } + + if (updated || force) { + _update_params(); + } +} + +void LandingTargetEstimator::_initialize_topics() +{ + // subscribe to position, attitude, arming and velocity changes + _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); + _attitudeSub = orb_subscribe(ORB_ID(vehicle_attitude)); + _sensorBiasSub = orb_subscribe(ORB_ID(sensor_bias)); + _irlockReportSub = orb_subscribe(ORB_ID(irlock_report)); + _parameterSub = orb_subscribe(ORB_ID(parameter_update)); +} + +void LandingTargetEstimator::_update_topics() +{ + _vehicleLocalPosition_valid = _orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, + &_vehicleLocalPosition); + _vehicleAttitude_valid = _orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude); + _sensorBias_valid = _orb_update(ORB_ID(sensor_bias), _sensorBiasSub, &_sensorBias); + + _new_irlockReport = _orb_update(ORB_ID(irlock_report), _irlockReportSub, &_irlockReport); +} + + +bool LandingTargetEstimator::_orb_update(const struct orb_metadata *meta, int handle, void *buffer) +{ + bool newData = false; + + // check if there is new data to grab + if (orb_check(handle, &newData) != OK) { + return false; + } + + if (!newData) { + return false; + } + + if (orb_copy(meta, handle, buffer) != OK) { + return false; + } + + return true; +} + +void LandingTargetEstimator::_update_params() +{ + param_get(_paramHandle.acc_unc, &_params.acc_unc); + param_get(_paramHandle.meas_unc, &_params.meas_unc); + param_get(_paramHandle.pos_unc_init, &_params.pos_unc_init); + param_get(_paramHandle.vel_unc_init, &_params.vel_unc_init); + int mode = 0; + param_get(_paramHandle.mode, &mode); + _params.mode = (TargetMode)mode; + param_get(_paramHandle.scale_x, &_params.scale_x); + param_get(_paramHandle.scale_y, &_params.scale_y); +} + + +} // namespace landing_target_estimator diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.h b/src/modules/landing_target_estimator/LandingTargetEstimator.h new file mode 100644 index 0000000000..be43df3d10 --- /dev/null +++ b/src/modules/landing_target_estimator/LandingTargetEstimator.h @@ -0,0 +1,173 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file LandingTargetEstimator.h + * Landing target position estimator. Filter and publish the position of a landing target on the ground as observed by an onboard sensor. + * + * @author Nicolas de Palezieux (Sunflower Labs) + * @author Mohammed Kabir + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "KalmanFilter.h" + + +namespace landing_target_estimator +{ + +class LandingTargetEstimator +{ +public: + + LandingTargetEstimator(); + virtual ~LandingTargetEstimator(); + + /* + * Get new measurements and update the state estimate + */ + void update(); + +protected: + /* + * Called once to initialize uORB topics. + */ + void _initialize_topics(); + + /* + * Update uORB topics. + */ + void _update_topics(); + + /* + * Update parameters. + */ + void _update_params(); + + /* + * Convenience function for polling uORB subscriptions. + * + * @return true if there was new data and it was successfully copied + */ + static bool _orb_update(const struct orb_metadata *meta, int handle, void *buffer); + + /* timeout after which filter is reset if target not seen */ + static constexpr uint32_t landing_target_estimator_TIMEOUT_US = 2000000; + + orb_advert_t _targetPosePub; + struct landing_target_pose_s _target_pose; + + orb_advert_t _targetInnovationsPub; + struct landing_target_innovations_s _target_innovations; + + int _parameterSub; + +private: + + enum class TargetMode { + Moving = 0, + Stationary + }; + + /** + * Handles for parameters + **/ + struct { + param_t acc_unc; + param_t meas_unc; + param_t pos_unc_init; + param_t vel_unc_init; + param_t mode; + param_t scale_x; + param_t scale_y; + } _paramHandle; + + struct { + float acc_unc; + float meas_unc; + float pos_unc_init; + float vel_unc_init; + TargetMode mode; + float scale_x; + float scale_y; + } _params; + + int _vehicleLocalPositionSub; + int _attitudeSub; + int _sensorBiasSub; + int _irlockReportSub; + + struct vehicle_local_position_s _vehicleLocalPosition; + struct vehicle_attitude_s _vehicleAttitude; + struct sensor_bias_s _sensorBias; + struct irlock_report_s _irlockReport; + + // keep track of which topics we have received + bool _vehicleLocalPosition_valid; + bool _vehicleAttitude_valid; + bool _sensorBias_valid; + bool _new_irlockReport; + bool _estimator_initialized; + // keep track of whether last measurement was rejected + bool _faulty; + + matrix::Dcm _R_att; + matrix::Vector _rel_pos; + KalmanFilter _kalman_filter_x; + KalmanFilter _kalman_filter_y; + hrt_abstime _last_predict; // timestamp of last filter prediction + hrt_abstime _last_update; // timestamp of last filter update (used to check timeout) + + void _check_params(const bool force); + + void _update_state(); +}; + + +} // namespace landing_target_estimator diff --git a/src/modules/landing_target_estimator/landing_target_estimator_main.cpp b/src/modules/landing_target_estimator/landing_target_estimator_main.cpp new file mode 100644 index 0000000000..46a1c802c3 --- /dev/null +++ b/src/modules/landing_target_estimator/landing_target_estimator_main.cpp @@ -0,0 +1,154 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file landing_target_estimator_main.cpp + * Landing target position estimator. Filter and publish the position of a landing target on the ground as observed by an onboard sensor. + * + * @author Nicolas de Palezieux (Sunflower Labs) + * @author Mohammed Kabir + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "LandingTargetEstimator.h" + + +namespace landing_target_estimator +{ + +static bool thread_should_exit = false; /**< daemon exit flag */ +static bool thread_running = false; /**< daemon status flag */ +static int daemon_task; /**< Handle of daemon task / thread */ + +/* Run main loop at this rate in Hz. */ +static constexpr uint32_t landing_target_estimator_UPDATE_RATE_HZ = 50; + +/** + * Landing target position estimator app start / stop handling function + * This makes the module accessible from the nuttx shell + * @ingroup apps + */ +extern "C" __EXPORT int landing_target_estimator_main(int argc, char *argv[]); + +/** + * Mainloop of daemon. + */ +int landing_target_estimator_thread_main(int argc, char *argv[]); + +/** +* Main entry point for this module +**/ +int landing_target_estimator_main(int argc, char *argv[]) +{ + + if (argc < 2) { + goto exiterr; + } + + if (argc >= 2 && !strcmp(argv[1], "start")) { + if (thread_running) { + PX4_INFO("already running"); + /* this is not an error */ + return 0; + } + + thread_should_exit = false; + daemon_task = px4_task_spawn_cmd("landing_target_estimator", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2000, + landing_target_estimator_thread_main, + (argv) ? (char *const *)&argv[2] : nullptr); + return 0; + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + + if (!thread_running) { + PX4_WARN("landing_target_estimator not running"); + } + + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + PX4_INFO("running"); + + } else { + PX4_INFO("not started"); + } + + return 0; + } + +exiterr: + PX4_WARN("usage: landing_target_estimator {start|stop|status}"); + return 1; +} + +int landing_target_estimator_thread_main(int argc, char *argv[]) +{ + PX4_DEBUG("starting"); + + thread_running = true; + + LandingTargetEstimator est; + + while (!thread_should_exit) { + est.update(); + usleep(1000000 / landing_target_estimator_UPDATE_RATE_HZ); + } + + PX4_DEBUG("exiting"); + + thread_running = false; + + return 0; +} + +} diff --git a/src/modules/landing_target_estimator/landing_target_estimator_params.c b/src/modules/landing_target_estimator/landing_target_estimator_params.c new file mode 100644 index 0000000000..2b68695de9 --- /dev/null +++ b/src/modules/landing_target_estimator/landing_target_estimator_params.c @@ -0,0 +1,134 @@ +/**************************************************************************** + * + * Copyright (c) 2014-2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file landing_target_estimator_params.c + * Landing target estimator algorithm parameters. + * + * @author Nicolas de Palezieux (Sunflower Labs) + * @author Mohammed Kabir + * + */ + +/** + * Landing target mode + * + * Configure the mode of the landing target. Depending on the mode, the landing target observations are used differently to aid position estimation. + * + * Mode Moving: The landing target may be moving around while in the field of view of the vehicle. Landing target measurements are not used to aid positioning. + * Mode Stationary: The landing target is stationary. Measured velocity w.r.t. the landing target is used to aid velocity estimation. + * + * @min 0 + * @max 1 + * @group Landing target Estimator + * @value 0 Moving + * @value 1 Stationary + */ +PARAM_DEFINE_INT32(LTEST_MODE, 0); + +/** + * Acceleration uncertainty + * + * Variance of acceleration measurement used for landing target position prediction. + * Higher values results in tighter following of the measurements and more lenient outlier rejection + * + * @unit (m/s^2)^2 + * @min 0.01 + * @decimal 2 + * + * @group Landing target Estimator + */ +PARAM_DEFINE_FLOAT(LTEST_ACC_UNC, 10.0f); + +/** + * Landing target measurement uncertainty + * + * Variance of the landing target measurement from the driver. + * Higher values results in less agressive following of the measurement and a smoother output as well as fewer rejected measurements. + * + * @unit tan(rad)^2 + * @decimal 4 + * + * @group Landing target Estimator + */ +PARAM_DEFINE_FLOAT(LTEST_MEAS_UNC, 0.005f); + +/** + * Initial landing target position uncertainty + * + * Initial variance of the relative landing target position in x and y direction + * + * @unit m^2 + * @min 0.001 + * @decimal 3 + * + * @group Landing target Estimator + */ +PARAM_DEFINE_FLOAT(LTEST_POS_UNC_IN, 0.1f); + +/** + * Initial landing target velocity uncertainty + * + * Initial variance of the relative landing target velocity in x and y direction + * + * @unit (m/s)^2 + * @min 0.001 + * @decimal 3 + * + * @group Landing target Estimator + */ +PARAM_DEFINE_FLOAT(LTEST_VEL_UNC_IN, 1.0f); + +/** + * Scale factor for sensor measurements in sensor x axis + * + * Landing target x measurements are scaled by this factor before being used + * + * @min 0.01 + * @decimal 3 + * + * @group Landing target Estimator + */ +PARAM_DEFINE_FLOAT(LTEST_SCALE_X, 1.0f); + +/** + * Scale factor for sensor measurements in sensor y axis + * + * Landing target y measurements are scaled by this factor before being used + * + * @min 0.01 + * @decimal 3 + * + * @group Landing target Estimator + */ +PARAM_DEFINE_FLOAT(LTEST_SCALE_Y, 1.0f);