mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 05:04:08 +08:00
ekf2: airspeed 2d fusion
This commit is contained in:
Daniel Agar
parent
de9f3a3268
commit
b01467da32
@ -15,6 +15,7 @@ CONFIG_MODULES_DATAMAN=y
|
||||
CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
|
||||
CONFIG_MODULES_EKF2=y
|
||||
CONFIG_EKF2_VERBOSE_STATUS=y
|
||||
CONFIG_EKF2_AIRSPEED2D=y
|
||||
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
|
||||
CONFIG_MODULES_EVENTS=y
|
||||
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
|
||||
|
||||
@ -19,4 +19,4 @@ bool fused # true if the sample was successfully fused
|
||||
|
||||
# TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos estimator_aid_src_aux_global_position
|
||||
# TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow estimator_aid_src_terrain_optical_flow
|
||||
# TOPICS estimator_aid_src_drag
|
||||
# TOPICS estimator_aid_src_airspeed2d estimator_aid_src_drag
|
||||
|
||||
@ -140,6 +140,10 @@ if(CONFIG_EKF2_AIRSPEED)
|
||||
list(APPEND EKF_SRCS EKF/airspeed_fusion.cpp)
|
||||
endif()
|
||||
|
||||
if(CONFIG_EKF2_AIRSPEED2D)
|
||||
list(APPEND EKF_SRCS EKF/aid_sources/Airspeed2D.cpp)
|
||||
endif()
|
||||
|
||||
if(CONFIG_EKF2_AUX_GLOBAL_POSITION)
|
||||
list(APPEND EKF_SRCS EKF/aux_global_position.cpp)
|
||||
endif()
|
||||
|
||||
@ -57,6 +57,10 @@ if(CONFIG_EKF2_AIRSPEED)
|
||||
list(APPEND EKF_SRCS airspeed_fusion.cpp)
|
||||
endif()
|
||||
|
||||
if(CONFIG_EKF2_AIRSPEED2D)
|
||||
list(APPEND EKF_SRCS aid_sources/Airspeed2D.cpp)
|
||||
endif()
|
||||
|
||||
if(CONFIG_EKF2_AUX_GLOBAL_POSITION)
|
||||
list(APPEND EKF_SRCS aux_global_position.cpp)
|
||||
endif()
|
||||
|
||||
188
src/modules/ekf2/EKF/aid_sources/Airspeed2D.cpp
Normal file
188
src/modules/ekf2/EKF/aid_sources/Airspeed2D.cpp
Normal file
@ -0,0 +1,188 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "Airspeed2D.hpp"
|
||||
|
||||
#include "../ekf.h"
|
||||
|
||||
#include <ekf_derivation/generated/compute_airspeed_2d_innov_innov_var_and_hx.h>
|
||||
#include <ekf_derivation/generated/compute_airspeed_2d_y_innov_innov_var_and_hy.h>
|
||||
|
||||
bool Airspeed2D::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
|
||||
{
|
||||
|
||||
#if defined(MODULE_NAME)
|
||||
|
||||
if (_sensor_airflow_sub.updated()) {
|
||||
sensor_airflow_s sensor_airflow;
|
||||
|
||||
if (_sensor_airflow_sub.copy(&sensor_airflow)) {
|
||||
|
||||
AirflowSample sample{};
|
||||
sample.time_us = sensor_airflow.timestamp;
|
||||
sample.speed = sensor_airflow.speed;
|
||||
sample.direction_rad = sensor_airflow.direction;
|
||||
|
||||
_airflow_buffer.push(sample);
|
||||
_time_last_buffer_push = imu_delayed.time_us;
|
||||
}
|
||||
}
|
||||
|
||||
#endif // MODULE_NAME
|
||||
|
||||
|
||||
|
||||
AirflowSample sample;
|
||||
|
||||
if (_airflow_buffer.pop_first_older_than(imu_delayed.time_us, &sample)) {
|
||||
|
||||
|
||||
|
||||
|
||||
//ECL_INFO("%lu: %lu sensor wind fusion", _time_delayed_us, sample_delayed.time_us);
|
||||
|
||||
ekf.resetEstimatorAidStatus(_aid_src);
|
||||
|
||||
_aid_src.timestamp_sample = sample.time_us;
|
||||
|
||||
// body frame wind speed and direction
|
||||
const Vector2f measurement{sample.speed * cosf(sample.direction_rad), sample.speed * sinf(sample.direction_rad)};
|
||||
|
||||
float wind_noise = 4.f; // m/s
|
||||
const float R = sq(wind_noise);
|
||||
|
||||
const auto state_vector = ekf.state().vector();
|
||||
const auto &P = ekf.covariances();
|
||||
|
||||
Vector2f innov{};
|
||||
Vector2f innov_var{};
|
||||
Ekf::VectorState H;
|
||||
|
||||
sym::ComputeAirspeed2DInnovInnovVarAndHx(state_vector, P, measurement, R, FLT_EPSILON,
|
||||
&innov, &innov_var, &H);
|
||||
|
||||
_aid_src.observation[0] = measurement(0);
|
||||
_aid_src.observation[1] = measurement(1);
|
||||
_aid_src.innovation[0] = innov(0);
|
||||
_aid_src.innovation[1] = innov(1);
|
||||
|
||||
_aid_src.observation_variance[0] = R;
|
||||
_aid_src.observation_variance[1] = R;
|
||||
_aid_src.innovation_variance[0] = innov_var(0);
|
||||
_aid_src.innovation_variance[1] = innov_var(1);
|
||||
|
||||
float innov_gate = 3.f; // innovation gate
|
||||
ekf.setEstimatorAidStatusTestRatio(_aid_src, innov_gate);
|
||||
|
||||
bool conditions_passing = ekf.control_status_flags().wind;
|
||||
|
||||
bool update_all_states = ekf.control_status_flags().wind && ekf.control_status_flags().in_air
|
||||
&& (_aid_src.test_ratio[0] < 0.1f) && (_aid_src.test_ratio[1] < 0.1f)
|
||||
&& !ekf.getWindVelocityVariance().longerThan(0.1f * R);
|
||||
|
||||
// const bool update_wind_only = !ekf.control_status_flags().wind_dead_reckoning;
|
||||
|
||||
bool fused[2] {false, false};
|
||||
|
||||
if (ekf.control_status_flags().wind
|
||||
&& conditions_passing
|
||||
&& (_aid_src.test_ratio[0] < 1.f)
|
||||
&& (_aid_src.test_ratio[1] < 1.f)
|
||||
) {
|
||||
// fuse x
|
||||
{
|
||||
// H computed above
|
||||
Ekf::VectorState Kfusion = P * H / innov_var(0);
|
||||
|
||||
if (!update_all_states) {
|
||||
for (unsigned row = 0; row <= 21; row++) {
|
||||
Kfusion(row) = 0.f;
|
||||
}
|
||||
}
|
||||
|
||||
fused[0] = ekf.measurementUpdate(Kfusion, innov_var(0), innov(0));
|
||||
}
|
||||
|
||||
// fuse y
|
||||
{
|
||||
// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
|
||||
sym::ComputeAirspeed2DYInnovInnovVarAndHy(state_vector, P, measurement, R, FLT_EPSILON,
|
||||
&innov(1), &innov_var(1), &H);
|
||||
|
||||
Ekf::VectorState Kfusion = P * H / innov_var(1);
|
||||
|
||||
if (!update_all_states) {
|
||||
for (unsigned row = 0; row <= 21; row++) {
|
||||
Kfusion(row) = 0.f;
|
||||
}
|
||||
}
|
||||
|
||||
fused[1] = ekf.measurementUpdate(Kfusion, innov_var(1), innov(1));
|
||||
}
|
||||
}
|
||||
|
||||
if (fused[0] && fused[1]) {
|
||||
_aid_src.time_last_fuse = imu_delayed.time_us;
|
||||
_aid_src.fused = true;
|
||||
}
|
||||
|
||||
|
||||
if (!ekf.control_status_flags().wind && ekf.local_position_is_valid()) {
|
||||
|
||||
Vector3f airflow_bf{measurement(0), measurement(1), 0.f};
|
||||
Vector3f offset_body{0.f, 0.f, 0.f};
|
||||
|
||||
const Dcmf R_to_earth{ekf.state().quat_nominal};
|
||||
|
||||
Vector3f airflow_ned = (R_to_earth * (airflow_bf - offset_body));
|
||||
|
||||
Vector2f wind_vel{
|
||||
ekf.state().vel(0) - airflow_ned(0),
|
||||
ekf.state().vel(1) - airflow_ned(1)
|
||||
};
|
||||
|
||||
ekf.resetWind(wind_vel, R);
|
||||
|
||||
_aid_src.time_last_fuse = imu_delayed.time_us;
|
||||
}
|
||||
|
||||
#if defined(MODULE_NAME)
|
||||
_aid_src.timestamp = hrt_absolute_time();
|
||||
_estimator_aid_src_airspeed2d_pub.publish(_aid_src);
|
||||
#endif // MODULE_NAME
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
84
src/modules/ekf2/EKF/aid_sources/Airspeed2D.hpp
Normal file
84
src/modules/ekf2/EKF/aid_sources/Airspeed2D.hpp
Normal file
@ -0,0 +1,84 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#ifndef EKF_AIRSPEED2D_HPP
|
||||
#define EKF_AIRSPEED2D_HPP
|
||||
|
||||
#if defined(CONFIG_EKF2_AIRSPEED2D) && defined(MODULE_NAME)
|
||||
|
||||
#include "EstimatorAidSource.hpp"
|
||||
|
||||
#include "../RingBuffer.h"
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/estimator_aid_source2d.h>
|
||||
#include <uORB/topics/sensor_airflow.h>
|
||||
|
||||
class Airspeed2D : public EstimatorAidSource, public ModuleParams
|
||||
{
|
||||
public:
|
||||
Airspeed2D() : ModuleParams(nullptr) {}
|
||||
virtual ~Airspeed2D() = default;
|
||||
|
||||
void reset() override {};
|
||||
bool update(Ekf &ekf, const estimator::imuSample &imu_delayed) override;
|
||||
|
||||
private:
|
||||
struct AirflowSample {
|
||||
uint64_t time_us{};
|
||||
float speed{};
|
||||
float direction_rad{};
|
||||
};
|
||||
|
||||
RingBuffer<AirflowSample> _airflow_buffer{20}; // TODO: size with _obs_buffer_length and actual publication rate
|
||||
uint64_t _time_last_buffer_push{0};
|
||||
|
||||
estimator_aid_source2d_s _aid_src{};
|
||||
|
||||
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_airspeed2d_pub {ORB_ID(estimator_aid_src_airspeed2d)};
|
||||
uORB::Subscription _sensor_airflow_sub{ORB_ID(sensor_airflow)};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::EKF2_ASP2D_CTRL>) _param_ekf2_asp2d_ctrl,
|
||||
(ParamFloat<px4::params::EKF2_ASP2D_DELAY>) _param_ekf2_asp2d_delay,
|
||||
(ParamFloat<px4::params::EKF2_ASP2D_NOISE>) _param_ekf2_asp2d_noise,
|
||||
(ParamFloat<px4::params::EKF2_ASP2D_GATE>) _param_ekf2_asp2d_gate
|
||||
)
|
||||
|
||||
};
|
||||
|
||||
#endif // CONFIG_EKF2_AIRSPEED2D && MODULE_NAME
|
||||
|
||||
#endif // !EKF_AIRSPEED2D_HPP
|
||||
@ -59,6 +59,10 @@
|
||||
#include <uORB/topics/estimator_aid_source2d.h>
|
||||
#include <uORB/topics/estimator_aid_source3d.h>
|
||||
|
||||
#if defined(CONFIG_EKF2_AIRSPEED2D)
|
||||
# include "aid_sources/Airspeed2D.hpp"
|
||||
#endif // CONFIG_EKF2_AIRSPEED2D
|
||||
|
||||
#include "aid_sources/ZeroGyroUpdate.hpp"
|
||||
#include "aid_sources/ZeroVelocityUpdate.hpp"
|
||||
|
||||
@ -524,6 +528,99 @@ public:
|
||||
|
||||
friend class AuxGlobalPosition;
|
||||
|
||||
|
||||
#if defined(CONFIG_EKF2_WIND)
|
||||
// perform a reset of the wind states and related covariances
|
||||
void resetWind();
|
||||
void resetWind(const Vector2f& wind_vel, float wind_vel_var = NAN);
|
||||
void resetWindCov();
|
||||
void resetWindToZero();
|
||||
#endif // CONFIG_EKF2_WIND
|
||||
|
||||
void resetEstimatorAidStatus(estimator_aid_source1d_s &status) const
|
||||
{
|
||||
// only bother resetting if timestamp_sample is set
|
||||
if (status.timestamp_sample != 0) {
|
||||
status.timestamp_sample = 0;
|
||||
|
||||
// preserve status.time_last_fuse
|
||||
|
||||
status.observation = 0;
|
||||
status.observation_variance = 0;
|
||||
|
||||
status.innovation = 0;
|
||||
status.innovation_variance = 0;
|
||||
status.test_ratio = INFINITY;
|
||||
|
||||
status.innovation_rejected = true;
|
||||
status.fused = false;
|
||||
}
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
void resetEstimatorAidStatus(T &status) const
|
||||
{
|
||||
// only bother resetting if timestamp_sample is set
|
||||
if (status.timestamp_sample != 0) {
|
||||
status.timestamp_sample = 0;
|
||||
|
||||
// preserve status.time_last_fuse
|
||||
|
||||
for (size_t i = 0; i < (sizeof(status.observation) / sizeof(status.observation[0])); i++) {
|
||||
status.observation[i] = 0;
|
||||
status.observation_variance[i] = 0;
|
||||
|
||||
status.innovation[i] = 0;
|
||||
status.innovation_variance[i] = 0;
|
||||
status.test_ratio[i] = INFINITY;
|
||||
}
|
||||
|
||||
status.innovation_rejected = true;
|
||||
status.fused = false;
|
||||
}
|
||||
}
|
||||
|
||||
void setEstimatorAidStatusTestRatio(estimator_aid_source1d_s &status, float innovation_gate) const
|
||||
{
|
||||
if (PX4_ISFINITE(status.innovation)
|
||||
&& PX4_ISFINITE(status.innovation_variance)
|
||||
&& (status.innovation_variance > 0.f)
|
||||
) {
|
||||
status.test_ratio = sq(status.innovation) / (sq(innovation_gate) * status.innovation_variance);
|
||||
status.innovation_rejected = (status.test_ratio > 1.f);
|
||||
|
||||
} else {
|
||||
status.test_ratio = INFINITY;
|
||||
status.innovation_rejected = true;
|
||||
}
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
void setEstimatorAidStatusTestRatio(T &status, float innovation_gate) const
|
||||
{
|
||||
bool innovation_rejected = false;
|
||||
|
||||
for (size_t i = 0; i < (sizeof(status.test_ratio) / sizeof(status.test_ratio[0])); i++) {
|
||||
if (PX4_ISFINITE(status.innovation[i])
|
||||
&& PX4_ISFINITE(status.innovation_variance[i])
|
||||
&& (status.innovation_variance[i] > 0.f)
|
||||
) {
|
||||
status.test_ratio[i] = sq(status.innovation[i]) / (sq(innovation_gate) * status.innovation_variance[i]);
|
||||
|
||||
if (status.test_ratio[i] > 1.f) {
|
||||
innovation_rejected = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
status.test_ratio[i] = INFINITY;
|
||||
innovation_rejected = true;
|
||||
}
|
||||
}
|
||||
|
||||
// if any of the innovations are rejected, then the overall innovation is rejected
|
||||
status.innovation_rejected = innovation_rejected;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
// set the internal states and status to their default value
|
||||
@ -1115,13 +1212,6 @@ private:
|
||||
void resetMagCov();
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_WIND)
|
||||
// perform a reset of the wind states and related covariances
|
||||
void resetWind();
|
||||
void resetWindCov();
|
||||
void resetWindToZero();
|
||||
#endif // CONFIG_EKF2_WIND
|
||||
|
||||
void resetGyroBiasZCov();
|
||||
|
||||
// uncorrelate quaternion states from other states
|
||||
@ -1160,96 +1250,21 @@ private:
|
||||
bool _ev_q_error_initialized{false};
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
void resetEstimatorAidStatus(estimator_aid_source1d_s &status) const
|
||||
{
|
||||
// only bother resetting if timestamp_sample is set
|
||||
if (status.timestamp_sample != 0) {
|
||||
status.timestamp_sample = 0;
|
||||
|
||||
// preserve status.time_last_fuse
|
||||
|
||||
status.observation = 0;
|
||||
status.observation_variance = 0;
|
||||
|
||||
status.innovation = 0;
|
||||
status.innovation_variance = 0;
|
||||
status.test_ratio = INFINITY;
|
||||
|
||||
status.innovation_rejected = true;
|
||||
status.fused = false;
|
||||
}
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
void resetEstimatorAidStatus(T &status) const
|
||||
{
|
||||
// only bother resetting if timestamp_sample is set
|
||||
if (status.timestamp_sample != 0) {
|
||||
status.timestamp_sample = 0;
|
||||
|
||||
// preserve status.time_last_fuse
|
||||
|
||||
for (size_t i = 0; i < (sizeof(status.observation) / sizeof(status.observation[0])); i++) {
|
||||
status.observation[i] = 0;
|
||||
status.observation_variance[i] = 0;
|
||||
|
||||
status.innovation[i] = 0;
|
||||
status.innovation_variance[i] = 0;
|
||||
status.test_ratio[i] = INFINITY;
|
||||
}
|
||||
|
||||
status.innovation_rejected = true;
|
||||
status.fused = false;
|
||||
}
|
||||
}
|
||||
|
||||
void setEstimatorAidStatusTestRatio(estimator_aid_source1d_s &status, float innovation_gate) const
|
||||
{
|
||||
if (PX4_ISFINITE(status.innovation)
|
||||
&& PX4_ISFINITE(status.innovation_variance)
|
||||
&& (status.innovation_variance > 0.f)
|
||||
) {
|
||||
status.test_ratio = sq(status.innovation) / (sq(innovation_gate) * status.innovation_variance);
|
||||
status.innovation_rejected = (status.test_ratio > 1.f);
|
||||
|
||||
} else {
|
||||
status.test_ratio = INFINITY;
|
||||
status.innovation_rejected = true;
|
||||
}
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
void setEstimatorAidStatusTestRatio(T &status, float innovation_gate) const
|
||||
{
|
||||
bool innovation_rejected = false;
|
||||
|
||||
for (size_t i = 0; i < (sizeof(status.test_ratio) / sizeof(status.test_ratio[0])); i++) {
|
||||
if (PX4_ISFINITE(status.innovation[i])
|
||||
&& PX4_ISFINITE(status.innovation_variance[i])
|
||||
&& (status.innovation_variance[i] > 0.f)
|
||||
) {
|
||||
status.test_ratio[i] = sq(status.innovation[i]) / (sq(innovation_gate) * status.innovation_variance[i]);
|
||||
|
||||
if (status.test_ratio[i] > 1.f) {
|
||||
innovation_rejected = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
status.test_ratio[i] = INFINITY;
|
||||
innovation_rejected = true;
|
||||
}
|
||||
}
|
||||
|
||||
// if any of the innovations are rejected, then the overall innovation is rejected
|
||||
status.innovation_rejected = innovation_rejected;
|
||||
}
|
||||
|
||||
ZeroGyroUpdate _zero_gyro_update{};
|
||||
ZeroVelocityUpdate _zero_velocity_update{};
|
||||
|
||||
#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
|
||||
#if defined(MODULE_NAME)
|
||||
|
||||
# if defined(CONFIG_EKF2_AIRSPEED2D)
|
||||
Airspeed2D _airspeed2d{};
|
||||
# endif // CONFIG_EKF2_AIRSPEED2D
|
||||
|
||||
# if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION)
|
||||
AuxGlobalPosition _aux_global_position{};
|
||||
#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
|
||||
# endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
|
||||
|
||||
#endif // MODULE_NAME
|
||||
|
||||
};
|
||||
|
||||
#endif // !EKF_EKF_H
|
||||
|
||||
@ -769,6 +769,23 @@ void Ekf::resetWind()
|
||||
resetWindToZero();
|
||||
}
|
||||
|
||||
void Ekf::resetWind(const Vector2f& wind_vel, const float wind_vel_var)
|
||||
{
|
||||
_control_status.flags.wind = true;
|
||||
|
||||
ECL_INFO("reset wind velocity to (%.1f, %.1f) m/s", double(wind_vel(0)), double(wind_vel(1)));
|
||||
|
||||
_state.wind_vel = wind_vel;
|
||||
|
||||
float obs_var = sq(_params.initial_wind_uncertainty);
|
||||
|
||||
if (PX4_ISFINITE(wind_vel_var)) {
|
||||
obs_var = math::max(obs_var, wind_vel_var);
|
||||
}
|
||||
|
||||
P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, obs_var);
|
||||
}
|
||||
|
||||
void Ekf::resetWindToZero()
|
||||
{
|
||||
ECL_INFO("reset wind to zero");
|
||||
|
||||
@ -282,6 +282,64 @@ def compute_wind_init_and_cov_from_airspeed(
|
||||
wind = wind.subs({sideslip: 0.0})
|
||||
return (wind, P)
|
||||
|
||||
def predict_body_airspeed(state: State) -> (sf.V2):
|
||||
|
||||
wind = sf.V3(state["wind_vel"][0], state["wind_vel"][1], 0.0)
|
||||
vel_rel = state["vel"] - wind
|
||||
|
||||
relative_wind_body = state["quat_nominal"].inverse() * vel_rel
|
||||
|
||||
return relative_wind_body
|
||||
|
||||
def compute_airspeed_2d_innov_innov_var_and_hx(
|
||||
state: VState,
|
||||
P: MTangent,
|
||||
airspeed_2d: sf.V2,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.V2, sf.V2, VTangent):
|
||||
|
||||
state = vstate_to_state(state)
|
||||
relative_wind_body = predict_body_airspeed(state)
|
||||
|
||||
meas_pred = sf.V2()
|
||||
meas_pred[0] = relative_wind_body[0]
|
||||
meas_pred[1] = relative_wind_body[1]
|
||||
|
||||
innov = meas_pred - airspeed_2d
|
||||
|
||||
# initialize outputs
|
||||
innov_var = sf.V2()
|
||||
H = [None] * 2
|
||||
|
||||
# calculate observation jacobian (H), kalman gain (K), and innovation variance (S)
|
||||
# for each axis
|
||||
for i in range(2):
|
||||
H[i] = sf.V1(meas_pred[i]).jacobian(state)
|
||||
innov_var[i] = (H[i] * P * H[i].T + R)[0,0]
|
||||
|
||||
return (innov, innov_var, H[0].T)
|
||||
|
||||
def compute_airspeed_2d_y_innov_innov_var_and_hy(
|
||||
state: VState,
|
||||
P: MTangent,
|
||||
airspeed_2d: sf.V2,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, sf.Scalar, VTangent):
|
||||
|
||||
state = vstate_to_state(state)
|
||||
meas_pred = predict_body_airspeed(state)
|
||||
|
||||
innov = meas_pred[1] - airspeed_2d[1]
|
||||
|
||||
# calculate observation jacobian (H), kalman gain (K), and innovation variance (S)
|
||||
# for each axis
|
||||
H = sf.V1(meas_pred[1]).jacobian(state)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov, innov_var, H.T)
|
||||
|
||||
def predict_sideslip(
|
||||
state: State,
|
||||
epsilon: sf.Scalar
|
||||
@ -627,6 +685,8 @@ if not args.disable_wind:
|
||||
generate_px4_function(compute_sideslip_h_and_k, output_names=["H", "K"])
|
||||
generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"])
|
||||
generate_px4_function(compute_wind_init_and_cov_from_airspeed, output_names=["wind", "P_wind"])
|
||||
generate_px4_function(compute_airspeed_2d_innov_innov_var_and_hx, output_names=["innov", "innov_var", "Hx"])
|
||||
generate_px4_function(compute_airspeed_2d_y_innov_innov_var_and_hy, output_names=["innov", "innov_var", "Hy"])
|
||||
|
||||
generate_px4_function(compute_yaw_innov_var_and_h, output_names=["innov_var", "H"])
|
||||
generate_px4_function(compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"])
|
||||
|
||||
@ -0,0 +1,144 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// This file was autogenerated by symforce from template:
|
||||
// function/FUNCTION.h.jinja
|
||||
// Do NOT modify by hand.
|
||||
// -----------------------------------------------------------------------------
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
namespace sym {
|
||||
|
||||
/**
|
||||
* This function was autogenerated from a symbolic function. Do not modify by hand.
|
||||
*
|
||||
* Symbolic function: compute_airspeed_2d_innov_innov_var_and_hx
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* airspeed_2d: Matrix21
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov: Matrix21
|
||||
* innov_var: Matrix21
|
||||
* Hx: Matrix23_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeAirspeed2DInnovInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P,
|
||||
const matrix::Matrix<Scalar, 2, 1>& airspeed_2d,
|
||||
const Scalar R, const Scalar epsilon,
|
||||
matrix::Matrix<Scalar, 2, 1>* const innov = nullptr,
|
||||
matrix::Matrix<Scalar, 2, 1>* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const Hx = nullptr) {
|
||||
// Total ops: 287
|
||||
|
||||
// Unused inputs
|
||||
(void)epsilon;
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (33)
|
||||
const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp1 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp2 = 1 - 2 * _tmp1;
|
||||
const Scalar _tmp3 = -2 * _tmp0 + _tmp2;
|
||||
const Scalar _tmp4 = -state(22, 0) + state(4, 0);
|
||||
const Scalar _tmp5 = 2 * state(0, 0);
|
||||
const Scalar _tmp6 = _tmp5 * state(3, 0);
|
||||
const Scalar _tmp7 = 2 * state(2, 0);
|
||||
const Scalar _tmp8 = _tmp7 * state(1, 0);
|
||||
const Scalar _tmp9 = _tmp6 + _tmp8;
|
||||
const Scalar _tmp10 = -state(23, 0) + state(5, 0);
|
||||
const Scalar _tmp11 = _tmp7 * state(0, 0);
|
||||
const Scalar _tmp12 = -_tmp11;
|
||||
const Scalar _tmp13 = 2 * state(1, 0) * state(3, 0);
|
||||
const Scalar _tmp14 = _tmp12 + _tmp13;
|
||||
const Scalar _tmp15 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp16 = -2 * _tmp15 + _tmp2;
|
||||
const Scalar _tmp17 = -_tmp6;
|
||||
const Scalar _tmp18 = _tmp17 + _tmp8;
|
||||
const Scalar _tmp19 = _tmp7 * state(3, 0);
|
||||
const Scalar _tmp20 = _tmp5 * state(1, 0);
|
||||
const Scalar _tmp21 = _tmp19 + _tmp20;
|
||||
const Scalar _tmp22 = _tmp18 * _tmp4 + _tmp21 * state(6, 0);
|
||||
const Scalar _tmp23 = -_tmp1;
|
||||
const Scalar _tmp24 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp25 = -_tmp15;
|
||||
const Scalar _tmp26 = _tmp10 * (_tmp0 + _tmp23 + _tmp24 + _tmp25) + _tmp22;
|
||||
const Scalar _tmp27 = -_tmp13;
|
||||
const Scalar _tmp28 = _tmp0 - _tmp24;
|
||||
const Scalar _tmp29 = _tmp10 * (-_tmp19 + _tmp20) + _tmp4 * (_tmp12 + _tmp27) +
|
||||
state(6, 0) * (_tmp15 + _tmp23 + _tmp28);
|
||||
const Scalar _tmp30 = _tmp1 + _tmp25;
|
||||
const Scalar _tmp31 = _tmp10 * (_tmp19 - _tmp20) + _tmp4 * (_tmp11 + _tmp13) +
|
||||
state(6, 0) * (-_tmp0 + _tmp24 + _tmp30);
|
||||
const Scalar _tmp32 =
|
||||
_tmp10 * (_tmp17 - _tmp8) + _tmp4 * (_tmp28 + _tmp30) + state(6, 0) * (_tmp11 + _tmp27);
|
||||
|
||||
// Output terms (3)
|
||||
if (innov != nullptr) {
|
||||
matrix::Matrix<Scalar, 2, 1>& _innov = (*innov);
|
||||
|
||||
_innov(0, 0) = _tmp10 * _tmp9 + _tmp14 * state(6, 0) + _tmp3 * _tmp4 - airspeed_2d(0, 0);
|
||||
_innov(1, 0) = _tmp10 * _tmp16 + _tmp22 - airspeed_2d(1, 0);
|
||||
}
|
||||
|
||||
if (innov_var != nullptr) {
|
||||
matrix::Matrix<Scalar, 2, 1>& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var(0, 0) =
|
||||
R +
|
||||
_tmp14 * (P(1, 5) * _tmp29 + P(2, 5) * _tmp26 - P(21, 5) * _tmp3 - P(22, 5) * _tmp9 +
|
||||
P(3, 5) * _tmp3 + P(4, 5) * _tmp9 + P(5, 5) * _tmp14) +
|
||||
_tmp26 * (P(1, 2) * _tmp29 + P(2, 2) * _tmp26 - P(21, 2) * _tmp3 - P(22, 2) * _tmp9 +
|
||||
P(3, 2) * _tmp3 + P(4, 2) * _tmp9 + P(5, 2) * _tmp14) +
|
||||
_tmp29 * (P(1, 1) * _tmp29 + P(2, 1) * _tmp26 - P(21, 1) * _tmp3 - P(22, 1) * _tmp9 +
|
||||
P(3, 1) * _tmp3 + P(4, 1) * _tmp9 + P(5, 1) * _tmp14) -
|
||||
_tmp3 * (P(1, 21) * _tmp29 + P(2, 21) * _tmp26 - P(21, 21) * _tmp3 - P(22, 21) * _tmp9 +
|
||||
P(3, 21) * _tmp3 + P(4, 21) * _tmp9 + P(5, 21) * _tmp14) +
|
||||
_tmp3 * (P(1, 3) * _tmp29 + P(2, 3) * _tmp26 - P(21, 3) * _tmp3 - P(22, 3) * _tmp9 +
|
||||
P(3, 3) * _tmp3 + P(4, 3) * _tmp9 + P(5, 3) * _tmp14) -
|
||||
_tmp9 * (P(1, 22) * _tmp29 + P(2, 22) * _tmp26 - P(21, 22) * _tmp3 - P(22, 22) * _tmp9 +
|
||||
P(3, 22) * _tmp3 + P(4, 22) * _tmp9 + P(5, 22) * _tmp14) +
|
||||
_tmp9 * (P(1, 4) * _tmp29 + P(2, 4) * _tmp26 - P(21, 4) * _tmp3 - P(22, 4) * _tmp9 +
|
||||
P(3, 4) * _tmp3 + P(4, 4) * _tmp9 + P(5, 4) * _tmp14);
|
||||
_innov_var(1, 0) =
|
||||
R -
|
||||
_tmp16 * (P(0, 22) * _tmp31 + P(2, 22) * _tmp32 - P(21, 22) * _tmp18 - P(22, 22) * _tmp16 +
|
||||
P(3, 22) * _tmp18 + P(4, 22) * _tmp16 + P(5, 22) * _tmp21) +
|
||||
_tmp16 * (P(0, 4) * _tmp31 + P(2, 4) * _tmp32 - P(21, 4) * _tmp18 - P(22, 4) * _tmp16 +
|
||||
P(3, 4) * _tmp18 + P(4, 4) * _tmp16 + P(5, 4) * _tmp21) -
|
||||
_tmp18 * (P(0, 21) * _tmp31 + P(2, 21) * _tmp32 - P(21, 21) * _tmp18 - P(22, 21) * _tmp16 +
|
||||
P(3, 21) * _tmp18 + P(4, 21) * _tmp16 + P(5, 21) * _tmp21) +
|
||||
_tmp18 * (P(0, 3) * _tmp31 + P(2, 3) * _tmp32 - P(21, 3) * _tmp18 - P(22, 3) * _tmp16 +
|
||||
P(3, 3) * _tmp18 + P(4, 3) * _tmp16 + P(5, 3) * _tmp21) +
|
||||
_tmp21 * (P(0, 5) * _tmp31 + P(2, 5) * _tmp32 - P(21, 5) * _tmp18 - P(22, 5) * _tmp16 +
|
||||
P(3, 5) * _tmp18 + P(4, 5) * _tmp16 + P(5, 5) * _tmp21) +
|
||||
_tmp31 * (P(0, 0) * _tmp31 + P(2, 0) * _tmp32 - P(21, 0) * _tmp18 - P(22, 0) * _tmp16 +
|
||||
P(3, 0) * _tmp18 + P(4, 0) * _tmp16 + P(5, 0) * _tmp21) +
|
||||
_tmp32 * (P(0, 2) * _tmp31 + P(2, 2) * _tmp32 - P(21, 2) * _tmp18 - P(22, 2) * _tmp16 +
|
||||
P(3, 2) * _tmp18 + P(4, 2) * _tmp16 + P(5, 2) * _tmp21);
|
||||
}
|
||||
|
||||
if (Hx != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _hx = (*Hx);
|
||||
|
||||
_hx.setZero();
|
||||
|
||||
_hx(1, 0) = _tmp29;
|
||||
_hx(2, 0) = _tmp26;
|
||||
_hx(3, 0) = _tmp3;
|
||||
_hx(4, 0) = _tmp9;
|
||||
_hx(5, 0) = _tmp14;
|
||||
_hx(21, 0) = -_tmp3;
|
||||
_hx(22, 0) = -_tmp9;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
@ -0,0 +1,113 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// This file was autogenerated by symforce from template:
|
||||
// function/FUNCTION.h.jinja
|
||||
// Do NOT modify by hand.
|
||||
// -----------------------------------------------------------------------------
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
namespace sym {
|
||||
|
||||
/**
|
||||
* This function was autogenerated from a symbolic function. Do not modify by hand.
|
||||
*
|
||||
* Symbolic function: compute_airspeed_2d_y_innov_innov_var_and_hy
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* airspeed_2d: Matrix21
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov: Scalar
|
||||
* innov_var: Scalar
|
||||
* Hy: Matrix23_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeAirspeed2DYInnovInnovVarAndHy(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P,
|
||||
const matrix::Matrix<Scalar, 2, 1>& airspeed_2d,
|
||||
const Scalar R, const Scalar epsilon,
|
||||
Scalar* const innov = nullptr,
|
||||
Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const Hy = nullptr) {
|
||||
// Total ops: 154
|
||||
|
||||
// Unused inputs
|
||||
(void)epsilon;
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (20)
|
||||
const Scalar _tmp0 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp2 = -2 * _tmp0 - 2 * _tmp1 + 1;
|
||||
const Scalar _tmp3 = -state(23, 0) + state(5, 0);
|
||||
const Scalar _tmp4 = 2 * state(0, 0);
|
||||
const Scalar _tmp5 = -_tmp4 * state(3, 0);
|
||||
const Scalar _tmp6 = 2 * state(2, 0);
|
||||
const Scalar _tmp7 = _tmp6 * state(1, 0);
|
||||
const Scalar _tmp8 = _tmp5 + _tmp7;
|
||||
const Scalar _tmp9 = -state(22, 0) + state(4, 0);
|
||||
const Scalar _tmp10 = _tmp6 * state(3, 0);
|
||||
const Scalar _tmp11 = _tmp4 * state(1, 0);
|
||||
const Scalar _tmp12 = _tmp10 + _tmp11;
|
||||
const Scalar _tmp13 = _tmp6 * state(0, 0);
|
||||
const Scalar _tmp14 = 2 * state(1, 0) * state(3, 0);
|
||||
const Scalar _tmp15 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp16 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp17 = _tmp0 - _tmp1;
|
||||
const Scalar _tmp18 = _tmp3 * (_tmp10 - _tmp11) + _tmp9 * (_tmp13 + _tmp14) +
|
||||
state(6, 0) * (_tmp15 - _tmp16 + _tmp17);
|
||||
const Scalar _tmp19 = _tmp3 * (_tmp5 - _tmp7) + _tmp9 * (-_tmp15 + _tmp16 + _tmp17) +
|
||||
state(6, 0) * (_tmp13 - _tmp14);
|
||||
|
||||
// Output terms (3)
|
||||
if (innov != nullptr) {
|
||||
Scalar& _innov = (*innov);
|
||||
|
||||
_innov = _tmp12 * state(6, 0) + _tmp2 * _tmp3 + _tmp8 * _tmp9 - airspeed_2d(1, 0);
|
||||
}
|
||||
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var =
|
||||
R +
|
||||
_tmp12 * (P(0, 5) * _tmp18 + P(2, 5) * _tmp19 - P(21, 5) * _tmp8 - P(22, 5) * _tmp2 +
|
||||
P(3, 5) * _tmp8 + P(4, 5) * _tmp2 + P(5, 5) * _tmp12) +
|
||||
_tmp18 * (P(0, 0) * _tmp18 + P(2, 0) * _tmp19 - P(21, 0) * _tmp8 - P(22, 0) * _tmp2 +
|
||||
P(3, 0) * _tmp8 + P(4, 0) * _tmp2 + P(5, 0) * _tmp12) +
|
||||
_tmp19 * (P(0, 2) * _tmp18 + P(2, 2) * _tmp19 - P(21, 2) * _tmp8 - P(22, 2) * _tmp2 +
|
||||
P(3, 2) * _tmp8 + P(4, 2) * _tmp2 + P(5, 2) * _tmp12) -
|
||||
_tmp2 * (P(0, 22) * _tmp18 + P(2, 22) * _tmp19 - P(21, 22) * _tmp8 - P(22, 22) * _tmp2 +
|
||||
P(3, 22) * _tmp8 + P(4, 22) * _tmp2 + P(5, 22) * _tmp12) +
|
||||
_tmp2 * (P(0, 4) * _tmp18 + P(2, 4) * _tmp19 - P(21, 4) * _tmp8 - P(22, 4) * _tmp2 +
|
||||
P(3, 4) * _tmp8 + P(4, 4) * _tmp2 + P(5, 4) * _tmp12) -
|
||||
_tmp8 * (P(0, 21) * _tmp18 + P(2, 21) * _tmp19 - P(21, 21) * _tmp8 - P(22, 21) * _tmp2 +
|
||||
P(3, 21) * _tmp8 + P(4, 21) * _tmp2 + P(5, 21) * _tmp12) +
|
||||
_tmp8 * (P(0, 3) * _tmp18 + P(2, 3) * _tmp19 - P(21, 3) * _tmp8 - P(22, 3) * _tmp2 +
|
||||
P(3, 3) * _tmp8 + P(4, 3) * _tmp2 + P(5, 3) * _tmp12);
|
||||
}
|
||||
|
||||
if (Hy != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _hy = (*Hy);
|
||||
|
||||
_hy.setZero();
|
||||
|
||||
_hy(0, 0) = _tmp18;
|
||||
_hy(2, 0) = _tmp19;
|
||||
_hy(3, 0) = _tmp8;
|
||||
_hy(4, 0) = _tmp2;
|
||||
_hy(5, 0) = _tmp12;
|
||||
_hy(21, 0) = -_tmp8;
|
||||
_hy(22, 0) = -_tmp2;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
@ -31,6 +31,14 @@ depends on MODULES_EKF2
|
||||
---help---
|
||||
EKF2 airspeed fusion support.
|
||||
|
||||
menuconfig EKF2_AIRSPEED2D
|
||||
depends on MODULES_EKF2
|
||||
bool "airspeed 2D fusion support"
|
||||
default n
|
||||
depends on EKF2_WIND
|
||||
---help---
|
||||
EKF2 airspeed 2D fusion support.
|
||||
|
||||
menuconfig EKF2_AUX_GLOBAL_POSITION
|
||||
depends on MODULES_EKF2
|
||||
bool "aux global position fusion support"
|
||||
|
||||
75
src/modules/ekf2/ekf2_params_airspeed_2d.c
Normal file
75
src/modules/ekf2/ekf2_params_airspeed_2d.c
Normal file
@ -0,0 +1,75 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* Airspeed 2D sensor aiding
|
||||
*
|
||||
* @group EKF2
|
||||
* @boolean
|
||||
*/
|
||||
PARAM_DEFINE_INT32(EKF2_ASP2D_CTRL, 0);
|
||||
|
||||
/**
|
||||
* Airspeed 2D measurement delay relative to IMU measurements
|
||||
*
|
||||
* @group EKF2
|
||||
* @min 0
|
||||
* @max 300
|
||||
* @unit ms
|
||||
* @reboot_required true
|
||||
* @decimal 1
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF2_ASP2D_DELAY, 100);
|
||||
|
||||
/**
|
||||
* Airspeed 2D measurement noise.
|
||||
*
|
||||
* @group EKF2
|
||||
* @min 0.5
|
||||
* @max 5.0
|
||||
* @unit m/s
|
||||
* @decimal 1
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF2_ASP2D_NOISE, 3.0f);
|
||||
|
||||
/**
|
||||
* Airspeed 2D gate size
|
||||
*
|
||||
* Sets the number of standard deviations used by the innovation consistency test.
|
||||
*
|
||||
* @group EKF2
|
||||
* @min 1.0
|
||||
* @unit SD
|
||||
* @decimal 1
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF2_ASP2D_GATE, 3.0f);
|
||||
@ -209,6 +209,7 @@ void LoggedTopics::add_default_topics()
|
||||
// add_optional_topic_multi("estimator_aid_src_optical_flow", 100, MAX_ESTIMATOR_INSTANCES);
|
||||
// add_optional_topic_multi("estimator_aid_src_terrain_optical_flow", 100, MAX_ESTIMATOR_INSTANCES);
|
||||
// add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES);
|
||||
add_optional_topic_multi("estimator_aid_src_airspeed2d", 100, MAX_ESTIMATOR_INSTANCES);
|
||||
add_optional_topic_multi("estimator_aid_src_aux_global_position", 100, MAX_ESTIMATOR_INSTANCES);
|
||||
|
||||
// log all raw sensors at minimal rate (at least 1 Hz)
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user