fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 20:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: estimator aid source status (GPS pos, GPS vel)

Browse Source
This commit is contained in:
Daniel Agar
2022-05-23 16:21:49 -04:00
parent 1ae467e9cd
commit 639222dd65
16 changed files with 813 additions and 628 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/gps_fusion.cpp
+122 -16
View File
@@ -39,39 +39,145 @@
/* #include <mathlib/mathlib.h> */
#include "ekf.h"
void Ekf::fuseGpsVelPos()
void Ekf::updateGpsVel(const gpsSample &gps_sample)
{
Vector3f gps_pos_obs_var;
const float vel_var = sq(gps_sample.sacc);
const Vector3f obs_var{vel_var, vel_var, vel_var * sq(1.5f)};
// innovation gate size
const float innov_gate = fmaxf(_params.gps_vel_innov_gate, 1.f);
auto &gps_vel = _aid_src_gnss_vel;
for (int i = 0; i < 3; i++) {
gps_vel.observation[i] = gps_sample.vel(i);
gps_vel.observation_variance[i] = obs_var(i);
gps_vel.innovation[i] = _state.vel(i) - gps_sample.vel(i);
gps_vel.innovation_variance[i] = P(4 + i, 4 + i) + obs_var(i);
gps_vel.test_ratio[i] = sq(gps_vel.innovation[i]) / (sq(innov_gate) * gps_vel.innovation_variance[i]);
gps_vel.innovation_rejected[i] = (gps_vel.test_ratio[i] > 1.f);
}
// vz 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 && gps_vel.innovation_rejected[2]) {
const float innov_limit = innov_gate * sqrtf(gps_vel.innovation_variance[2]);
gps_vel.innovation[2] = math::constrain(gps_vel.innovation[2], -innov_limit, innov_limit);
gps_vel.innovation_rejected[2] = false;
}
gps_vel.timestamp_sample = gps_sample.time_us;
}
void Ekf::updateGpsPos(const gpsSample &gps_sample)
{
Vector3f position;
position(0) = gps_sample.pos(0);
position(1) = gps_sample.pos(1);
// vertical position - gps measurement has opposite sign to earth z axis
position(2) = -(gps_sample.hgt - _gps_alt_ref - _gps_hgt_offset);
const float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
Vector3f obs_var;
if (isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
// if we are using other sources of aiding, then relax the upper observation
// noise limit which prevents bad GPS perturbing the position estimate
gps_pos_obs_var(0) = gps_pos_obs_var(1) = sq(fmaxf(_gps_sample_delayed.hacc, lower_limit));
obs_var(0) = obs_var(1) = sq(fmaxf(gps_sample.hacc, lower_limit));
} else {
// if we are not using another source of aiding, then we are reliant on the GPS
// observations to constrain attitude errors and must limit the observation noise value.
float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
gps_pos_obs_var(0) = gps_pos_obs_var(1) = sq(math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit));
obs_var(0) = obs_var(1) = sq(math::constrain(gps_sample.hacc, lower_limit, upper_limit));
}
obs_var(2) = getGpsHeightVariance();
// innovation gate size
float innov_gate = fmaxf(_params.gps_pos_innov_gate, 1.f);
const float vel_var = sq(_gps_sample_delayed.sacc);
const Vector3f gps_vel_obs_var{vel_var, vel_var, vel_var * sq(1.5f)};
auto &gps_pos = _aid_src_gnss_pos;
// calculate innovations
_gps_vel_innov = _state.vel - _gps_sample_delayed.vel;
_gps_pos_innov.xy() = Vector2f(_state.pos) - _gps_sample_delayed.pos;
for (int i = 0; i < 3; i++) {
gps_pos.observation[i] = position(i);
gps_pos.observation_variance[i] = obs_var(i);
// set innovation gate size
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);
gps_pos.innovation[i] = _state.pos(i) - position(i);
gps_pos.innovation_variance[i] = P(7 + i, 7 + i) + obs_var(i);
gps_pos.test_ratio[i] = sq(gps_pos.innovation[i]) / (sq(innov_gate) * gps_pos.innovation_variance[i]);
// fuse GPS measurement
fuseHorizontalVelocity(_gps_vel_innov, vel_innov_gate, 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, pos_innov_gate, gps_pos_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio);
gps_pos.innovation_rejected[i] = (gps_pos.test_ratio[i] > 1.f);
}
// 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 && gps_pos.innovation_rejected[2]) {
const float innov_limit = innov_gate * sqrtf(gps_pos.innovation_variance[2]);
gps_pos.innovation[2] = math::constrain(gps_pos.innovation[2], -innov_limit, innov_limit);
gps_pos.innovation_rejected[2] = false;
}
gps_pos.timestamp_sample = gps_sample.time_us;
}
void Ekf::fuseGpsVel()
{
// velocity
auto &gps_vel = _aid_src_gnss_vel;
// vx & vy
gps_vel.fusion_enabled[0] = true;
gps_vel.fusion_enabled[1] = true;
if (!gps_vel.innovation_rejected[0] && !gps_vel.innovation_rejected[1]) {
for (int i = 0; i < 2; i++) {
if (fuseVelPosHeight(gps_vel.innovation[i], gps_vel.innovation_variance[i], i)) {
gps_vel.fused[i] = true;
gps_vel.time_last_fuse[i] = _time_last_imu;
}
}
}
// vz
gps_vel.fusion_enabled[2] = true;
if (gps_vel.fusion_enabled[2] && !gps_vel.innovation_rejected[2]) {
if (fuseVelPosHeight(gps_vel.innovation[2], gps_vel.innovation_variance[2], 2)) {
gps_vel.fused[2] = true;
gps_vel.time_last_fuse[2] = _time_last_imu;
}
}
}
void Ekf::fuseGpsPos()
{
auto &gps_pos = _aid_src_gnss_pos;
// x & y
gps_pos.fusion_enabled[0] = true;
gps_pos.fusion_enabled[1] = true;
if (!gps_pos.innovation_rejected[0] && !gps_pos.innovation_rejected[1]) {
for (int i = 0; i < 2; i++) {
if (fuseVelPosHeight(gps_pos.innovation[i], gps_pos.innovation_variance[i], 3 + i)) {
gps_pos.fused[i] = true;
gps_pos.time_last_fuse[i] = _time_last_imu;
}
}
}
// z
gps_pos.fusion_enabled[2] = _control_status.flags.gps_hgt;
if (gps_pos.fusion_enabled[2] && !gps_pos.innovation_rejected[2]) {
if (fuseVelPosHeight(gps_pos.innovation[2], gps_pos.innovation_variance[2], 5)) {
gps_pos.fused[2] = true;
gps_pos.time_last_fuse[2] = _time_last_imu;
}
}
}