fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-08-21 14:10:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

LTE: refactor to use multiple target estimator types

Browse Source
This commit is contained in:
bresch
2022-10-06 18:54:29 +02:00
parent ecdc11edf6
commit aa09918f0d
5 changed files with 174 additions and 129 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/landing_target_estimator/KalmanFilter.cpp
+3 -47
View File
@@ -43,30 +43,8 @@
namespace landing_target_estimator
{
KalmanFilter::KalmanFilter(matrix::Vector<float, 2> &initial, matrix::Matrix<float, 2, 2> &covInit)
{
init(initial, covInit);
}
void KalmanFilter::init(matrix::Vector<float, 2> &initial, matrix::Matrix<float, 2, 2> &covInit)
{
_x = initial;
_covariance = covInit;
}
void KalmanFilter::init(float initial0, float initial1, float covInit00, float covInit11)
{
matrix::Vector<float, 2> initial;
initial(0) = initial0;
initial(1) = initial1;
matrix::Matrix<float, 2, 2> 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<float, 2, 2> process_noise = G * G.transpose() * acc_unc;
matrix::Matrix<float, 2, 2> 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<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)
{
innov = _residual;
src/modules/landing_target_estimator/KalmanFilter.h
+11 -47
View File
@@ -51,11 +51,13 @@
#include <matrix/Matrix.hpp>
#include <matrix/Vector.hpp>
#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<float, 2> &initial, matrix::Matrix<float, 2, 2> &covInit);
/**
* Default desctructor
*/
virtual ~KalmanFilter() {};
/**
* Initialize filter state
* @param initial initial state
* @param covInit initial covariance
*/
void init(matrix::Vector<float, 2> &initial, matrix::Matrix<float, 2, 2> &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<float, 2> &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<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);
float getPosVar() override { return _covariance(0, 0); }
float getVelVar() override { return _covariance(0, 0); }
/**
* Get measurement innovation and covariance of last update call
src/modules/landing_target_estimator/LandingTargetEstimator.cpp
+87 -32
View File
@@ -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
src/modules/landing_target_estimator/LandingTargetEstimator.h
+4 -3
View File
@@ -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};
src/modules/landing_target_estimator/target_estimator.h
+69
View File
@@ -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{};
};