Skip to content

Commit

Permalink
move more ekf function implementations to cpp
Browse files Browse the repository at this point in the history
  • Loading branch information
@haumarco @dagar
haumarco authored and dagar committed Sep 23, 2024
1 parent e3f1388 commit 8afd267
Show file tree
Hide file tree
Showing 2 changed files with 132 additions and 126 deletions.
130 changes: 4 additions & 126 deletions src/modules/ekf2/EKF/ekf.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -135,92 +135,9 @@ class Ekf final : public EstimatorInterface
const Vector3f &getFlowRefBodyRate() const { return _ref_body_rate; }
#endif // CONFIG_EKF2_OPTICAL_FLOW

float getHeadingInnov() const
{
#if defined(CONFIG_EKF2_MAGNETOMETER)

if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
return Vector3f(_aid_src_mag.innovation).max();
}

#endif // CONFIG_EKF2_MAGNETOMETER

#if defined(CONFIG_EKF2_GNSS_YAW)

if (_control_status.flags.gnss_yaw) {
return _aid_src_gnss_yaw.innovation;
}

#endif // CONFIG_EKF2_GNSS_YAW

#if defined(CONFIG_EKF2_EXTERNAL_VISION)

if (_control_status.flags.ev_yaw) {
return _aid_src_ev_yaw.innovation;
}

#endif // CONFIG_EKF2_EXTERNAL_VISION

return 0.f;
}

float getHeadingInnovVar() const
{
#if defined(CONFIG_EKF2_MAGNETOMETER)

if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
return Vector3f(_aid_src_mag.innovation_variance).max();
}

#endif // CONFIG_EKF2_MAGNETOMETER

#if defined(CONFIG_EKF2_GNSS_YAW)

if (_control_status.flags.gnss_yaw) {
return _aid_src_gnss_yaw.innovation_variance;
}

#endif // CONFIG_EKF2_GNSS_YAW

#if defined(CONFIG_EKF2_EXTERNAL_VISION)

if (_control_status.flags.ev_yaw) {
return _aid_src_ev_yaw.innovation_variance;
}

#endif // CONFIG_EKF2_EXTERNAL_VISION

return 0.f;
}

float getHeadingInnovRatio() const
{
#if defined(CONFIG_EKF2_MAGNETOMETER)

if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
return Vector3f(_aid_src_mag.test_ratio).max();
}

#endif // CONFIG_EKF2_MAGNETOMETER

#if defined(CONFIG_EKF2_GNSS_YAW)

if (_control_status.flags.gnss_yaw) {
return _aid_src_gnss_yaw.test_ratio;
}

#endif // CONFIG_EKF2_GNSS_YAW

#if defined(CONFIG_EKF2_EXTERNAL_VISION)

if (_control_status.flags.ev_yaw) {
return _aid_src_ev_yaw.test_ratio;
}

#endif // CONFIG_EKF2_EXTERNAL_VISION

return 0.f;
}
float getHeadingInnov() const;
float getHeadingInnovVar() const;
float getHeadingInnovRatio() const;

#if defined(CONFIG_EKF2_DRAG_FUSION)
const auto &aid_src_drag() const { return _aid_src_drag; }
Expand Down Expand Up @@ -862,46 +779,7 @@ class Ekf final : public EstimatorInterface
float getMagDeclination();
#endif // CONFIG_EKF2_MAGNETOMETER

void clearInhibitedStateKalmanGains(VectorState &K) const
{
for (unsigned i = 0; i < State::gyro_bias.dof; i++) {
if (_gyro_bias_inhibit[i]) {
K(State::gyro_bias.idx + i) = 0.f;
}
}

for (unsigned i = 0; i < State::accel_bias.dof; i++) {
if (_accel_bias_inhibit[i]) {
K(State::accel_bias.idx + i) = 0.f;
}
}

#if defined(CONFIG_EKF2_MAGNETOMETER)

if (!_control_status.flags.mag) {
for (unsigned i = 0; i < State::mag_I.dof; i++) {
K(State::mag_I.idx + i) = 0.f;
}
}

if (!_control_status.flags.mag) {
for (unsigned i = 0; i < State::mag_B.dof; i++) {
K(State::mag_B.idx + i) = 0.f;
}
}

#endif // CONFIG_EKF2_MAGNETOMETER

#if defined(CONFIG_EKF2_WIND)

if (!_control_status.flags.wind) {
for (unsigned i = 0; i < State::wind_vel.dof; i++) {
K(State::wind_vel.idx + i) = 0.f;
}
}

#endif // CONFIG_EKF2_WIND
}
void clearInhibitedStateKalmanGains(VectorState &K) const;

// limit the diagonal of the covariance matrix
void constrainStateVariances();
Expand Down
128 changes: 128 additions & 0 deletions src/modules/ekf2/EKF/ekf_helper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1194,3 +1194,131 @@ void Ekf::updateAidSourceStatus(estimator_aid_source1d_s &status, const uint64_t
// reset
status.fused = false;
}

void Ekf::clearInhibitedStateKalmanGains(VectorState &K) const
{
for (unsigned i = 0; i < State::gyro_bias.dof; i++) {
if (_gyro_bias_inhibit[i]) {
K(State::gyro_bias.idx + i) = 0.f;
}
}

for (unsigned i = 0; i < State::accel_bias.dof; i++) {
if (_accel_bias_inhibit[i]) {
K(State::accel_bias.idx + i) = 0.f;
}
}

#if defined(CONFIG_EKF2_MAGNETOMETER)

if (!_control_status.flags.mag) {
for (unsigned i = 0; i < State::mag_I.dof; i++) {
K(State::mag_I.idx + i) = 0.f;
}
}

if (!_control_status.flags.mag) {
for (unsigned i = 0; i < State::mag_B.dof; i++) {
K(State::mag_B.idx + i) = 0.f;
}
}

#endif // CONFIG_EKF2_MAGNETOMETER

#if defined(CONFIG_EKF2_WIND)

if (!_control_status.flags.wind) {
for (unsigned i = 0; i < State::wind_vel.dof; i++) {
K(State::wind_vel.idx + i) = 0.f;
}
}

#endif // CONFIG_EKF2_WIND
}

float Ekf::getHeadingInnov() const
{
#if defined(CONFIG_EKF2_MAGNETOMETER)

if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
return Vector3f(_aid_src_mag.innovation).max();
}

#endif // CONFIG_EKF2_MAGNETOMETER

#if defined(CONFIG_EKF2_GNSS_YAW)

if (_control_status.flags.gnss_yaw) {
return _aid_src_gnss_yaw.innovation;
}

#endif // CONFIG_EKF2_GNSS_YAW

#if defined(CONFIG_EKF2_EXTERNAL_VISION)

if (_control_status.flags.ev_yaw) {
return _aid_src_ev_yaw.innovation;
}

#endif // CONFIG_EKF2_EXTERNAL_VISION

return 0.f;
}

float Ekf::getHeadingInnovVar() const
{
#if defined(CONFIG_EKF2_MAGNETOMETER)

if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
return Vector3f(_aid_src_mag.innovation_variance).max();
}

#endif // CONFIG_EKF2_MAGNETOMETER

#if defined(CONFIG_EKF2_GNSS_YAW)

if (_control_status.flags.gnss_yaw) {
return _aid_src_gnss_yaw.innovation_variance;
}

#endif // CONFIG_EKF2_GNSS_YAW

#if defined(CONFIG_EKF2_EXTERNAL_VISION)

if (_control_status.flags.ev_yaw) {
return _aid_src_ev_yaw.innovation_variance;
}

#endif // CONFIG_EKF2_EXTERNAL_VISION

return 0.f;
}

float Ekf::getHeadingInnovRatio() const
{
#if defined(CONFIG_EKF2_MAGNETOMETER)

if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
return Vector3f(_aid_src_mag.test_ratio).max();
}

#endif // CONFIG_EKF2_MAGNETOMETER

#if defined(CONFIG_EKF2_GNSS_YAW)

if (_control_status.flags.gnss_yaw) {
return _aid_src_gnss_yaw.test_ratio;
}

#endif // CONFIG_EKF2_GNSS_YAW

#if defined(CONFIG_EKF2_EXTERNAL_VISION)

if (_control_status.flags.ev_yaw) {
return _aid_src_ev_yaw.test_ratio;
}

#endif // CONFIG_EKF2_EXTERNAL_VISION

return 0.f;
}

0 comments on commit 8afd267

Please sign in to comment.