zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

ekf: use float instead of Vector2f for innov gate in pos/vel fusion 

We always use the same innovation gate for X and Y anyway
master
bresch 3 years ago committed by Daniel Agar
parent
5fb0084524
commit
3077f27821
5 changed files with 24 additions and 28 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 12
      src/modules/ekf2/EKF/control.cpp
  2. 8
      src/modules/ekf2/EKF/ekf.h
  3. 2
      src/modules/ekf2/EKF/fake_pos_control.cpp
  4. 10
      src/modules/ekf2/EKF/gps_fusion.cpp
  5. 20
      src/modules/ekf2/EKF/vel_pos_fusion.cpp

12
src/modules/ekf2/EKF/control.cpp
Unescape View File

@ -243,7 +243,6 @@ void Ekf::controlExternalVisionFusion()
} }
Vector3f ev_pos_obs_var; Vector3f ev_pos_obs_var;
Vector2f ev_pos_innov_gates;
// correct position and height for offset relative to IMU // correct position and height for offset relative to IMU
const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
@ -307,9 +306,9 @@ void Ekf::controlExternalVisionFusion()
} }
// innovation gate size // innovation gate size
ev_pos_innov_gates(0) = fmaxf(_params.ev_pos_innov_gate, 1.0f); const float ev_pos_innov_gate = fmaxf(_params.ev_pos_innov_gate, 1.0f);
fuseHorizontalPosition(_ev_pos_innov, ev_pos_innov_gates, ev_pos_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio); fuseHorizontalPosition(_ev_pos_innov, ev_pos_innov_gate, ev_pos_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio);
} }
// determine if we should use the velocity observations // determine if we should use the velocity observations
@ -332,10 +331,9 @@ void Ekf::controlExternalVisionFusion()
const Vector3f obs_var = matrix::max(getVisionVelocityVarianceInEkfFrame(), sq(0.05f)); const Vector3f obs_var = matrix::max(getVisionVelocityVarianceInEkfFrame(), sq(0.05f));
const float innov_gate = fmaxf(_params.ev_vel_innov_gate, 1.f); const float innov_gate = fmaxf(_params.ev_vel_innov_gate, 1.f);
const Vector2f ev_vel_innov_gates{innov_gate, innov_gate};
fuseHorizontalVelocity(_ev_vel_innov, ev_vel_innov_gates, obs_var, _ev_vel_innov_var, _ev_vel_test_ratio); fuseHorizontalVelocity(_ev_vel_innov, innov_gate, obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
fuseVerticalVelocity(_ev_vel_innov, ev_vel_innov_gates, obs_var, _ev_vel_innov_var, _ev_vel_test_ratio); fuseVerticalVelocity(_ev_vel_innov, innov_gate, obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
} }
// determine if we should use the yaw observation // determine if we should use the yaw observation
@ -1073,7 +1071,7 @@ void Ekf::controlAuxVelFusion()
if (isHorizontalAidingActive()) { if (isHorizontalAidingActive()) {
const Vector2f aux_vel_innov_gate(_params.auxvel_gate, _params.auxvel_gate); const float aux_vel_innov_gate = fmaxf(_params.auxvel_gate, 1.f);
_aux_vel_innov = _state.vel - auxvel_sample_delayed.vel; _aux_vel_innov = _state.vel - auxvel_sample_delayed.vel;

8
src/modules/ekf2/EKF/ekf.h
Unescape View File

@ -660,16 +660,16 @@ private:
// fuse optical flow line of sight rate measurements // fuse optical flow line of sight rate measurements
void fuseOptFlow(); void fuseOptFlow();
bool fuseHorizontalVelocity(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var, bool fuseHorizontalVelocity(const Vector3f &innov, float innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratio); Vector3f &innov_var, Vector2f &test_ratio);
bool fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var, bool fuseVerticalVelocity(const Vector3f &innov, float innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratio); Vector3f &innov_var, Vector2f &test_ratio);
bool fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var, bool fuseHorizontalPosition(const Vector3f &innov, float innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratiov, bool inhibit_gate = false); Vector3f &innov_var, Vector2f &test_ratiov, bool inhibit_gate = false);
bool fuseVerticalPosition(const float innov, const float innov_gate, const float obs_var, bool fuseVerticalPosition(float innov, float innov_gate, float obs_var,
float &innov_var, float &test_ratio); float &innov_var, float &test_ratio);
void fuseGpsVelPos(); void fuseGpsVelPos();

2
src/modules/ekf2/EKF/fake_pos_control.cpp
Unescape View File

@ -116,7 +116,7 @@ void Ekf::fuseFakePosition()
_gps_pos_innov.xy() = Vector2f(_state.pos) - _last_known_posNE; _gps_pos_innov.xy() = Vector2f(_state.pos) - _last_known_posNE;
const Vector2f fake_pos_innov_gate(3.0f, 3.0f); const float fake_pos_innov_gate = 3.f;
if (fuseHorizontalPosition(_gps_pos_innov, fake_pos_innov_gate, fake_pos_obs_var, if (fuseHorizontalPosition(_gps_pos_innov, fake_pos_innov_gate, fake_pos_obs_var,
_gps_pos_innov_var, _gps_pos_test_ratio, true)) { _gps_pos_innov_var, _gps_pos_test_ratio, true)) {

10
src/modules/ekf2/EKF/gps_fusion.cpp
Unescape View File

@ -78,13 +78,11 @@ void Ekf::fuseGpsVelPos()
_gps_pos_innov.xy() = Vector2f(_state.pos) - _gps_sample_delayed.pos; _gps_pos_innov.xy() = Vector2f(_state.pos) - _gps_sample_delayed.pos;
// set innovation gate size // set innovation gate size
const Vector2f gps_pos_innov_gates{fmaxf(_params.gps_pos_innov_gate, 1.f), 0.f}; const float pos_innov_gate = fmaxf(_params.gps_pos_innov_gate, 1.f);
const float vel_innov_gate = fmaxf(_params.gps_vel_innov_gate, 1.f); const float vel_innov_gate = fmaxf(_params.gps_vel_innov_gate, 1.f);
const Vector2f gps_vel_innov_gates{vel_innov_gate, vel_innov_gate};
// fuse GPS measurement // fuse GPS measurement
fuseHorizontalVelocity(_gps_vel_innov, gps_vel_innov_gates, gps_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio); fuseHorizontalVelocity(_gps_vel_innov, vel_innov_gate, gps_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio);
fuseVerticalVelocity(_gps_vel_innov, gps_vel_innov_gates, gps_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio); fuseVerticalVelocity(_gps_vel_innov, vel_innov_gate, gps_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio);
fuseHorizontalPosition(_gps_pos_innov, gps_pos_innov_gates, gps_pos_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio); fuseHorizontalPosition(_gps_pos_innov, pos_innov_gate, gps_pos_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio);
} }

20
src/modules/ekf2/EKF/vel_pos_fusion.cpp
Unescape View File

@ -44,14 +44,14 @@
#include <mathlib/mathlib.h> #include <mathlib/mathlib.h>
#include "ekf.h" #include "ekf.h"
bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var, bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const float innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratio) Vector3f &innov_var, Vector2f &test_ratio)
{ {
innov_var(0) = P(4, 4) + obs_var(0); innov_var(0) = P(4, 4) + obs_var(0);
innov_var(1) = P(5, 5) + obs_var(1); innov_var(1) = P(5, 5) + obs_var(1);
test_ratio(0) = fmaxf(sq(innov(0)) / (sq(innov_gate(0)) * innov_var(0)), test_ratio(0) = fmaxf(sq(innov(0)) / (sq(innov_gate) * innov_var(0)),
sq(innov(1)) / (sq(innov_gate(0)) * innov_var(1))); sq(innov(1)) / (sq(innov_gate) * innov_var(1)));
const bool innov_check_pass = (test_ratio(0) <= 1.0f); const bool innov_check_pass = (test_ratio(0) <= 1.0f);
@ -70,12 +70,12 @@ bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const Vector2f &innov_ga
} }
} }
bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var, bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const float innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratio) Vector3f &innov_var, Vector2f &test_ratio)
{ {
innov_var(2) = P(6, 6) + obs_var(2); innov_var(2) = P(6, 6) + obs_var(2);
test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2)); test_ratio(1) = sq(innov(2)) / (sq(innov_gate) * innov_var(2));
_vert_vel_innov_ratio = innov(2) / sqrtf(innov_var(2)); _vert_vel_innov_ratio = innov(2) / sqrtf(innov_var(2));
_vert_vel_fuse_time_us = _time_last_imu; _vert_vel_fuse_time_us = _time_last_imu;
bool innov_check_pass = (test_ratio(1) <= 1.0f); bool innov_check_pass = (test_ratio(1) <= 1.0f);
@ -85,7 +85,7 @@ bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate
float innovation; float innovation;
if (_fault_status.flags.bad_acc_vertical && !innov_check_pass) { if (_fault_status.flags.bad_acc_vertical && !innov_check_pass) {
const float innov_limit = innov_gate(1) * sqrtf(innov_var(2)); const float innov_limit = innov_gate * sqrtf(innov_var(2));
innovation = math::constrain(innov(2), -innov_limit, innov_limit); innovation = math::constrain(innov(2), -innov_limit, innov_limit);
innov_check_pass = true; innov_check_pass = true;
@ -107,19 +107,19 @@ bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate
} }
} }
bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var, bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const float innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratio, bool inhibit_gate) Vector3f &innov_var, Vector2f &test_ratio, bool inhibit_gate)
{ {
innov_var(0) = P(7, 7) + obs_var(0); innov_var(0) = P(7, 7) + obs_var(0);
innov_var(1) = P(8, 8) + obs_var(1); innov_var(1) = P(8, 8) + obs_var(1);
test_ratio(0) = fmaxf(sq(innov(0)) / (sq(innov_gate(0)) * innov_var(0)), test_ratio(0) = fmaxf(sq(innov(0)) / (sq(innov_gate) * innov_var(0)),
sq(innov(1)) / (sq(innov_gate(0)) * innov_var(1))); sq(innov(1)) / (sq(innov_gate) * innov_var(1)));
const bool innov_check_pass = test_ratio(0) <= 1.0f; const bool innov_check_pass = test_ratio(0) <= 1.0f;
if (innov_check_pass || inhibit_gate) { if (innov_check_pass || inhibit_gate) {
if (inhibit_gate && test_ratio(0) > sq(100.0f / innov_gate(0))) { if (inhibit_gate && test_ratio(0) > sq(100.0f / innov_gate)) {
// always protect against extreme values that could result in a NaN // always protect against extreme values that could result in a NaN
return false; return false;
} }

Write Preview
Loading…
Cancel
Save