mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-09 17:08:52 +08:00
added prior update and posterior update
This commit is contained in:
Marvin Harms
@@ -47,7 +47,7 @@ using matrix::wrap_pi;
|
||||
FixedwingShearEstimator::FixedwingShearEstimator() :
|
||||
ModuleParams(nullptr),
|
||||
WorkItem(MODULE_NAME, px4::wq_configurations::test1),
|
||||
_soaring_estimator_shear_pub(ORB_ID(soaring_estimator_shear))
|
||||
_soaring_estimator_shear_pub(ORB_ID(soaring_estimator_shear)),
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
|
||||
{
|
||||
// limit to 10 Hz
|
||||
@@ -66,12 +66,41 @@ FixedwingShearEstimator::~FixedwingShearEstimator()
|
||||
bool
|
||||
FixedwingShearEstimator::init()
|
||||
{
|
||||
if (!_vehicle_angular_velocity_sub.registerCallback()) {
|
||||
if (!_soaring_controller_wind_sub.registerCallback()) {
|
||||
PX4_ERR("vehicle position callback registration failed!");
|
||||
return false;
|
||||
}
|
||||
PX4_INFO("Starting FW_DYN_SOAR_ESTIMATOR");
|
||||
return true;
|
||||
|
||||
// init horizontal wind field
|
||||
for (uint i=0; i<6; i++){
|
||||
_X_prior_horizontal(i) = 0.f;
|
||||
_X_posterior_horizontal(i) = 0.f;
|
||||
for (uint j=0; j<6; j++){
|
||||
_A_horizontal(i,j) = 0.f;
|
||||
}
|
||||
}
|
||||
_P_prior_horizontal = _Q_horizontal;
|
||||
_P_posterior_horizontal = _Q_horizontal;
|
||||
|
||||
// init vertical wind field
|
||||
for (uint i=0; i<_dim_vertical; i++){
|
||||
_X_prior_vertical(i) = 0.f;
|
||||
_X_posterior_vertical(i) = 0.f;
|
||||
for (uint j=0; j<_dim_vertical; j++){
|
||||
_A_vertical(i,j) = 0.f;
|
||||
}
|
||||
}
|
||||
_P_prior_vertical = _Q_vertical;
|
||||
_P_posterior_vertical = _Q_vertical;
|
||||
|
||||
// init time
|
||||
_last_run = hrt_absolute_time();
|
||||
|
||||
// init reset counter
|
||||
_reset_counter = 0;
|
||||
|
||||
}
|
||||
|
||||
int
|
||||
@@ -90,11 +119,11 @@ FixedwingShearEstimator::parameters_update()
|
||||
_R_horizontal(0,0) = powf(_param_sigma_r_vel.get(),2);
|
||||
_R_horizontal(1,1) = powf(_param_sigma_r_vel.get(),2);
|
||||
|
||||
for (int i=0;i<_dim_vertical;i++){
|
||||
for (int i=0;i<(int)_dim_vertical;i++){
|
||||
_Q_vertical(i,i) = powf(_param_sigma_q_vel.get(),2);
|
||||
}
|
||||
|
||||
_R_vertical = powf(_param_sigma_r_vel.get(),2);
|
||||
_R_vertical(0,0) = powf(_param_sigma_r_vel.get(),2);
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
@@ -105,17 +134,17 @@ FixedwingShearEstimator::reset_filter()
|
||||
// reset all states of the filter to some initial guess.
|
||||
|
||||
// reset horizontal wind state
|
||||
for (int i=0;i<6;i++){
|
||||
_X_prior_horizontal(i,i) = 0.0f;
|
||||
_X_posterior_horizontal(i,i) = 0.0f;
|
||||
for (uint i=0;i<6;i++){
|
||||
_X_prior_horizontal(i) = 0.0f;
|
||||
_X_posterior_horizontal(i) = 0.0f;
|
||||
_P_prior_horizontal(i,i) = 1.0f;
|
||||
_P_posterior_horizontal(i,i) = 1.0f;
|
||||
}
|
||||
|
||||
// reset vertical wind state
|
||||
for (int i=0;i<_dim_vertical;i++){
|
||||
_X_prior_vertical(i,i) = 0.0f;
|
||||
_X_posterior_vertical(i,i) = 0.0f;
|
||||
for (uint i=0;i<_dim_vertical;i++){
|
||||
_X_prior_vertical(i) = 0.0f;
|
||||
_X_posterior_vertical(i) = 0.0f;
|
||||
_P_prior_vertical(i,i) = 1.0f;
|
||||
_P_posterior_vertical(i,i) = 1.0f;
|
||||
}
|
||||
@@ -124,17 +153,116 @@ FixedwingShearEstimator::reset_filter()
|
||||
void
|
||||
FixedwingShearEstimator::perform_prior_update()
|
||||
{
|
||||
// get time since last run
|
||||
float dt = (hrt_absolute_time() - _last_run)/1000000.f;
|
||||
_last_run = hrt_absolute_time();
|
||||
|
||||
// perform prior update assuming trivial dynamics of the wind field (mean field stays the same)
|
||||
_X_prior_horizontal = _X_posterior_horizontal;
|
||||
_X_prior_vertical = _X_posterior_vertical;
|
||||
_P_prior_horizontal = _P_posterior_horizontal + dt*_Q_horizontal;
|
||||
_P_prior_vertical = _P_posterior_vertical + dt*_Q_vertical;
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingShearEstimator::perform_posterior_update()
|
||||
FixedwingShearEstimator::perform_posterior_update(float height, Vector3f wind)
|
||||
{
|
||||
|
||||
//
|
||||
bool error = false;
|
||||
|
||||
// first fill the horizontal observation matrix:
|
||||
float vx = _X_prior_horizontal(0);
|
||||
float vy = _X_prior_horizontal(1);
|
||||
float h = _X_prior_horizontal(4);
|
||||
float a = _X_prior_horizontal(5);
|
||||
_H_horizontal(0,0) = 1.f/(1.f + expf(-(height-h)*a));
|
||||
_H_horizontal(1,1) = 1.f/(1.f + expf(-(height-h)*a));
|
||||
_H_horizontal(0,2) = 1.f;
|
||||
_H_horizontal(1,3) = 1.f;
|
||||
_H_horizontal(0,4) = -((a*vx*expf(-(height-h)*a)))/powf(1.f + expf(-(height-h)*a),2);
|
||||
_H_horizontal(1,4) = -((a*vy*expf(-(height-h)*a)))/powf(1.f + expf(-(height-h)*a),2);
|
||||
_H_horizontal(0,5) = -((vx*(h-height))*expf(-(height-h)*a))/powf(1.f + expf(-(height-h)*a),2);
|
||||
_H_horizontal(1,5) = -((vx*(h-height))*expf(-(height-h)*a))/powf(1.f + expf(-(height-h)*a),2);
|
||||
|
||||
// then fill the vertical observation matrix
|
||||
for (uint i=0;i<_dim_vertical;i++){
|
||||
_H_vertical(0,i) = powf(height,i);
|
||||
}
|
||||
|
||||
// compute Kalman gain matrix for horizontal wind states
|
||||
Matrix<float, 6, 2> tmp1_horizontal = _P_prior_horizontal*_H_horizontal.T();
|
||||
Matrix<float, 2, 2> tmp2_horizontal = _H_horizontal*_P_prior_horizontal*_H_horizontal.T() + _R_horizontal;
|
||||
Matrix<float, 2, 2> inv_horizontal;
|
||||
inv_horizontal(0,0) = tmp2_horizontal(1,1);
|
||||
inv_horizontal(0,1) = -tmp2_horizontal(0,1);
|
||||
inv_horizontal(1,1) = tmp2_horizontal(0,0);
|
||||
inv_horizontal(1,0) = -tmp2_horizontal(1,0);
|
||||
float determinant_horizontal = tmp2_horizontal(0,0)*tmp2_horizontal(1,1) - tmp2_horizontal(1,0)*tmp2_horizontal(0,1);
|
||||
if (fabs(determinant_horizontal)>0.000001f){
|
||||
inv_horizontal /= determinant_horizontal;
|
||||
_K_horizontal = tmp1_horizontal*inv_horizontal;
|
||||
}
|
||||
else{
|
||||
PX4_ERR("singular horizontal matrix, resetting filter");
|
||||
error = true;
|
||||
}
|
||||
|
||||
// then fill the vertical observation matrix
|
||||
for (uint i=0;i<_dim_vertical;i++){
|
||||
_H_vertical(0,i) = powf(height,i);
|
||||
}
|
||||
|
||||
// compute Kalman gain matrix for vertical wind states
|
||||
Matrix<float, _dim_vertical, 1> tmp1_vertical = _P_prior_vertical*_H_vertical.T();
|
||||
Matrix<float, 1, 1> tmp2_vertical = _H_vertical*_P_prior_vertical*_H_vertical.T() + _R_vertical;
|
||||
Matrix<float, 1, 1> inv_vertical;
|
||||
float determinant_vertical = tmp2_vertical(0,0);
|
||||
if (fabs(determinant_vertical)>0.000001f){
|
||||
inv_vertical(0,0) = 1.f / determinant_vertical;
|
||||
_K_vertical = tmp1_vertical*inv_vertical;
|
||||
}
|
||||
else{
|
||||
PX4_ERR("singular vertical matrix, resetting filter");
|
||||
error = true;
|
||||
}
|
||||
|
||||
if (error) {
|
||||
reset_filter();
|
||||
_reset_counter += 1;
|
||||
}
|
||||
else {
|
||||
// perform horizontal update
|
||||
Vector<float, 2> z_expected_horizontal = (Vector<float, 2>) (_H_horizontal*_X_prior_horizontal);
|
||||
Vector<float, 2> wind_horizontal;
|
||||
Matrix<float, 6, 6> identity_1;
|
||||
wind_horizontal(0) = wind(0);
|
||||
wind_horizontal(1) = wind(1);
|
||||
identity_1.setIdentity();
|
||||
_X_posterior_horizontal = _X_prior_horizontal + _K_horizontal*(wind_horizontal - z_expected_horizontal);
|
||||
_P_posterior_horizontal = (identity_1 - _K_horizontal*_H_horizontal)*_P_prior_horizontal;
|
||||
|
||||
// perform vertical update
|
||||
Vector<float, 1> z_expected_vertical = (Vector<float, 1>) (_H_vertical*_X_prior_vertical);
|
||||
Vector<float, 1> wind_vertical;
|
||||
Matrix<float, _dim_vertical, _dim_vertical> identity_2;
|
||||
wind_vertical(0) = wind(2);
|
||||
identity_2.setIdentity();
|
||||
_X_posterior_vertical = _X_prior_vertical + _K_vertical*(wind_vertical - z_expected_vertical);
|
||||
_P_posterior_vertical = (identity_2 - _K_vertical*_H_vertical)*_P_prior_vertical;
|
||||
}
|
||||
|
||||
// find the correct sign of params (parametrization is not unique)
|
||||
if (_X_posterior_horizontal(5)<0.f) {
|
||||
_X_posterior_horizontal(0) *= -1.f;
|
||||
_X_posterior_horizontal(1) *= -1.f;
|
||||
_X_posterior_horizontal(5) *= -1.f;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionINDIControl::Run()
|
||||
FixedwingShearEstimator::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
_soaring_controller_wind_sub.unregisterCallback();
|
||||
@@ -145,7 +273,7 @@ FixedwingPositionINDIControl::Run()
|
||||
perf_begin(_loop_perf);
|
||||
|
||||
// only run controller if wind info changed
|
||||
if (_soaring_controller_wind_sub.update(&soaring_controller_wind))
|
||||
if (_soaring_controller_wind_sub.update(&_soaring_controller_wind))
|
||||
{
|
||||
// only update parameters if they changed
|
||||
bool params_updated = _parameter_update_sub.updated();
|
||||
@@ -162,8 +290,8 @@ FixedwingPositionINDIControl::Run()
|
||||
}
|
||||
|
||||
// get current measurement
|
||||
_current_wind = Vector3f(soaring_controller_wind.wind_estimate_filtered);
|
||||
_current_height = Vector3f(soaring_controller_wind.position)(2);
|
||||
_current_wind = Vector3f(_soaring_controller_wind.wind_estimate_filtered);
|
||||
_current_height = Vector3f(_soaring_controller_wind.position)(2);
|
||||
|
||||
// prior update
|
||||
perform_prior_update();
|
||||
@@ -175,9 +303,78 @@ FixedwingPositionINDIControl::Run()
|
||||
// maybe reset filters...
|
||||
|
||||
// publish shear params
|
||||
// ========================================
|
||||
// publish controller position in ENU frame
|
||||
// ========================================
|
||||
_soaring_estimator_shear.timestamp = hrt_absolute_time();
|
||||
_soaring_estimator_shear.vx = _X_posterior_horizontal(0);
|
||||
_soaring_estimator_shear.vy = _X_posterior_horizontal(1);
|
||||
_soaring_estimator_shear.bx = _X_posterior_horizontal(2);
|
||||
_soaring_estimator_shear.bx = _X_posterior_horizontal(3);
|
||||
_soaring_estimator_shear.h = _X_posterior_horizontal(4);
|
||||
_soaring_estimator_shear.a = _X_posterior_horizontal(5);
|
||||
_soaring_estimator_shear.reset_counter = _reset_counter;
|
||||
_soaring_estimator_shear_pub.publish(_soaring_estimator_shear);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
int FixedwingShearEstimator::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
FixedwingShearEstimator *instance = new FixedwingShearEstimator();
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init()) {
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("alloc failed");
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
|
||||
int FixedwingShearEstimator::custom_command(int argc, char *argv[])
|
||||
{
|
||||
return print_usage("unknown command");
|
||||
}
|
||||
|
||||
int FixedwingShearEstimator::print_usage(const char *reason)
|
||||
{
|
||||
if (reason) {
|
||||
PX4_WARN("%s\n", reason);
|
||||
}
|
||||
|
||||
PRINT_MODULE_DESCRIPTION(
|
||||
R"DESCR_STR(
|
||||
### Description
|
||||
fw_dyn_soar_estimator is the fixed wing shear estimator for dynamic soaring.
|
||||
|
||||
)DESCR_STR");
|
||||
|
||||
PRINT_MODULE_USAGE_NAME("fw_dyn_soar_estimator", "estimator");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true);
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int fw_dyn_soar_estimator_main(int argc, char *argv[])
|
||||
{
|
||||
return FixedwingShearEstimator::main(argc, argv);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -96,17 +96,23 @@ private:
|
||||
void perform_posterior_update(float height, Vector3f wind);
|
||||
bool check_plausibility();
|
||||
void publish_estimate();
|
||||
void status_publish();
|
||||
|
||||
// control variables
|
||||
hrt_abstime _last_run{0};
|
||||
uint _reset_counter = {};
|
||||
const static size_t _dim_vertical = 2; // order of vertical approximation function for vertical wind
|
||||
|
||||
Vector<float, 6> _X_prior_horizontal= {};
|
||||
Matrix<float, 6, 6> _P_prior_horizontal = {};
|
||||
Vector<float, 6> _X_posterior_horizontal= {};
|
||||
Matrix<float, 6, 6> _P_prosterior_horizontal = {};
|
||||
Matrix<float, 6, 6> _P_posterior_horizontal = {};
|
||||
Matrix<float, 6, 6> _Q_horizontal = {};
|
||||
Matrix<float, 2, 2> _R_horizontal = {};
|
||||
Matrix<float, 2, 6> _H_horizontal = {};
|
||||
Matrix<float, 6, 6> _A_horizontal = {};
|
||||
Matrix<float, 6, 2> _K_horizontal = {};
|
||||
|
||||
|
||||
Vector<float, _dim_vertical> _X_prior_vertical= {};
|
||||
Matrix<float, _dim_vertical, _dim_vertical> _P_prior_vertical = {};
|
||||
@@ -114,8 +120,10 @@ private:
|
||||
Matrix<float, _dim_vertical, _dim_vertical> _P_posterior_vertical = {};
|
||||
Vector<float, _dim_vertical> _X_vertical = {}; // params of vertical wind
|
||||
Matrix<float, _dim_vertical, _dim_vertical> _Q_vertical = {};
|
||||
float _R_vertical = {};
|
||||
Matrix<float, 1, 1> _R_vertical = {};
|
||||
Matrix<float, 1, _dim_vertical> _H_vertical = {};
|
||||
Matrix<float, _dim_vertical, _dim_vertical> _A_vertical = {};
|
||||
Matrix<float, _dim_vertical, 1> _K_vertical = {};
|
||||
|
||||
// measurement variables
|
||||
Vector3f _current_wind = {};
|
||||
|
||||
@@ -16,13 +16,13 @@
|
||||
* This is the std dev of the wind velocity in each direction
|
||||
*
|
||||
* @unit kg
|
||||
* @min 0.01
|
||||
* @min 0.0
|
||||
* @max 10
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @decimal 6
|
||||
* @increment 0.000001
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(DS_SIGMA_Q_V, 1.f);
|
||||
PARAM_DEFINE_FLOAT(DS_SIGMA_Q_V, 0.0001f);
|
||||
|
||||
/**
|
||||
* Standard deviation of vertical shear position
|
||||
@@ -30,13 +30,13 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_Q_V, 1.f);
|
||||
* This is the std dev of the shear vertical position
|
||||
*
|
||||
* @unit kg
|
||||
* @min 0.01
|
||||
* @min 0.0
|
||||
* @max 10
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @decimal 6
|
||||
* @increment 0.000001
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(DS_SIGMA_Q_H, 1.f);
|
||||
PARAM_DEFINE_FLOAT(DS_SIGMA_Q_H, 0.001f);
|
||||
|
||||
/**
|
||||
* Standard deviation of velicity state in shear model
|
||||
@@ -44,13 +44,13 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_Q_H, 1.f);
|
||||
* This is the std dev of the shear strenght param
|
||||
*
|
||||
* @unit kg
|
||||
* @min 0.01
|
||||
* @min 0.0
|
||||
* @max 10
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @decimal 6
|
||||
* @increment 0.000001
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(DS_SIGMA_Q_A, 1.f);
|
||||
PARAM_DEFINE_FLOAT(DS_SIGMA_Q_A, 0.00003f);
|
||||
|
||||
/**
|
||||
* Standard deviation of velicity measurement (wind)
|
||||
@@ -58,10 +58,10 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_Q_A, 1.f);
|
||||
* This is the std dev of the wind pseudomeasurement passed to the EKF
|
||||
*
|
||||
* @unit kg
|
||||
* @min 0.01
|
||||
* @min 0.0
|
||||
* @max 10
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @decimal 6
|
||||
* @increment 0.000001
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(DS_SIGMA_R_V, 1.f);
|
||||
Reference in New Issue
Block a user