dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: split vel_pos_fusion.cpp

This commit is contained in:
Daniel Agar 2024-02-14 20:50:04 -05:00
parent c4c41c49e5
commit c338891677
5 changed files with 114 additions and 88 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/ekf2/CMakeLists.txt
View File

@ -127,7 +127,8 @@ list(APPEND EKF_SRCS
EKF/height_control.cpp
EKF/imu_down_sampler.cpp
EKF/output_predictor.cpp
EKF/vel_pos_fusion.cpp
EKF/velocity_fusion.cpp
EKF/position_fusion.cpp
EKF/yaw_fusion.cpp
EKF/zero_innovation_heading_update.cpp

3
src/modules/ekf2/EKF/CMakeLists.txt
View File

@ -44,7 +44,8 @@ list(APPEND EKF_SRCS
height_control.cpp
imu_down_sampler.cpp
output_predictor.cpp
vel_pos_fusion.cpp
velocity_fusion.cpp
position_fusion.cpp
yaw_fusion.cpp
zero_innovation_heading_update.cpp

2
src/modules/ekf2/EKF/optflow_fusion.cpp
View File

@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file vel_pos_fusion.cpp
* @file optflow_fusion.cpp
* Function for fusing gps and baro measurements/
* equations generated using EKF/python/ekf_derivation/main.py
*

108
src/modules/ekf2/EKF/position_fusion.cpp Normal file
View File

@ -0,0 +1,108 @@
/****************************************************************************
*
* Copyright (c) 2015-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 "ekf.h"
void Ekf::updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var,
const float innov_gate, estimator_aid_source1d_s &aid_src) const
{
resetEstimatorAidStatus(aid_src);
aid_src.observation = obs;
aid_src.innovation = _state.pos(2) - aid_src.observation;
aid_src.observation_variance = math::max(sq(0.01f), obs_var);
aid_src.innovation_variance = P(State::pos.idx + 2, State::pos.idx + 2) + aid_src.observation_variance;
setEstimatorAidStatusTestRatio(aid_src, innov_gate);
// z special case if there is bad vertical acceleration data, then don't reject measurement,
// but limit innovation to prevent spikes that could destabilise the filter
if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance);
aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit);
aid_src.innovation_rejected = false;
}
aid_src.timestamp_sample = time_us;
}
void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var,
const float innov_gate, estimator_aid_source2d_s &aid_src) const
{
resetEstimatorAidStatus(aid_src);
for (int i = 0; i < 2; i++) {
aid_src.observation[i] = obs(i);
aid_src.innovation[i] = _state.pos(i) - aid_src.observation[i];
aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i));
const int state_index = State::pos.idx + i;
aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i];
}
setEstimatorAidStatusTestRatio(aid_src, innov_gate);
aid_src.timestamp_sample = time_us;
}
void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
{
// x & y
if (!aid_src.innovation_rejected) {
if (fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::pos.idx)
&& fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::pos.idx + 1)
) {
aid_src.fused = true;
aid_src.time_last_fuse = _time_delayed_us;
_time_last_hor_pos_fuse = _time_delayed_us;
} else {
aid_src.fused = false;
}
}
}
void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
{
// z
if (!aid_src.innovation_rejected) {
if (fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, State::pos.idx + 2)) {
aid_src.fused = true;
aid_src.time_last_fuse = _time_delayed_us;
_time_last_hgt_fuse = _time_delayed_us;
}
}
}

86
src/modules/ekf2/EKF/vel_pos_fusion.cpp → src/modules/ekf2/EKF/velocity_fusion.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2015-2023 PX4 Development Team. All rights reserved.
* Copyright (c) 2015-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
@ -31,16 +31,6 @@
*
****************************************************************************/
/**
* @file vel_pos_fusion.cpp
*
* @author Roman Bast <bapstroman@gmail.com>
* @author Siddharth Bharat Purohit <siddharthbharatpurohit@gmail.com>
* @author Paul Riseborough <p_riseborough@live.com.au>
*
*/
#include <mathlib/mathlib.h>
#include "ekf.h"
void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var,
@ -89,49 +79,6 @@ void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &ob
aid_src.timestamp_sample = time_us;
}
void Ekf::updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var,
const float innov_gate, estimator_aid_source1d_s &aid_src) const
{
resetEstimatorAidStatus(aid_src);
aid_src.observation = obs;
aid_src.innovation = _state.pos(2) - aid_src.observation;
aid_src.observation_variance = math::max(sq(0.01f), obs_var);
aid_src.innovation_variance = P(State::pos.idx + 2, State::pos.idx + 2) + aid_src.observation_variance;
setEstimatorAidStatusTestRatio(aid_src, innov_gate);
// z special case if there is bad vertical acceleration data, then don't reject measurement,
// but limit innovation to prevent spikes that could destabilise the filter
if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance);
aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit);
aid_src.innovation_rejected = false;
}
aid_src.timestamp_sample = time_us;
}
void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var,
const float innov_gate, estimator_aid_source2d_s &aid_src) const
{
resetEstimatorAidStatus(aid_src);
for (int i = 0; i < 2; i++) {
aid_src.observation[i] = obs(i);
aid_src.innovation[i] = _state.pos(i) - aid_src.observation[i];
aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i));
const int state_index = State::pos.idx + i;
aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i];
}
setEstimatorAidStatusTestRatio(aid_src, innov_gate);
aid_src.timestamp_sample = time_us;
}
void Ekf::fuseVelocity(estimator_aid_source2d_s &aid_src)
{
if (!aid_src.innovation_rejected) {
@ -169,34 +116,3 @@ void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
}
}
}
void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
{
// x & y
if (!aid_src.innovation_rejected) {
if (fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::pos.idx)
&& fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::pos.idx + 1)
) {
aid_src.fused = true;
aid_src.time_last_fuse = _time_delayed_us;
_time_last_hor_pos_fuse = _time_delayed_us;
} else {
aid_src.fused = false;
}
}
}
void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
{
// z
if (!aid_src.innovation_rejected) {
if (fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, State::pos.idx + 2)) {
aid_src.fused = true;
aid_src.time_last_fuse = _time_delayed_us;
_time_last_hgt_fuse = _time_delayed_us;
}
}
}