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

ekf2: move zero gyro update to aid source class

Browse Source
This commit is contained in:
Daniel Agar 2023-10-11 14:47:27 -04:00
parent e79737a38d
commit 27f9b1b65a
6 changed files with 127 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -130,7 +130,8 @@ list(APPEND EKF_SRCS
EKF/vel_pos_fusion.cpp
EKF/yaw_fusion.cpp
EKF/zero_innovation_heading_update.cpp
EKF/zero_gyro_update.cpp
EKF/aid_sources/ZeroGyroUpdate.cpp
EKF/aid_sources/ZeroVelocityUpdate.cpp
)

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

@ -47,8 +47,8 @@ list(APPEND EKF_SRCS
vel_pos_fusion.cpp
yaw_fusion.cpp
zero_innovation_heading_update.cpp
zero_gyro_update.cpp
aid_sources/ZeroGyroUpdate.cpp
aid_sources/ZeroVelocityUpdate.cpp
)

60
src/modules/ekf2/EKF/zero_gyro_update.cpp → src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.cpp
View File

@ -31,21 +31,25 @@
*
****************************************************************************/
/**
* @file zero_gyro_update.cpp
* Control function for ekf zero gyro update
*/
#include "ZeroGyroUpdate.hpp"
#include "ekf.h"
#include "../ekf.h"
void Ekf::controlZeroGyroUpdate(const imuSample &imu_delayed)
ZeroGyroUpdate::ZeroGyroUpdate()
{
if (!(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GyroBias))) {
return;
}
reset();
}
void ZeroGyroUpdate::reset()
{
_zgup_delta_ang.setZero();
_zgup_delta_ang_dt = 0.f;
}
bool ZeroGyroUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
{
// When at rest, fuse the gyro data as a direct observation of the gyro bias
if (_control_status.flags.vehicle_at_rest) {
if (ekf.control_status_flags().vehicle_at_rest) {
// Downsample gyro data to run the fusion at a lower rate
_zgup_delta_ang += imu_delayed.delta_ang;
_zgup_delta_ang_dt += imu_delayed.delta_ang_dt;
@ -54,38 +58,38 @@ void Ekf::controlZeroGyroUpdate(const imuSample &imu_delayed)
const bool zero_gyro_update_data_ready = _zgup_delta_ang_dt >= zgup_dt;
if (zero_gyro_update_data_ready) {
Vector3f gyro_bias = _zgup_delta_ang / _zgup_delta_ang_dt;
Vector3f innovation = _state.gyro_bias - gyro_bias;
Vector3f innovation = ekf.state().gyro_bias - gyro_bias;
const float obs_var = sq(math::constrain(_params.gyro_noise, 0.f, 1.f));
const float obs_var = sq(math::constrain(ekf.getGyroNoise(), 0.f, 1.f));
const Vector3f innov_var = getGyroBiasVariance() + obs_var;
const Vector3f innov_var = ekf.getGyroBiasVariance() + obs_var;
for (int i = 0; i < 3; i++) {
fuseDeltaAngBias(innovation(i), innov_var(i), i);
Ekf::VectorState K; // Kalman gain vector for any single observation - sequential fusion is used.
const unsigned state_index = State::gyro_bias.idx + i;
// calculate kalman gain K = PHS, where S = 1/innovation variance
for (int row = 0; row < State::size; row++) {
K(row) = ekf.stateCovariance(row, state_index) / innov_var(i);
}
ekf.measurementUpdate(K, innov_var(i), innovation(i));
}
// Reset the integrators
_zgup_delta_ang.setZero();
_zgup_delta_ang_dt = 0.f;
return true;
}
} else if (_control_status_prev.flags.vehicle_at_rest) {
} else if (ekf.control_status_prev_flags().vehicle_at_rest) {
// Reset the integrators
_zgup_delta_ang.setZero();
_zgup_delta_ang_dt = 0.f;
}
}
void Ekf::fuseDeltaAngBias(const float innov, const float innov_var, const int obs_index)
{
VectorState K; // Kalman gain vector for any single observation - sequential fusion is used.
const unsigned state_index = obs_index + State::gyro_bias.idx;
// calculate kalman gain K = PHS, where S = 1/innovation variance
for (int row = 0; row < State::size; row++) {
K(row) = P(row, state_index) / innov_var;
}
measurementUpdate(K, innov_var, innov);
return false;
}

54
src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.hpp Normal file
View File

@ -0,0 +1,54 @@
/****************************************************************************
*
* Copyright (c) 2023 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_ZERO_GYRO_UPDATE_HPP
#define EKF_ZERO_GYRO_UPDATE_HPP
#include "EstimatorAidSource.hpp"
class ZeroGyroUpdate : public EstimatorAidSource
{
public:
ZeroGyroUpdate();
virtual ~ZeroGyroUpdate() = default;
void reset() override;
bool update(Ekf &ekf, const estimator::imuSample &imu_delayed) override;
private:
matrix::Vector3f _zgup_delta_ang{};
float _zgup_delta_ang_dt{0.f};
};
#endif // !EKF_ZERO_GYRO_UPDATE_HPP

4
src/modules/ekf2/EKF/control.cpp
View File

@ -147,7 +147,9 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
_zero_velocity_update.update(*this, imu_delayed);
controlZeroGyroUpdate(imu_delayed);
if (_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GyroBias)) {
_zero_gyro_update.update(*this, imu_delayed);
}
// Fake position measurement for constraining drift when no other velocity or position measurements
controlFakePosFusion();

72
src/modules/ekf2/EKF/ekf.h
View File

@ -59,6 +59,7 @@
#include <uORB/topics/estimator_aid_source2d.h>
#include <uORB/topics/estimator_aid_source3d.h>
#include "aid_sources/ZeroGyroUpdate.hpp"
#include "aid_sources/ZeroVelocityUpdate.hpp"
enum class Likelihood { LOW, MEDIUM, HIGH };
@ -249,6 +250,7 @@ public:
// get the full covariance matrix
const matrix::SquareMatrix<float, State::size> &covariances() const { return P; }
float stateCovariance(unsigned r, unsigned c) const { return P(r, c); }
// get the diagonal elements of the covariance matrix
matrix::Vector<float, State::size> covariances_diagonal() const { return P.diag(); }
@ -328,6 +330,7 @@ public:
const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias in rad/s
Vector3f getGyroBiasVariance() const { return getStateVariance<State::gyro_bias>(); } // get the gyroscope bias variance in rad/s
float getGyroBiasLimit() const { return _params.gyro_bias_lim; }
float getGyroNoise() const { return _params.gyro_noise; }
// accel bias
const Vector3f &getAccelBias() const { return _state.accel_bias; } // get the accelerometer bias in m/s**2
@ -473,6 +476,37 @@ public:
const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; }
#endif // CONFIG_EKF2_AUXVEL
bool measurementUpdate(VectorState &K, float innovation_variance, float innovation)
{
clearInhibitedStateKalmanGains(K);
const VectorState KS = K * innovation_variance;
SquareMatrixState KHP;
for (unsigned row = 0; row < State::size; row++) {
for (unsigned col = 0; col < State::size; col++) {
// Instad of literally computing KHP, use an equvalent
// equation involving less mathematical operations
KHP(row, col) = KS(row) * K(col);
}
}
const bool is_healthy = checkAndFixCovarianceUpdate(KHP);
if (is_healthy) {
// apply the covariance corrections
P -= KHP;
fixCovarianceErrors(true);
// apply the state corrections
fuse(K, innovation);
}
return is_healthy;
}
private:
// set the internal states and status to their default value
@ -530,10 +564,6 @@ private:
Dcmf _R_to_earth{}; ///< transformation matrix from body frame to earth frame from last EKF prediction
// zero gyro update
Vector3f _zgup_delta_ang{};
float _zgup_delta_ang_dt{0.f};
Vector2f _accel_lpf_NE{}; ///< Low pass filtered horizontal earth frame acceleration (m/sec**2)
float _height_rate_lpf{0.0f};
float _yaw_delta_ef{0.0f}; ///< Recent change in yaw angle measured about the earth frame D axis (rad)
@ -917,36 +947,6 @@ private:
#endif // CONFIG_EKF2_WIND
}
bool measurementUpdate(VectorState &K, float innovation_variance, float innovation)
{
clearInhibitedStateKalmanGains(K);
const VectorState KS = K * innovation_variance;
SquareMatrixState KHP;
for (unsigned row = 0; row < State::size; row++) {
for (unsigned col = 0; col < State::size; col++) {
// Instad of literally computing KHP, use an equvalent
// equation involving less mathematical operations
KHP(row, col) = KS(row) * K(col);
}
}
const bool is_healthy = checkAndFixCovarianceUpdate(KHP);
if (is_healthy) {
// apply the covariance corrections
P -= KHP;
fixCovarianceErrors(true);
// apply the state corrections
fuse(K, innovation);
}
return is_healthy;
}
// if the covariance correction will result in a negative variance, then
// the covariance matrix is unhealthy and must be corrected
bool checkAndFixCovarianceUpdate(const SquareMatrixState &KHP);
@ -1073,9 +1073,6 @@ private:
void resetHeightToLastKnown();
void stopFakeHgtFusion();
void controlZeroGyroUpdate(const imuSample &imu_delayed);
void fuseDeltaAngBias(float innov, float innov_var, int obs_index);
void controlZeroInnovationHeadingUpdate();
#if defined(CONFIG_EKF2_AUXVEL)
@ -1242,6 +1239,7 @@ private:
status.innovation_rejected = innovation_rejected;
}
ZeroGyroUpdate _zero_gyro_update{};
ZeroVelocityUpdate _zero_velocity_update{};
};