mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-19 15:49:06 +08:00
cleanup landing_target_estimator
This commit is contained in:
Daniel Agar
parent
17540cd525
commit
6c7800faab
@ -1,4 +1,6 @@
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
uint64 timestamp_sample # the timestamp of the raw data (microseconds)
|
||||
|
||||
# Innovation of landing target position estimator
|
||||
float32 innov_x
|
||||
float32 innov_y
|
||||
|
||||
@ -1,6 +1,7 @@
|
||||
# Relative position of precision land target in navigation (body fixed, north aligned, NED) and inertial (world fixed, north aligned, NED) frames
|
||||
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
uint64 timestamp_sample # the timestamp of the raw data (microseconds)
|
||||
|
||||
bool is_static # Flag indicating whether the landing target is static or moving with respect to the ground
|
||||
|
||||
|
||||
@ -568,6 +568,9 @@ SquareMatrix <Type, M> choleskyInv(const SquareMatrix<Type, M> &A)
|
||||
return L_inv.T() * L_inv;
|
||||
}
|
||||
|
||||
using Matrix2f = SquareMatrix<float, 2>;
|
||||
using Matrix2d = SquareMatrix<double, 2>;
|
||||
|
||||
using Matrix3f = SquareMatrix<float, 3>;
|
||||
using Matrix3d = SquareMatrix<double, 3>;
|
||||
|
||||
|
||||
@ -1515,7 +1515,7 @@ void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps)
|
||||
if (landing_target_pose.is_static && landing_target_pose.rel_vel_valid) {
|
||||
// velocity of vehicle relative to target has opposite sign to target relative to vehicle
|
||||
auxVelSample auxvel_sample{
|
||||
.time_us = landing_target_pose.timestamp,
|
||||
.time_us = landing_target_pose.timestamp_sample,
|
||||
.vel = Vector3f{-landing_target_pose.vx_rel, -landing_target_pose.vy_rel, 0.0f},
|
||||
.velVar = Vector3f{landing_target_pose.cov_vx_rel, landing_target_pose.cov_vy_rel, 0.0f},
|
||||
};
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2018 PX4 Development Team. All rights reserved.
|
||||
# Copyright (c) 2018-2022 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
|
||||
@ -36,7 +36,9 @@ px4_add_module(
|
||||
MAIN landing_target_estimator
|
||||
COMPILE_FLAGS
|
||||
SRCS
|
||||
LandingTargetEstimator.cpp
|
||||
KalmanFilter.cpp
|
||||
KalmanFilter.hpp
|
||||
LandingTargetEstimator.cpp
|
||||
LandingTargetEstimator.hpp
|
||||
)
|
||||
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2018 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2018-2022 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
|
||||
@ -39,14 +39,14 @@
|
||||
*
|
||||
*/
|
||||
|
||||
#include "KalmanFilter.h"
|
||||
#include "KalmanFilter.hpp"
|
||||
|
||||
KalmanFilter::KalmanFilter(matrix::Vector<float, 2> &initial, matrix::Matrix<float, 2, 2> &covInit)
|
||||
KalmanFilter::KalmanFilter(matrix::Vector2f &initial, matrix::Matrix2f &covInit)
|
||||
{
|
||||
init(initial, covInit);
|
||||
}
|
||||
|
||||
void KalmanFilter::init(matrix::Vector<float, 2> &initial, matrix::Matrix<float, 2, 2> &covInit)
|
||||
void KalmanFilter::init(matrix::Vector2f &initial, matrix::Matrix2f &covInit)
|
||||
{
|
||||
_x = initial;
|
||||
_covariance = covInit;
|
||||
@ -54,10 +54,10 @@ void KalmanFilter::init(matrix::Vector<float, 2> &initial, matrix::Matrix<float,
|
||||
|
||||
void KalmanFilter::init(float initial0, float initial1, float covInit00, float covInit11)
|
||||
{
|
||||
matrix::Vector<float, 2> initial;
|
||||
matrix::Vector2f initial;
|
||||
initial(0) = initial0;
|
||||
initial(1) = initial1;
|
||||
matrix::Matrix<float, 2, 2> covInit;
|
||||
matrix::Matrix2f covInit;
|
||||
covInit(0, 0) = covInit00;
|
||||
covInit(1, 1) = covInit11;
|
||||
|
||||
@ -69,7 +69,7 @@ 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<float, 2, 2> A; // propagation matrix
|
||||
matrix::Matrix2f A; // propagation matrix
|
||||
A(0, 0) = 1;
|
||||
A(1, 1) = 1;
|
||||
A(0, 1) = dt;
|
||||
@ -78,14 +78,13 @@ void KalmanFilter::predict(float dt, float acc, float acc_unc)
|
||||
G(0, 0) = dt * dt / 2;
|
||||
G(1, 0) = dt;
|
||||
|
||||
matrix::Matrix<float, 2, 2> process_noise = G * G.transpose() * acc_unc;
|
||||
matrix::Matrix2f 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);
|
||||
|
||||
@ -100,46 +99,23 @@ bool KalmanFilter::update(float meas, float measUnc)
|
||||
return false;
|
||||
}
|
||||
|
||||
matrix::Vector<float, 2> kalmanGain;
|
||||
matrix::Vector2f kalmanGain;
|
||||
kalmanGain(0) = _covariance(0, 0);
|
||||
kalmanGain(1) = _covariance(1, 0);
|
||||
kalmanGain /= _innovCov;
|
||||
|
||||
_x += kalmanGain * _residual;
|
||||
|
||||
matrix::Matrix<float, 2, 2> identity;
|
||||
matrix::Matrix2f identity;
|
||||
identity.identity();
|
||||
|
||||
matrix::Matrix<float, 2, 2> KH; // kalmanGain * H
|
||||
matrix::Matrix2f KH; // kalmanGain * H
|
||||
KH(0, 0) = kalmanGain(0);
|
||||
KH(1, 0) = kalmanGain(1);
|
||||
|
||||
_covariance = (identity - KH) * _covariance;
|
||||
|
||||
return true;
|
||||
|
||||
}
|
||||
|
||||
void KalmanFilter::getState(matrix::Vector<float, 2> &state)
|
||||
{
|
||||
state = _x;
|
||||
}
|
||||
|
||||
void KalmanFilter::getState(float &state0, float &state1)
|
||||
{
|
||||
state0 = _x(0);
|
||||
state1 = _x(1);
|
||||
}
|
||||
|
||||
void KalmanFilter::getCovariance(matrix::Matrix<float, 2, 2> &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)
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2018 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2018-2022 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
|
||||
@ -59,24 +59,24 @@ public:
|
||||
/**
|
||||
* Default constructor, state not initialized
|
||||
*/
|
||||
KalmanFilter() {};
|
||||
KalmanFilter() = default;
|
||||
|
||||
/**
|
||||
* Constructor, initialize state
|
||||
*/
|
||||
KalmanFilter(matrix::Vector<float, 2> &initial, matrix::Matrix<float, 2, 2> &covInit);
|
||||
KalmanFilter(matrix::Vector2f &initial, matrix::Matrix2f &covInit);
|
||||
|
||||
/**
|
||||
* Default desctructor
|
||||
*/
|
||||
virtual ~KalmanFilter() {};
|
||||
~KalmanFilter() = default;
|
||||
|
||||
/**
|
||||
* Initialize filter state
|
||||
* @param initial initial state
|
||||
* @param covInit initial covariance
|
||||
*/
|
||||
void init(matrix::Vector<float, 2> &initial, matrix::Matrix<float, 2, 2> &covInit);
|
||||
void init(matrix::Vector2f &initial, matrix::Matrix2f &covInit);
|
||||
|
||||
/**
|
||||
* Initialize filter state, only specifying diagonal covariance elements
|
||||
@ -107,27 +107,13 @@ public:
|
||||
* Get the current filter state
|
||||
* @param x1 State
|
||||
*/
|
||||
void getState(matrix::Vector<float, 2> &state);
|
||||
|
||||
/**
|
||||
* Get the current filter state
|
||||
* @param state0 First state
|
||||
* @param state1 Second state
|
||||
*/
|
||||
void getState(float &state0, float &state1);
|
||||
const matrix::Vector2f &getState() const { return _x; }
|
||||
|
||||
/**
|
||||
* Get state covariance
|
||||
* @param covariance Covariance of the state
|
||||
*/
|
||||
void getCovariance(matrix::Matrix<float, 2, 2> &covariance);
|
||||
|
||||
/**
|
||||
* Get state variances (diagonal elements)
|
||||
* @param cov00 Variance of first state
|
||||
* @param cov11 Variance of second state
|
||||
*/
|
||||
void getCovariance(float &cov00, float &cov11);
|
||||
const matrix::Matrix2f &getCovariance() const { return _covariance; }
|
||||
|
||||
/**
|
||||
* Get measurement innovation and covariance of last update call
|
||||
@ -137,11 +123,10 @@ public:
|
||||
void getInnovations(float &innov, float &innovCov);
|
||||
|
||||
private:
|
||||
matrix::Vector<float, 2> _x; // state
|
||||
matrix::Vector2f _x{}; // state
|
||||
matrix::Matrix2f _covariance{}; // state covariance
|
||||
|
||||
matrix::Matrix<float, 2, 2> _covariance; // state covariance
|
||||
float _residual{0.f}; // residual of last measurement update
|
||||
|
||||
float _residual{0.0f}; // residual of last measurement update
|
||||
|
||||
float _innovCov{0.0f}; // innovation covariance of last measurement update
|
||||
float _innovCov{0.f}; // innovation covariance of last measurement update
|
||||
};
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2018 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2018-2022 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
|
||||
@ -31,293 +31,306 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include "LandingTargetEstimator.h"
|
||||
|
||||
#define SEC2USEC 1000000.0f
|
||||
#include "LandingTargetEstimator.hpp"
|
||||
|
||||
LandingTargetEstimator::LandingTargetEstimator() :
|
||||
ModuleParams(nullptr),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
|
||||
{
|
||||
_check_params(true);
|
||||
}
|
||||
|
||||
LandingTargetEstimator::~LandingTargetEstimator()
|
||||
{
|
||||
perf_free(_cycle_perf);
|
||||
}
|
||||
|
||||
bool LandingTargetEstimator::init()
|
||||
{
|
||||
if (!_vehicle_local_position_sub.registerCallback()) {
|
||||
PX4_ERR("callback registration failed");
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void LandingTargetEstimator::Run()
|
||||
{
|
||||
_check_params(false);
|
||||
|
||||
_update_topics();
|
||||
|
||||
/* predict */
|
||||
if (_estimator_initialized) {
|
||||
if (hrt_absolute_time() - _last_update > TARGET_UPDATE_TIMEOUT_US) {
|
||||
PX4_INFO("Target lost");
|
||||
_estimator_initialized = false;
|
||||
|
||||
} else {
|
||||
float dt = (hrt_absolute_time() - _last_predict) / SEC2USEC;
|
||||
|
||||
// predict target position with the help of accel data
|
||||
matrix::Vector3f a{_vehicle_acceleration.xyz};
|
||||
|
||||
if (_vehicleAttitude_valid && _vehicle_acceleration_valid) {
|
||||
matrix::Quaternion<float> q_att(&_vehicleAttitude.q[0]);
|
||||
_R_att = matrix::Dcm<float>(q_att);
|
||||
a = _R_att * a;
|
||||
|
||||
} else {
|
||||
a.zero();
|
||||
}
|
||||
|
||||
_kalman_filter_x.predict(dt, -a(0), _param_acc_unc.get());
|
||||
_kalman_filter_y.predict(dt, -a(1), _param_acc_unc.get());
|
||||
|
||||
_last_predict = hrt_absolute_time();
|
||||
}
|
||||
}
|
||||
|
||||
if (!_new_sensorReport) {
|
||||
// nothing to do
|
||||
if (should_exit()) {
|
||||
_vehicle_local_position_sub.unregisterCallback();
|
||||
exit_and_cleanup();
|
||||
return;
|
||||
}
|
||||
|
||||
// mark this sensor measurement as consumed
|
||||
_new_sensorReport = false;
|
||||
// backup schedule
|
||||
ScheduleDelayed(100_ms);
|
||||
|
||||
if (!PX4_ISFINITE(_irlockReport.pos_y) || !PX4_ISFINITE(_irlockReport.pos_x)) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (!_estimator_initialized) {
|
||||
float vx_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vx : 0.f;
|
||||
float vy_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vy : 0.f;
|
||||
PX4_INFO("Target acquired %.2f %.2f", (double)vx_init, (double)vy_init);
|
||||
_kalman_filter_x.init(_target_position_report.rel_pos_x, vx_init, _param_pos_unc_in.get(), _param_vel_unc_in.get());
|
||||
_kalman_filter_y.init(_target_position_report.rel_pos_y, vy_init, _param_pos_unc_in.get(), _param_vel_unc_in.get());
|
||||
|
||||
_estimator_initialized = true;
|
||||
_last_update = hrt_absolute_time();
|
||||
_last_predict = _last_update;
|
||||
|
||||
} else {
|
||||
const float measurement_uncertainty = _param_meas_unc.get() * _dist_z * _dist_z;
|
||||
bool update_x = _kalman_filter_x.update(_target_position_report.rel_pos_x, measurement_uncertainty);
|
||||
bool update_y = _kalman_filter_y.update(_target_position_report.rel_pos_y, measurement_uncertainty);
|
||||
|
||||
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 = _target_position_report.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 = ((TargetMode)_param_mode.get() == 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 = _target_position_report.rel_pos_z ;
|
||||
_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 = _target_position_report.rel_pos_z + _vehicleLocalPosition.z;
|
||||
_target_pose.abs_pos_valid = true;
|
||||
|
||||
} else {
|
||||
_target_pose.abs_pos_valid = false;
|
||||
}
|
||||
|
||||
_targetPosePub.publish(_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 = _target_position_report.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;
|
||||
|
||||
_targetInnovationsPub.publish(_target_innovations);
|
||||
}
|
||||
}
|
||||
|
||||
void LandingTargetEstimator::_check_params(const bool force)
|
||||
{
|
||||
if (_parameter_update_sub.updated() || force) {
|
||||
if (_parameter_update_sub.updated()) {
|
||||
parameter_update_s pupdate;
|
||||
_parameter_update_sub.copy(&pupdate);
|
||||
|
||||
ModuleParams::updateParams();
|
||||
}
|
||||
}
|
||||
|
||||
void LandingTargetEstimator::_update_topics()
|
||||
{
|
||||
_vehicleLocalPosition_valid = _vehicleLocalPositionSub.update(&_vehicleLocalPosition);
|
||||
_vehicleAttitude_valid = _attitudeSub.update(&_vehicleAttitude);
|
||||
_vehicle_acceleration_valid = _vehicle_acceleration_sub.update(&_vehicle_acceleration);
|
||||
perf_begin(_cycle_perf);
|
||||
|
||||
// predict
|
||||
vehicle_local_position_s vehicle_local_position;
|
||||
|
||||
if (_irlockReportSub.update(&_irlockReport)) { //
|
||||
_new_irlockReport = true;
|
||||
if (_vehicle_local_position_sub.update(&vehicle_local_position)) {
|
||||
if (_estimator_initialized) {
|
||||
const float dt = math::constrain((vehicle_local_position.timestamp_sample - _last_predict) * 1e-6f, 0.001f, 0.1f);
|
||||
|
||||
if (!_vehicleAttitude_valid || !_vehicleLocalPosition_valid || !_vehicleLocalPosition.dist_bottom_valid) {
|
||||
// don't have the data needed for an update
|
||||
return;
|
||||
// predict target position with the help of accel data
|
||||
_kalman_filter_x.predict(dt, -vehicle_local_position.ax, _param_ltest_acc_unc.get());
|
||||
_kalman_filter_y.predict(dt, -vehicle_local_position.ay, _param_ltest_acc_unc.get());
|
||||
|
||||
_last_predict = vehicle_local_position.timestamp_sample;
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE(_irlockReport.pos_y) || !PX4_ISFINITE(_irlockReport.pos_x)) {
|
||||
return;
|
||||
}
|
||||
|
||||
matrix::Vector<float, 3> sensor_ray; // ray pointing towards target in body frame
|
||||
sensor_ray(0) = _irlockReport.pos_x * _param_scale_x.get(); // forward
|
||||
sensor_ray(1) = _irlockReport.pos_y * _param_scale_y.get(); // right
|
||||
sensor_ray(2) = 1.0f;
|
||||
|
||||
// rotate unit ray according to sensor orientation
|
||||
_S_att = get_rot_matrix((Rotation)_param_sens_rot.get());
|
||||
sensor_ray = _S_att * sensor_ray;
|
||||
|
||||
// rotate the unit ray into the navigation frame
|
||||
matrix::Quaternion<float> q_att(&_vehicleAttitude.q[0]);
|
||||
_R_att = matrix::Dcm<float>(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;
|
||||
}
|
||||
|
||||
_dist_z = _vehicleLocalPosition.dist_bottom - _param_sens_pos_z.get();
|
||||
|
||||
// scale the ray s.t. the z component has length of _uncertainty_scale
|
||||
_target_position_report.timestamp = _irlockReport.timestamp;
|
||||
_target_position_report.rel_pos_x = sensor_ray(0) / sensor_ray(2) * _dist_z;
|
||||
_target_position_report.rel_pos_y = sensor_ray(1) / sensor_ray(2) * _dist_z;
|
||||
_target_position_report.rel_pos_z = _dist_z;
|
||||
|
||||
// Adjust relative position according to sensor offset
|
||||
_target_position_report.rel_pos_x += _param_sens_pos_x.get();
|
||||
_target_position_report.rel_pos_y += _param_sens_pos_y.get();
|
||||
|
||||
_new_sensorReport = true;
|
||||
|
||||
} else if (_uwbDistanceSub.update(&_uwbDistance)) {
|
||||
if (!_vehicleAttitude_valid || !_vehicleLocalPosition_valid) {
|
||||
// don't have the data needed for an update
|
||||
PX4_INFO("Attitude: %d, Local pos: %d", _vehicleAttitude_valid, _vehicleLocalPosition_valid);
|
||||
return;
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE((float)_uwbDistance.position[0]) || !PX4_ISFINITE((float)_uwbDistance.position[1]) ||
|
||||
!PX4_ISFINITE((float)_uwbDistance.position[2])) {
|
||||
PX4_WARN("Position is corrupt!");
|
||||
return;
|
||||
}
|
||||
|
||||
_new_sensorReport = true;
|
||||
|
||||
// The coordinate system is NED (north-east-down)
|
||||
// the uwb_distance msg contains the Position in NED, Vehicle relative to LP
|
||||
// The coordinates "rel_pos_*" are the position of the landing point relative to the vehicle.
|
||||
// To change POV we negate every Axis:
|
||||
_target_position_report.timestamp = _uwbDistance.timestamp;
|
||||
_target_position_report.rel_pos_x = -_uwbDistance.position[0];
|
||||
_target_position_report.rel_pos_y = -_uwbDistance.position[1];
|
||||
_target_position_report.rel_pos_z = -_uwbDistance.position[2];
|
||||
}
|
||||
}
|
||||
|
||||
int LandingTargetEstimator::start()
|
||||
{
|
||||
ScheduleOnInterval(1000000 / SAMPLE_RATE);
|
||||
return PX4_OK;
|
||||
landing_target_pose_s landing_target_pose{};
|
||||
|
||||
bool update_x = false;
|
||||
bool update_y = false;
|
||||
|
||||
if (_irlock_report_sub.advertised()) {
|
||||
// using irlock_report
|
||||
irlock_report_s irlock_report;
|
||||
|
||||
if (_irlock_report_sub.update(&irlock_report)) {
|
||||
vehicle_attitude_s vehicle_attitude;
|
||||
|
||||
if (!vehicle_local_position.v_xy_valid || !vehicle_local_position.dist_bottom_valid
|
||||
|| !_vehicle_attitude_sub.update(&vehicle_attitude)
|
||||
|| !PX4_ISFINITE(irlock_report.pos_y) || !PX4_ISFINITE(irlock_report.pos_x)) {
|
||||
// don't have the data needed for an update
|
||||
return;
|
||||
}
|
||||
|
||||
matrix::Vector3f sensor_ray; // ray pointing towards target in body frame
|
||||
sensor_ray(0) = irlock_report.pos_x * _param_ltest_scale_x.get(); // forward
|
||||
sensor_ray(1) = irlock_report.pos_y * _param_ltest_scale_y.get(); // right
|
||||
sensor_ray(2) = 1.f;
|
||||
|
||||
// Orientation of the sensor relative to body frame
|
||||
const matrix::Dcmf S_att = get_rot_matrix((Rotation)_param_ltest_sens_rot.get());
|
||||
// rotate unit ray according to sensor orientation
|
||||
sensor_ray = S_att * sensor_ray;
|
||||
|
||||
// rotate the unit ray into the navigation frame
|
||||
const matrix::Quatf q_att(vehicle_attitude.q);
|
||||
const matrix::Dcmf R_att(q_att); // Orientation of the body frame
|
||||
sensor_ray = R_att * sensor_ray;
|
||||
|
||||
if (fabsf(sensor_ray(2)) < 1e-6f) {
|
||||
// z component of measurement unsafe, don't use this measurement
|
||||
return;
|
||||
}
|
||||
|
||||
matrix::Vector3f rel_pos{};
|
||||
|
||||
const float dist_z = vehicle_local_position.dist_bottom - _param_ltest_sens_pos_z.get();
|
||||
|
||||
// scale the ray s.t. the z component has length of _uncertainty_scale
|
||||
// - adjust relative position according to sensor offset
|
||||
rel_pos(0) = (sensor_ray(0) / sensor_ray(2) * dist_z) + _param_ltest_sens_pos_x.get();
|
||||
rel_pos(1) = (sensor_ray(1) / sensor_ray(2) * dist_z) + _param_ltest_sens_pos_x.get();
|
||||
rel_pos(2) = dist_z;
|
||||
|
||||
if (!_estimator_initialized) {
|
||||
float vx_init = -vehicle_local_position.vx;
|
||||
float vy_init = -vehicle_local_position.vy;
|
||||
|
||||
PX4_INFO("IRLock target acquired %.2f %.2f", (double)vx_init, (double)vy_init);
|
||||
|
||||
_kalman_filter_x.init(rel_pos(0), vx_init, _param_ltest_pos_unc_in.get(), _param_ltest_vel_unc_in.get());
|
||||
_kalman_filter_y.init(rel_pos(1), vy_init, _param_ltest_pos_unc_in.get(), _param_ltest_vel_unc_in.get());
|
||||
|
||||
_estimator_initialized = true;
|
||||
_last_predict = vehicle_local_position.timestamp_sample;
|
||||
_last_update = vehicle_local_position.timestamp_sample;
|
||||
|
||||
} else {
|
||||
const float measurement_uncertainty = _param_ltest_meas_unc.get() * dist_z * dist_z;
|
||||
|
||||
update_x = _kalman_filter_x.update(rel_pos(0), measurement_uncertainty);
|
||||
update_y = _kalman_filter_y.update(rel_pos(1), measurement_uncertainty);
|
||||
|
||||
if (!update_x || !update_y) {
|
||||
PX4_DEBUG("IRLock distance measurement rejected (%.3f, %.3f)", (double)rel_pos(0), (double)rel_pos(1));
|
||||
}
|
||||
|
||||
landing_target_pose.timestamp_sample = irlock_report.timestamp;
|
||||
landing_target_pose.z_rel = dist_z;
|
||||
}
|
||||
}
|
||||
|
||||
} else if (_uwb_distance_sub.advertised()) {
|
||||
uwb_distance_s uwb_distance;
|
||||
|
||||
if (_uwb_distance_sub.update(&uwb_distance)) {
|
||||
|
||||
if (!vehicle_local_position.v_xy_valid) {
|
||||
// don't have the data needed for an update
|
||||
return;
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE((float)uwb_distance.position[0])
|
||||
|| !PX4_ISFINITE((float)uwb_distance.position[1])
|
||||
|| !PX4_ISFINITE((float)uwb_distance.position[2])) {
|
||||
|
||||
PX4_WARN("uwb_distance position is corrupt!");
|
||||
return;
|
||||
}
|
||||
|
||||
const matrix::Vector3f position(-uwb_distance.position[0], -uwb_distance.position[1], -uwb_distance.position[2]);
|
||||
|
||||
// The coordinate system is NED (north-east-down)
|
||||
// the uwb_distance msg contains the Position in NED, Vehicle relative to LP
|
||||
// The coordinates "rel_pos_*" are the position of the landing point relative to the vehicle.
|
||||
// To change POV we negate every Axis:
|
||||
if (!_estimator_initialized) {
|
||||
float vx_init = -vehicle_local_position.vx;
|
||||
float vy_init = -vehicle_local_position.vy;
|
||||
|
||||
PX4_INFO("UWB distance target acquired %.2f %.2f", (double)vx_init, (double)vy_init);
|
||||
|
||||
_kalman_filter_x.init(position(0), vx_init,
|
||||
_param_ltest_pos_unc_in.get(), _param_ltest_vel_unc_in.get());
|
||||
|
||||
_kalman_filter_y.init(position(1), vy_init,
|
||||
_param_ltest_pos_unc_in.get(), _param_ltest_vel_unc_in.get());
|
||||
|
||||
_estimator_initialized = true;
|
||||
_last_predict = vehicle_local_position.timestamp_sample;
|
||||
_last_update = vehicle_local_position.timestamp_sample;
|
||||
|
||||
} else {
|
||||
|
||||
float measurement_uncertainty = _param_ltest_meas_unc.get();
|
||||
|
||||
update_x = _kalman_filter_x.update(position(0), measurement_uncertainty);
|
||||
update_y = _kalman_filter_y.update(position(1), measurement_uncertainty);
|
||||
|
||||
if (!update_x || !update_y) {
|
||||
PX4_DEBUG("UWB distance measurement rejected (%.3f, %.3f)", (double)position(1), (double)position(1));
|
||||
}
|
||||
|
||||
landing_target_pose.timestamp_sample = uwb_distance.timestamp;
|
||||
landing_target_pose.z_rel = uwb_distance.position[2];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (vehicle_local_position.timestamp_sample > _last_update + TARGET_UPDATE_TIMEOUT_US) {
|
||||
if (_estimator_initialized) {
|
||||
PX4_INFO("Target lost");
|
||||
_estimator_initialized = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (_estimator_initialized) {
|
||||
// publish landing_target_pose
|
||||
if (update_x && update_y) {
|
||||
_last_update = vehicle_local_position.timestamp_sample;
|
||||
|
||||
// only publish if both measurements were good
|
||||
landing_target_pose.is_static = ((TargetMode)_param_ltest_mode.get() == TargetMode::Stationary);
|
||||
|
||||
landing_target_pose.rel_pos_valid = true;
|
||||
landing_target_pose.rel_vel_valid = true;
|
||||
|
||||
landing_target_pose.x_rel = _kalman_filter_x.getState()(0);
|
||||
landing_target_pose.vx_rel = _kalman_filter_x.getState()(1);
|
||||
landing_target_pose.cov_x_rel = _kalman_filter_x.getCovariance()(0, 0);
|
||||
landing_target_pose.cov_vx_rel = _kalman_filter_x.getCovariance()(1, 1);
|
||||
|
||||
landing_target_pose.y_rel = _kalman_filter_y.getState()(0);
|
||||
landing_target_pose.vy_rel = _kalman_filter_y.getState()(1);
|
||||
landing_target_pose.cov_y_rel = _kalman_filter_y.getCovariance()(0, 0);
|
||||
landing_target_pose.cov_vy_rel = _kalman_filter_y.getCovariance()(1, 1);
|
||||
|
||||
if (vehicle_local_position.xy_valid) {
|
||||
landing_target_pose.x_abs = landing_target_pose.x_rel + vehicle_local_position.x;
|
||||
landing_target_pose.y_abs = landing_target_pose.y_rel + vehicle_local_position.y;
|
||||
landing_target_pose.z_abs = landing_target_pose.z_rel + vehicle_local_position.z;
|
||||
landing_target_pose.abs_pos_valid = true;
|
||||
|
||||
} else {
|
||||
landing_target_pose.x_abs = NAN;
|
||||
landing_target_pose.y_abs = NAN;
|
||||
landing_target_pose.z_abs = NAN;
|
||||
landing_target_pose.abs_pos_valid = false;
|
||||
}
|
||||
|
||||
landing_target_pose.timestamp = hrt_absolute_time();
|
||||
_landing_target_pose_pub.publish(landing_target_pose);
|
||||
}
|
||||
|
||||
// publish landing_target_innovations
|
||||
if (landing_target_pose.timestamp_sample != 0) {
|
||||
|
||||
landing_target_innovations_s landing_target_innovations{};
|
||||
_kalman_filter_x.getInnovations(landing_target_innovations.innov_x, landing_target_innovations.innov_cov_x);
|
||||
_kalman_filter_y.getInnovations(landing_target_innovations.innov_y, landing_target_innovations.innov_cov_y);
|
||||
|
||||
landing_target_innovations.timestamp_sample = landing_target_pose.timestamp_sample;
|
||||
landing_target_innovations.timestamp = hrt_absolute_time();
|
||||
_landing_target_innovations_pub.publish(landing_target_innovations);
|
||||
}
|
||||
}
|
||||
|
||||
perf_end(_cycle_perf);
|
||||
}
|
||||
|
||||
int LandingTargetEstimator::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
LandingTargetEstimator *instance = new LandingTargetEstimator();
|
||||
|
||||
if (!instance) {
|
||||
PX4_ERR("driver allocation failed");
|
||||
return PX4_ERROR;
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init()) {
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("alloc failed");
|
||||
}
|
||||
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
|
||||
instance->start();
|
||||
return 0;
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
int LandingTargetEstimator::custom_command(int argc, char *argv[])
|
||||
{
|
||||
return print_usage("unknown command");
|
||||
}
|
||||
|
||||
int LandingTargetEstimator::print_usage(const char *reason)
|
||||
{
|
||||
if (reason) {
|
||||
printf("%s\n\n", reason);
|
||||
PX4_WARN("%s\n", reason);
|
||||
}
|
||||
|
||||
PRINT_MODULE_DESCRIPTION(
|
||||
R"DESCR_STR(
|
||||
### Description
|
||||
Background process running periodically on the LP work queue which filters and estimates a targets position.
|
||||
Target position estimator using either IRLock or UWB (ultra wide band) data.
|
||||
|
||||
)DESCR_STR");
|
||||
|
||||
PRINT_MODULE_USAGE_NAME("landing_target_estimator", "system");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_COMMAND("stop");
|
||||
PRINT_MODULE_USAGE_COMMAND("status");
|
||||
PRINT_MODULE_USAGE_NAME("landing_target_estimator", "estimator");
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int LandingTargetEstimator::custom_command(int argc, char *argv[])
|
||||
{
|
||||
if (!is_running()) {
|
||||
PX4_INFO("not running");
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
return print_usage("Unrecognized command.");
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int landing_target_estimator_main(int argc, char *argv[])
|
||||
{
|
||||
return LandingTargetEstimator::main(argc, argv);
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2018 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2018-2022 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
|
||||
@ -34,25 +34,27 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "KalmanFilter.hpp"
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/conversion/rotation.h>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <lib/matrix/matrix/math.hpp>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/SubscriptionInterval.hpp>
|
||||
#include <uORB/topics/vehicle_acceleration.h>
|
||||
#include <uORB/topics/irlock_report.h>
|
||||
#include <uORB/topics/landing_target_innovations.h>
|
||||
#include <uORB/topics/landing_target_pose.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/uwb_distance.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/irlock_report.h>
|
||||
#include <uORB/topics/landing_target_pose.h>
|
||||
#include <uORB/topics/landing_target_innovations.h>
|
||||
#include <uORB/topics/uwb_distance.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <matrix/math.hpp>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <matrix/Matrix.hpp>
|
||||
#include <lib/conversion/rotation.h>
|
||||
#include "KalmanFilter.h"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
@ -60,90 +62,61 @@ class LandingTargetEstimator : public ModuleBase<LandingTargetEstimator>, public
|
||||
public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
|
||||
LandingTargetEstimator();
|
||||
virtual ~LandingTargetEstimator() = default;
|
||||
|
||||
static int print_usage(const char *reason = nullptr);
|
||||
static int custom_command(int argc, char *argv[]);
|
||||
~LandingTargetEstimator() override;
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
||||
int start();
|
||||
/** @see ModuleBase */
|
||||
static int custom_command(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int print_usage(const char *reason = nullptr);
|
||||
|
||||
bool init();
|
||||
|
||||
private:
|
||||
|
||||
void Run() override;
|
||||
|
||||
void _check_params(const bool force);
|
||||
void _update_topics();
|
||||
void _update_params();
|
||||
|
||||
enum class TargetMode {
|
||||
Moving = 0,
|
||||
Stationary
|
||||
};
|
||||
|
||||
static constexpr uint32_t TARGET_UPDATE_TIMEOUT_US{2000000};
|
||||
static constexpr uint32_t SAMPLE_RATE{50}; // samples per second
|
||||
static constexpr uint32_t TARGET_UPDATE_TIMEOUT_US{2_s};
|
||||
|
||||
uORB::Publication<landing_target_pose_s> _targetPosePub{ORB_ID(landing_target_pose)};
|
||||
landing_target_pose_s _target_pose{};
|
||||
|
||||
uORB::Publication<landing_target_innovations_s> _targetInnovationsPub{ORB_ID(landing_target_innovations)};
|
||||
landing_target_innovations_s _target_innovations{};
|
||||
uORB::Publication<landing_target_pose_s> _landing_target_pose_pub{ORB_ID(landing_target_pose)};
|
||||
uORB::Publication<landing_target_innovations_s> _landing_target_innovations_pub{ORB_ID(landing_target_innovations)};
|
||||
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
|
||||
struct {
|
||||
hrt_abstime timestamp;
|
||||
float rel_pos_x;
|
||||
float rel_pos_y;
|
||||
float rel_pos_z;
|
||||
} _target_position_report;
|
||||
uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)};
|
||||
|
||||
uORB::Subscription _vehicleLocalPositionSub{ORB_ID(vehicle_local_position)};
|
||||
uORB::Subscription _attitudeSub{ORB_ID(vehicle_attitude)};
|
||||
uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
|
||||
uORB::Subscription _irlockReportSub{ORB_ID(irlock_report)};
|
||||
uORB::Subscription _uwbDistanceSub{ORB_ID(uwb_distance)};
|
||||
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
|
||||
uORB::Subscription _irlock_report_sub{ORB_ID(irlock_report)};
|
||||
uORB::Subscription _uwb_distance_sub{ORB_ID(uwb_distance)};
|
||||
|
||||
vehicle_local_position_s _vehicleLocalPosition{};
|
||||
vehicle_attitude_s _vehicleAttitude{};
|
||||
vehicle_acceleration_s _vehicle_acceleration{};
|
||||
irlock_report_s _irlockReport{};
|
||||
uwb_distance_s _uwbDistance{};
|
||||
|
||||
// keep track of which topics we have received
|
||||
bool _vehicleLocalPosition_valid{false};
|
||||
bool _vehicleAttitude_valid{false};
|
||||
bool _vehicle_acceleration_valid{false};
|
||||
bool _new_irlockReport{false};
|
||||
bool _new_sensorReport{false};
|
||||
bool _estimator_initialized{false};
|
||||
// keep track of whether last measurement was rejected
|
||||
bool _faulty{false};
|
||||
|
||||
matrix::Dcmf _R_att; //Orientation of the body frame
|
||||
matrix::Dcmf _S_att; //Orientation of the sensor relative to body frame
|
||||
matrix::Vector2f _rel_pos;
|
||||
KalmanFilter _kalman_filter_x;
|
||||
KalmanFilter _kalman_filter_y;
|
||||
KalmanFilter _kalman_filter_x{};
|
||||
KalmanFilter _kalman_filter_y{};
|
||||
hrt_abstime _last_predict{0}; // timestamp of last filter prediction
|
||||
hrt_abstime _last_update{0}; // timestamp of last filter update (used to check timeout)
|
||||
float _dist_z{1.0f};
|
||||
|
||||
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::LTEST_MODE>) _param_mode,
|
||||
(ParamFloat<px4::params::LTEST_ACC_UNC>) _param_acc_unc,
|
||||
(ParamFloat<px4::params::LTEST_MEAS_UNC>) _param_meas_unc,
|
||||
(ParamFloat<px4::params::LTEST_POS_UNC_IN>) _param_pos_unc_in,
|
||||
(ParamFloat<px4::params::LTEST_VEL_UNC_IN>) _param_vel_unc_in,
|
||||
(ParamFloat<px4::params::LTEST_SCALE_X>) _param_scale_x,
|
||||
(ParamFloat<px4::params::LTEST_SCALE_Y>) _param_scale_y,
|
||||
(ParamInt<px4::params::LTEST_SENS_ROT>) _param_sens_rot,
|
||||
(ParamFloat<px4::params::LTEST_SENS_POS_X>) _param_sens_pos_x,
|
||||
(ParamFloat<px4::params::LTEST_SENS_POS_Y>) _param_sens_pos_y,
|
||||
(ParamFloat<px4::params::LTEST_SENS_POS_Z>) _param_sens_pos_z
|
||||
(ParamInt<px4::params::LTEST_MODE>) _param_ltest_mode,
|
||||
(ParamFloat<px4::params::LTEST_ACC_UNC>) _param_ltest_acc_unc,
|
||||
(ParamFloat<px4::params::LTEST_MEAS_UNC>) _param_ltest_meas_unc,
|
||||
(ParamFloat<px4::params::LTEST_POS_UNC_IN>) _param_ltest_pos_unc_in,
|
||||
(ParamFloat<px4::params::LTEST_VEL_UNC_IN>) _param_ltest_vel_unc_in,
|
||||
(ParamFloat<px4::params::LTEST_SCALE_X>) _param_ltest_scale_x,
|
||||
(ParamFloat<px4::params::LTEST_SCALE_Y>) _param_ltest_scale_y,
|
||||
(ParamInt<px4::params::LTEST_SENS_ROT>) _param_ltest_sens_rot,
|
||||
(ParamFloat<px4::params::LTEST_SENS_POS_X>) _param_ltest_sens_pos_x,
|
||||
(ParamFloat<px4::params::LTEST_SENS_POS_Y>) _param_ltest_sens_pos_y,
|
||||
(ParamFloat<px4::params::LTEST_SENS_POS_Z>) _param_ltest_sens_pos_z
|
||||
)
|
||||
};
|
||||
Loading…
x
Reference in New Issue
Block a user