From aa09918f0d89172235ff172a8eb9eddbbcce3b47 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 6 Oct 2022 18:54:29 +0200 Subject: [PATCH] LTE: refactor to use multiple target estimator types --- .../landing_target_estimator/KalmanFilter.cpp | 50 +------- .../landing_target_estimator/KalmanFilter.h | 58 ++------- .../LandingTargetEstimator.cpp | 119 +++++++++++++----- .../LandingTargetEstimator.h | 7 +- .../target_estimator.h | 69 ++++++++++ 5 files changed, 174 insertions(+), 129 deletions(-) create mode 100644 src/modules/landing_target_estimator/target_estimator.h diff --git a/src/modules/landing_target_estimator/KalmanFilter.cpp b/src/modules/landing_target_estimator/KalmanFilter.cpp index e4636b6e0f..913b98672c 100644 --- a/src/modules/landing_target_estimator/KalmanFilter.cpp +++ b/src/modules/landing_target_estimator/KalmanFilter.cpp @@ -43,30 +43,8 @@ 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) +void KalmanFilter::predict(float dt, float acc) { _x(0) += _x(1) * dt + dt * dt / 2 * acc; _x(1) += acc * dt; @@ -80,12 +58,12 @@ void KalmanFilter::predict(float dt, float acc, float acc_unc) G(0, 0) = dt * dt / 2; G(1, 0) = dt; - matrix::Matrix process_noise = G * G.transpose() * acc_unc; + matrix::Matrix process_noise = G * G.transpose() * _acc_var; _covariance = A * _covariance * A.transpose() + process_noise; } -bool KalmanFilter::update(float meas, float measUnc) +bool KalmanFilter::fusePosition(float meas, float measUnc) { // H = [1, 0] @@ -122,28 +100,6 @@ bool KalmanFilter::update(float meas, float measUnc) } -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; diff --git a/src/modules/landing_target_estimator/KalmanFilter.h b/src/modules/landing_target_estimator/KalmanFilter.h index 3d466d6b06..2ef3329ce9 100644 --- a/src/modules/landing_target_estimator/KalmanFilter.h +++ b/src/modules/landing_target_estimator/KalmanFilter.h @@ -51,11 +51,13 @@ #include #include +#include "target_estimator.h" + #pragma once namespace landing_target_estimator { -class KalmanFilter +class KalmanFilter : public TargetEstimator { public: /** @@ -63,31 +65,13 @@ public: */ 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); + void setPosition(float pos) override { _x(0) = pos; } + void setVelocity(float vel) override { _x(1) = vel; } /** * Predict the state with an external acceleration estimate @@ -95,7 +79,7 @@ public: * @param acc Acceleration estimate * @param acc_unc Variance of acceleration estimate */ - void predict(float dt, float acc, float acc_unc); + void predict(float dt, float acc) override; /** * Update the state estimate with a measurement @@ -103,33 +87,13 @@ public: * @param measUnc measurement uncertainty * @return update success (measurement not rejected) */ - bool update(float meas, float measUnc); + bool fusePosition(float meas, float measUnc) override; - /** - * Get the current filter state - * @param x1 State - */ - void getState(matrix::Vector &state); + float getPosition() override { return _x(0); } + float getVelocity() override { return _x(1); } - /** - * 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); + float getPosVar() override { return _covariance(0, 0); } + float getVelVar() override { return _covariance(0, 0); } /** * Get measurement innovation and covariance of last update call diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp index 091da87bce..1f95fffe82 100644 --- a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp +++ b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp @@ -50,16 +50,28 @@ namespace landing_target_estimator { +using namespace matrix; + LandingTargetEstimator::LandingTargetEstimator() : ModuleParams(nullptr) { _check_params(true); } +LandingTargetEstimator::~LandingTargetEstimator() +{ + delete _target_estimator[0]; + delete _target_estimator[1]; +} + void LandingTargetEstimator::update() { _check_params(false); + if ((_target_estimator[0] == nullptr) || (_target_estimator[1] == nullptr)) { + return; + } + _update_topics(); /* predict */ @@ -83,8 +95,8 @@ void LandingTargetEstimator::update() a.zero(); } - _kalman_filter_x.predict(dt, -a(0), _param_ltest_acc_unc.get()); - _kalman_filter_y.predict(dt, -a(1), _param_ltest_acc_unc.get()); + _target_estimator[0]->predict(dt, -a(0)); + _target_estimator[1]->predict(dt, -a(1)); _last_predict = hrt_absolute_time(); } @@ -100,13 +112,20 @@ void LandingTargetEstimator::update() 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("Init %.2f %.2f", (double)vx_init, (double)vy_init); - _kalman_filter_x.init(_target_position_report.rel_pos_x, vx_init, _param_ltest_pos_unc_in.get(), - _param_ltest_vel_unc_in.get()); - _kalman_filter_y.init(_target_position_report.rel_pos_y, vy_init, _param_ltest_pos_unc_in.get(), - _param_ltest_vel_unc_in.get()); + Vector2f v_init; + v_init(0) = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vx : 0.f; + v_init(1) = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vy : 0.f; + + Vector2f p_init(_target_position_report.rel_pos_x, _target_position_report.rel_pos_y); + + PX4_INFO("Init %.2f %.2f", (double)v_init(0), (double)v_init(1)); + + for (int i = 0; i < 2; i++) { + _target_estimator[i]->setPosition(p_init(i)); + _target_estimator[i]->setVelocity(v_init(i)); + _target_estimator[i]->setStatePosVar(_param_ltest_pos_unc_in.get()); + _target_estimator[i]->setStateVelVar(_param_ltest_vel_unc_in.get()); + } _estimator_initialized = true; _last_update = hrt_absolute_time(); @@ -115,8 +134,8 @@ void LandingTargetEstimator::update() } else { // update const float measurement_uncertainty = _param_ltest_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); + bool update_x = _target_estimator[0]->fusePosition(_target_position_report.rel_pos_x, measurement_uncertainty); + bool update_y = _target_estimator[1]->fusePosition(_target_position_report.rel_pos_y, measurement_uncertainty); if (!update_x || !update_y) { if (!_faulty) { @@ -131,15 +150,11 @@ void LandingTargetEstimator::update() if (!_faulty) { // only publish if both measurements were good + const float x = _target_estimator[0]->getPosition(); + const float y = _target_estimator[1]->getPosition(); + _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_ltest_mode.get() == TargetMode::Stationary); _target_pose.rel_pos_valid = true; @@ -147,14 +162,14 @@ void LandingTargetEstimator::update() _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.vx_rel = _target_estimator[0]->getVelocity(); + _target_pose.vy_rel = _target_estimator[1]->getVelocity(); - _target_pose.cov_x_rel = covx; - _target_pose.cov_y_rel = covy; + _target_pose.cov_x_rel = _target_estimator[0]->getPosVar(); + _target_pose.cov_y_rel = _target_estimator[1]->getPosVar(); - _target_pose.cov_vx_rel = covx_v; - _target_pose.cov_vy_rel = covy_v; + _target_pose.cov_vx_rel = _target_estimator[0]->getVelVar(); + _target_pose.cov_vy_rel = _target_estimator[0]->getVelVar(); if (_vehicleLocalPosition_valid && _vehicleLocalPosition.xy_valid) { _target_pose.x_abs = x + _vehicleLocalPosition.x; @@ -172,15 +187,16 @@ void LandingTargetEstimator::update() _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); + //TODO:fix + /* float innov_x, innov_cov_x, innov_y, innov_cov_y; */ + /* _target_estimator[0]->getInnovations(innov_x, innov_cov_x); */ + /* _target_estimator[1]->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; + /* _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); } @@ -276,7 +292,46 @@ void LandingTargetEstimator::_update_topics() void LandingTargetEstimator::updateParams() { + int32_t current_target_estimator_mode = _param_ltest_mode.get(); ModuleParams::updateParams(); + + if ((current_target_estimator_mode != _param_ltest_mode.get()) || (_target_estimator[0] == nullptr) + || (_target_estimator[1] == nullptr)) { + selectTargetEstimator(); + } + + _target_estimator[0]->setInputAccVar(_param_ltest_acc_unc.get()); + _target_estimator[1]->setInputAccVar(_param_ltest_acc_unc.get()); +} + +void LandingTargetEstimator::selectTargetEstimator() +{ + const TargetMode target_mode = (TargetMode)_param_ltest_mode.get(); + + TargetEstimator *tmp_x = nullptr; + TargetEstimator *tmp_y = nullptr; + + switch (target_mode) { + case TargetMode::Moving: + /* tmp = new xxx */ + break; + + case TargetMode::Stationary: + tmp_x = new KalmanFilter(); + tmp_y = new KalmanFilter(); + break; + } + + if ((tmp_x == nullptr) || (tmp_y == nullptr)) { + PX4_ERR("LTE init failed"); + _param_ltest_mode.set(0); + + } else { + delete _target_estimator[0]; + delete _target_estimator[1]; + _target_estimator[0] = tmp_x; + _target_estimator[1] = tmp_y; + } } } // namespace landing_target_estimator diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.h b/src/modules/landing_target_estimator/LandingTargetEstimator.h index e9e7aeac7d..a2ae185cfd 100644 --- a/src/modules/landing_target_estimator/LandingTargetEstimator.h +++ b/src/modules/landing_target_estimator/LandingTargetEstimator.h @@ -75,7 +75,7 @@ class LandingTargetEstimator: public ModuleParams public: LandingTargetEstimator(); - virtual ~LandingTargetEstimator() = default; + virtual ~LandingTargetEstimator(); /* * Get new measurements and update the state estimate @@ -119,6 +119,8 @@ private: float rel_pos_z; } _target_position_report; + void selectTargetEstimator(); + uORB::Subscription _vehicleLocalPositionSub{ORB_ID(vehicle_local_position)}; uORB::Subscription _attitudeSub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; @@ -145,8 +147,7 @@ private: 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; + TargetEstimator *_target_estimator[2] {nullptr, nullptr}; 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}; diff --git a/src/modules/landing_target_estimator/target_estimator.h b/src/modules/landing_target_estimator/target_estimator.h new file mode 100644 index 0000000000..230c4bb696 --- /dev/null +++ b/src/modules/landing_target_estimator/target_estimator.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * Copyright (c) 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 + * 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 target_estimator.cpp + * @brief Interface for target estimators + */ + +class TargetEstimator +{ +public: + TargetEstimator() = default; + virtual ~TargetEstimator() = default; + + virtual void predict(float dt, float acceleration) = 0; + virtual bool fusePosition(float pos, float var) { return true; } + virtual bool fuseVeloticy(float pos, float var) { return true; } + + virtual void setPosition(float pos) {}; + virtual void setVelocity(float vel) {}; + virtual void setTargetAcc(float acc) {}; + + virtual void setInputAccVar(float var) { _acc_var = var; } + + virtual void setStatePosVar(float var) {}; + virtual void setStateVelVar(float var) {}; + virtual void setStateAccVar(float var) {}; + + virtual float getPosition() { return 0.f; } + virtual float getVelocity() { return 0.f; } + virtual float getAcceleration() { return 0.f; } + + virtual float getPosVar() = 0; + virtual float getVelVar() { return 0.f; } + virtual float getAccVar() { return 0.f; } + +protected: + float _acc_var{}; +};