forked from Archive/PX4-Autopilot
Compare commits
3 Commits
main
...
pr-ekf2_me
Author | SHA1 | Date |
---|---|---|
Daniel Agar | f907c4a0a1 | |
Daniel Agar | e729c10a71 | |
Daniel Agar | 534071573a |
|
@ -19,3 +19,4 @@ bool fused # true if the sample was successfully fused
|
||||||
|
|
||||||
# TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos
|
# TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos
|
||||||
# TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow estimator_aid_src_terrain_optical_flow
|
# TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow estimator_aid_src_terrain_optical_flow
|
||||||
|
# TOPICS estimator_aid_src_drag
|
||||||
|
|
|
@ -19,7 +19,6 @@ float32 baro_vpos # barometer height innovation (m) and innovation variance (m**
|
||||||
|
|
||||||
# Auxiliary velocity
|
# Auxiliary velocity
|
||||||
float32[2] aux_hvel # horizontal auxiliary velocity innovation from landing target measurement (m/sec) and innovation variance ((m/sec)**2)
|
float32[2] aux_hvel # horizontal auxiliary velocity innovation from landing target measurement (m/sec) and innovation variance ((m/sec)**2)
|
||||||
float32 aux_vvel # vertical auxiliary velocity innovation from landing target measurement (m/sec) and innovation variance ((m/sec)**2)
|
|
||||||
|
|
||||||
# Optical flow
|
# Optical flow
|
||||||
float32[2] flow # flow innvoation (rad/sec) and innovation variance ((rad/sec)**2)
|
float32[2] flow # flow innvoation (rad/sec) and innovation variance ((rad/sec)**2)
|
||||||
|
|
|
@ -49,6 +49,41 @@
|
||||||
|
|
||||||
#include <mathlib/mathlib.h>
|
#include <mathlib/mathlib.h>
|
||||||
|
|
||||||
|
void Ekf::setAirspeedData(const airspeedSample &airspeed_sample)
|
||||||
|
{
|
||||||
|
if (!_initialised) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Allocate the required buffer size if not previously done
|
||||||
|
if (_airspeed_buffer == nullptr) {
|
||||||
|
_airspeed_buffer = new RingBuffer<airspeedSample>(_obs_buffer_length);
|
||||||
|
|
||||||
|
if (_airspeed_buffer == nullptr || !_airspeed_buffer->valid()) {
|
||||||
|
delete _airspeed_buffer;
|
||||||
|
_airspeed_buffer = nullptr;
|
||||||
|
printBufferAllocationFailed("airspeed");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
const int64_t time_us = airspeed_sample.time_us
|
||||||
|
- static_cast<int64_t>(_params.airspeed_delay_ms * 1000)
|
||||||
|
- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
|
||||||
|
|
||||||
|
// limit data rate to prevent data being lost
|
||||||
|
if (time_us >= static_cast<int64_t>(_airspeed_buffer->get_newest().time_us + _min_obs_interval_us)) {
|
||||||
|
|
||||||
|
airspeedSample airspeed_sample_new{airspeed_sample};
|
||||||
|
airspeed_sample_new.time_us = time_us;
|
||||||
|
|
||||||
|
_airspeed_buffer->push(airspeed_sample_new);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
ECL_WARN("airspeed data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _airspeed_buffer->get_newest().time_us, _min_obs_interval_us);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
|
void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
|
||||||
{
|
{
|
||||||
// control activation and initialisation/reset of wind states required for airspeed fusion
|
// control activation and initialisation/reset of wind states required for airspeed fusion
|
||||||
|
|
|
@ -33,6 +33,41 @@
|
||||||
|
|
||||||
#include "ekf.h"
|
#include "ekf.h"
|
||||||
|
|
||||||
|
void Ekf::setAuxVelData(const auxVelSample &auxvel_sample)
|
||||||
|
{
|
||||||
|
if (!_initialised) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Allocate the required buffer size if not previously done
|
||||||
|
if (_auxvel_buffer == nullptr) {
|
||||||
|
_auxvel_buffer = new RingBuffer<auxVelSample>(_obs_buffer_length);
|
||||||
|
|
||||||
|
if (_auxvel_buffer == nullptr || !_auxvel_buffer->valid()) {
|
||||||
|
delete _auxvel_buffer;
|
||||||
|
_auxvel_buffer = nullptr;
|
||||||
|
printBufferAllocationFailed("aux vel");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
const int64_t time_us = auxvel_sample.time_us
|
||||||
|
- static_cast<int64_t>(_params.auxvel_delay_ms * 1000)
|
||||||
|
- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
|
||||||
|
|
||||||
|
// limit data rate to prevent data being lost
|
||||||
|
if (time_us >= static_cast<int64_t>(_auxvel_buffer->get_newest().time_us + _min_obs_interval_us)) {
|
||||||
|
|
||||||
|
auxVelSample auxvel_sample_new{auxvel_sample};
|
||||||
|
auxvel_sample_new.time_us = time_us;
|
||||||
|
|
||||||
|
_auxvel_buffer->push(auxvel_sample_new);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
ECL_WARN("aux velocity data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _auxvel_buffer->get_newest().time_us, _min_obs_interval_us);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
void Ekf::controlAuxVelFusion()
|
void Ekf::controlAuxVelFusion()
|
||||||
{
|
{
|
||||||
if (_auxvel_buffer) {
|
if (_auxvel_buffer) {
|
||||||
|
|
|
@ -38,6 +38,42 @@
|
||||||
|
|
||||||
#include "ekf.h"
|
#include "ekf.h"
|
||||||
|
|
||||||
|
void Ekf::setBaroData(const baroSample &baro_sample)
|
||||||
|
{
|
||||||
|
if (!_initialised) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Allocate the required buffer size if not previously done
|
||||||
|
if (_baro_buffer == nullptr) {
|
||||||
|
_baro_buffer = new RingBuffer<baroSample>(_obs_buffer_length);
|
||||||
|
|
||||||
|
if (_baro_buffer == nullptr || !_baro_buffer->valid()) {
|
||||||
|
delete _baro_buffer;
|
||||||
|
_baro_buffer = nullptr;
|
||||||
|
printBufferAllocationFailed("baro");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
const int64_t time_us = baro_sample.time_us
|
||||||
|
- static_cast<int64_t>(_params.baro_delay_ms * 1000)
|
||||||
|
- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
|
||||||
|
|
||||||
|
// limit data rate to prevent data being lost
|
||||||
|
if (time_us >= static_cast<int64_t>(_baro_buffer->get_newest().time_us + _min_obs_interval_us)) {
|
||||||
|
|
||||||
|
baroSample baro_sample_new{baro_sample};
|
||||||
|
baro_sample_new.time_us = time_us;
|
||||||
|
|
||||||
|
_baro_buffer->push(baro_sample_new);
|
||||||
|
_time_last_baro_buffer_push = _time_latest_us;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
ECL_WARN("baro data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _baro_buffer->get_newest().time_us, _min_obs_interval_us);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
void Ekf::controlBaroHeightFusion()
|
void Ekf::controlBaroHeightFusion()
|
||||||
{
|
{
|
||||||
static constexpr const char *HGT_SRC_NAME = "baro";
|
static constexpr const char *HGT_SRC_NAME = "baro";
|
||||||
|
|
|
@ -122,7 +122,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
|
||||||
#endif // CONFIG_EKF2_SIDESLIP
|
#endif // CONFIG_EKF2_SIDESLIP
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
||||||
controlDragFusion();
|
controlDragFusion(imu_delayed);
|
||||||
#endif // CONFIG_EKF2_DRAG_FUSION
|
#endif // CONFIG_EKF2_DRAG_FUSION
|
||||||
|
|
||||||
controlHeightFusion(imu_delayed);
|
controlHeightFusion(imu_delayed);
|
||||||
|
|
|
@ -42,12 +42,73 @@
|
||||||
|
|
||||||
#include <mathlib/mathlib.h>
|
#include <mathlib/mathlib.h>
|
||||||
|
|
||||||
void Ekf::controlDragFusion()
|
void Ekf::setDragData(const imuSample &imu)
|
||||||
{
|
{
|
||||||
if ((_params.drag_ctrl > 0) && _drag_buffer &&
|
// down-sample the drag specific force data by accumulating and calculating the mean when
|
||||||
!_control_status.flags.fake_pos && _control_status.flags.in_air) {
|
// sufficient samples have been collected
|
||||||
|
if (_params.drag_ctrl > 0) {
|
||||||
|
|
||||||
if (!_control_status.flags.wind) {
|
// Allocate the required buffer size if not previously done
|
||||||
|
if (_drag_buffer == nullptr) {
|
||||||
|
_drag_buffer = new RingBuffer<dragSample>(_obs_buffer_length);
|
||||||
|
|
||||||
|
if (_drag_buffer == nullptr || !_drag_buffer->valid()) {
|
||||||
|
delete _drag_buffer;
|
||||||
|
_drag_buffer = nullptr;
|
||||||
|
printBufferAllocationFailed("drag");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// don't use any accel samples that are clipping
|
||||||
|
if (imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2]) {
|
||||||
|
// reset accumulators
|
||||||
|
_drag_sample_count = 0;
|
||||||
|
_drag_down_sampled.accelXY.zero();
|
||||||
|
_drag_down_sampled.time_us = 0;
|
||||||
|
_drag_sample_time_dt = 0.0f;
|
||||||
|
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
_drag_sample_count++;
|
||||||
|
// note acceleration is accumulated as a delta velocity
|
||||||
|
_drag_down_sampled.accelXY(0) += imu.delta_vel(0);
|
||||||
|
_drag_down_sampled.accelXY(1) += imu.delta_vel(1);
|
||||||
|
_drag_down_sampled.time_us += imu.time_us;
|
||||||
|
_drag_sample_time_dt += imu.delta_vel_dt;
|
||||||
|
|
||||||
|
// calculate the downsample ratio for drag specific force data
|
||||||
|
uint8_t min_sample_ratio = (uint8_t) ceilf((float)_imu_buffer_length / _obs_buffer_length);
|
||||||
|
|
||||||
|
if (min_sample_ratio < 5) {
|
||||||
|
min_sample_ratio = 5;
|
||||||
|
}
|
||||||
|
|
||||||
|
// calculate and store means from accumulated values
|
||||||
|
if (_drag_sample_count >= min_sample_ratio) {
|
||||||
|
// note conversion from accumulated delta velocity to acceleration
|
||||||
|
_drag_down_sampled.accelXY(0) /= _drag_sample_time_dt;
|
||||||
|
_drag_down_sampled.accelXY(1) /= _drag_sample_time_dt;
|
||||||
|
_drag_down_sampled.time_us /= _drag_sample_count;
|
||||||
|
|
||||||
|
// write to buffer
|
||||||
|
_drag_buffer->push(_drag_down_sampled);
|
||||||
|
|
||||||
|
// reset accumulators
|
||||||
|
_drag_sample_count = 0;
|
||||||
|
_drag_down_sampled.accelXY.zero();
|
||||||
|
_drag_down_sampled.time_us = 0;
|
||||||
|
_drag_sample_time_dt = 0.0f;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Ekf::controlDragFusion(const imuSample &imu_delayed)
|
||||||
|
{
|
||||||
|
if ((_params.drag_ctrl > 0) && _drag_buffer) {
|
||||||
|
|
||||||
|
if (!_control_status.flags.wind && !_control_status.flags.fake_pos && _control_status.flags.in_air) {
|
||||||
// reset the wind states and covariances when starting drag accel fusion
|
// reset the wind states and covariances when starting drag accel fusion
|
||||||
_control_status.flags.wind = true;
|
_control_status.flags.wind = true;
|
||||||
resetWindToZero();
|
resetWindToZero();
|
||||||
|
@ -55,7 +116,7 @@ void Ekf::controlDragFusion()
|
||||||
|
|
||||||
dragSample drag_sample;
|
dragSample drag_sample;
|
||||||
|
|
||||||
if (_drag_buffer->pop_first_older_than(_time_delayed_us, &drag_sample)) {
|
if (_drag_buffer->pop_first_older_than(imu_delayed.time_us, &drag_sample)) {
|
||||||
fuseDrag(drag_sample);
|
fuseDrag(drag_sample);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -87,14 +148,14 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
|
||||||
const float rel_wind_speed = rel_wind_body.norm();
|
const float rel_wind_speed = rel_wind_body.norm();
|
||||||
const VectorState state_vector_prev = getStateAtFusionHorizonAsVector();
|
const VectorState state_vector_prev = getStateAtFusionHorizonAsVector();
|
||||||
|
|
||||||
Vector2f bcoef_inv;
|
Vector2f bcoef_inv{0.f, 0.f};
|
||||||
|
|
||||||
if (using_bcoef_x) {
|
if (using_bcoef_x) {
|
||||||
bcoef_inv(0) = 1.0f / _params.bcoef_x;
|
bcoef_inv(0) = 1.f / _params.bcoef_x;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (using_bcoef_y) {
|
if (using_bcoef_y) {
|
||||||
bcoef_inv(1) = 1.0f / _params.bcoef_y;
|
bcoef_inv(1) = 1.f / _params.bcoef_y;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (using_bcoef_x && using_bcoef_y) {
|
if (using_bcoef_x && using_bcoef_y) {
|
||||||
|
@ -105,6 +166,11 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
|
||||||
bcoef_inv(1) = bcoef_inv(0);
|
bcoef_inv(1) = bcoef_inv(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
_aid_src_drag.timestamp_sample = drag_sample.time_us;
|
||||||
|
_aid_src_drag.fused = false;
|
||||||
|
|
||||||
|
bool fused[] {false, false};
|
||||||
|
|
||||||
VectorState Kfusion;
|
VectorState Kfusion;
|
||||||
|
|
||||||
// perform sequential fusion of XY specific forces
|
// perform sequential fusion of XY specific forces
|
||||||
|
@ -115,36 +181,51 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
|
||||||
// Drag is modelled as an arbitrary combination of bluff body drag that proportional to
|
// Drag is modelled as an arbitrary combination of bluff body drag that proportional to
|
||||||
// equivalent airspeed squared, and rotor momentum drag that is proportional to true airspeed
|
// equivalent airspeed squared, and rotor momentum drag that is proportional to true airspeed
|
||||||
// parallel to the rotor disc and mass flow through the rotor disc.
|
// parallel to the rotor disc and mass flow through the rotor disc.
|
||||||
|
const float pred_acc = -0.5f * bcoef_inv(axis_index) * rho * rel_wind_body(axis_index) * rel_wind_speed - rel_wind_body(axis_index) * mcoef_corrrected;
|
||||||
|
|
||||||
|
_aid_src_drag.observation[axis_index] = mea_acc;
|
||||||
|
_aid_src_drag.observation_variance[axis_index] = R_ACC;
|
||||||
|
_aid_src_drag.innovation[axis_index] = pred_acc - mea_acc;
|
||||||
|
_aid_src_drag.innovation_variance[axis_index] = NAN; // reset
|
||||||
|
|
||||||
if (axis_index == 0) {
|
if (axis_index == 0) {
|
||||||
|
sym::ComputeDragXInnovVarAndK(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON,
|
||||||
|
&_aid_src_drag.innovation_variance[axis_index], &Kfusion);
|
||||||
|
|
||||||
if (!using_bcoef_x && !using_mcoef) {
|
if (!using_bcoef_x && !using_mcoef) {
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
sym::ComputeDragXInnovVarAndK(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON, &_drag_innov_var(axis_index), &Kfusion);
|
|
||||||
|
|
||||||
} else if (axis_index == 1) {
|
} else if (axis_index == 1) {
|
||||||
|
sym::ComputeDragYInnovVarAndK(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON,
|
||||||
|
&_aid_src_drag.innovation_variance[axis_index], &Kfusion);
|
||||||
|
|
||||||
if (!using_bcoef_y && !using_mcoef) {
|
if (!using_bcoef_y && !using_mcoef) {
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
sym::ComputeDragYInnovVarAndK(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON, &_drag_innov_var(axis_index), &Kfusion);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (_drag_innov_var(axis_index) < R_ACC) {
|
if (_aid_src_drag.innovation_variance[axis_index] < R_ACC) {
|
||||||
// calculation is badly conditioned
|
// calculation is badly conditioned
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
const float pred_acc = -0.5f * bcoef_inv(axis_index) * rho * rel_wind_body(axis_index) * rel_wind_speed - rel_wind_body(axis_index) * mcoef_corrrected;
|
|
||||||
|
|
||||||
// Apply an innovation consistency check with a 5 Sigma threshold
|
// Apply an innovation consistency check with a 5 Sigma threshold
|
||||||
_drag_innov(axis_index) = pred_acc - mea_acc;
|
const float innov_gate = 5.f;
|
||||||
_drag_test_ratio(axis_index) = sq(_drag_innov(axis_index)) / (sq(5.0f) * _drag_innov_var(axis_index));
|
setEstimatorAidStatusTestRatio(_aid_src_drag, innov_gate);
|
||||||
|
|
||||||
// if the innovation consistency check fails then don't fuse the sample
|
if (_control_status.flags.in_air && _control_status.flags.wind && !_control_status.flags.fake_pos
|
||||||
if (_drag_test_ratio(axis_index) <= 1.0f) {
|
&& PX4_ISFINITE(_aid_src_drag.innovation_variance[axis_index]) && PX4_ISFINITE(_aid_src_drag.innovation[axis_index])
|
||||||
measurementUpdate(Kfusion, _drag_innov_var(axis_index), _drag_innov(axis_index));
|
&& (_aid_src_drag.test_ratio[axis_index] < 1.f)
|
||||||
|
) {
|
||||||
|
if (measurementUpdate(Kfusion, _aid_src_drag.innovation_variance[axis_index], _aid_src_drag.innovation[axis_index])) {
|
||||||
|
fused[axis_index] = true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (fused[0] && fused[1]) {
|
||||||
|
_aid_src_drag.fused = true;
|
||||||
|
_aid_src_drag.time_last_fuse = _time_delayed_us;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
File diff suppressed because it is too large
Load Diff
|
@ -291,84 +291,6 @@ Vector3f Ekf::calcEarthRateNED(float lat_rad) const
|
||||||
-CONSTANTS_EARTH_SPIN_RATE * sinf(lat_rad));
|
-CONSTANTS_EARTH_SPIN_RATE * sinf(lat_rad));
|
||||||
}
|
}
|
||||||
|
|
||||||
void Ekf::getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const
|
|
||||||
{
|
|
||||||
hvel[0] = _aid_src_gnss_vel.innovation[0];
|
|
||||||
hvel[1] = _aid_src_gnss_vel.innovation[1];
|
|
||||||
vvel = _aid_src_gnss_vel.innovation[2];
|
|
||||||
|
|
||||||
hpos[0] = _aid_src_gnss_pos.innovation[0];
|
|
||||||
hpos[1] = _aid_src_gnss_pos.innovation[1];
|
|
||||||
vpos = _aid_src_gnss_hgt.innovation;
|
|
||||||
}
|
|
||||||
|
|
||||||
void Ekf::getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const
|
|
||||||
{
|
|
||||||
hvel[0] = _aid_src_gnss_vel.innovation_variance[0];
|
|
||||||
hvel[1] = _aid_src_gnss_vel.innovation_variance[1];
|
|
||||||
vvel = _aid_src_gnss_vel.innovation_variance[2];
|
|
||||||
|
|
||||||
hpos[0] = _aid_src_gnss_pos.innovation_variance[0];
|
|
||||||
hpos[1] = _aid_src_gnss_pos.innovation_variance[1];
|
|
||||||
vpos = _aid_src_gnss_hgt.innovation_variance;
|
|
||||||
}
|
|
||||||
|
|
||||||
void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const
|
|
||||||
{
|
|
||||||
hvel = fmaxf(_aid_src_gnss_vel.test_ratio[0], _aid_src_gnss_vel.test_ratio[1]);
|
|
||||||
vvel = _aid_src_gnss_vel.test_ratio[2];
|
|
||||||
|
|
||||||
hpos = fmaxf(_aid_src_gnss_pos.test_ratio[0], _aid_src_gnss_pos.test_ratio[1]);
|
|
||||||
vpos = _aid_src_gnss_hgt.test_ratio;
|
|
||||||
}
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
|
||||||
void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const
|
|
||||||
{
|
|
||||||
hvel[0] = _aid_src_ev_vel.innovation[0];
|
|
||||||
hvel[1] = _aid_src_ev_vel.innovation[1];
|
|
||||||
vvel = _aid_src_ev_vel.innovation[2];
|
|
||||||
|
|
||||||
hpos[0] = _aid_src_ev_pos.innovation[0];
|
|
||||||
hpos[1] = _aid_src_ev_pos.innovation[1];
|
|
||||||
vpos = _aid_src_ev_hgt.innovation;
|
|
||||||
}
|
|
||||||
|
|
||||||
void Ekf::getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const
|
|
||||||
{
|
|
||||||
hvel[0] = _aid_src_ev_vel.innovation_variance[0];
|
|
||||||
hvel[1] = _aid_src_ev_vel.innovation_variance[1];
|
|
||||||
vvel = _aid_src_ev_vel.innovation_variance[2];
|
|
||||||
|
|
||||||
hpos[0] = _aid_src_ev_pos.innovation_variance[0];
|
|
||||||
hpos[1] = _aid_src_ev_pos.innovation_variance[1];
|
|
||||||
vpos = _aid_src_ev_hgt.innovation_variance;
|
|
||||||
}
|
|
||||||
|
|
||||||
void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const
|
|
||||||
{
|
|
||||||
hvel = fmaxf(_aid_src_ev_vel.test_ratio[0], _aid_src_ev_vel.test_ratio[1]);
|
|
||||||
vvel = _aid_src_ev_vel.test_ratio[2];
|
|
||||||
|
|
||||||
hpos = fmaxf(_aid_src_ev_pos.test_ratio[0], _aid_src_ev_pos.test_ratio[1]);
|
|
||||||
vpos = _aid_src_ev_hgt.test_ratio;
|
|
||||||
}
|
|
||||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_AUXVEL)
|
|
||||||
void Ekf::getAuxVelInnov(float aux_vel_innov[2]) const
|
|
||||||
{
|
|
||||||
aux_vel_innov[0] = _aid_src_aux_vel.innovation[0];
|
|
||||||
aux_vel_innov[1] = _aid_src_aux_vel.innovation[1];
|
|
||||||
}
|
|
||||||
|
|
||||||
void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const
|
|
||||||
{
|
|
||||||
aux_vel_innov_var[0] = _aid_src_aux_vel.innovation_variance[0];
|
|
||||||
aux_vel_innov_var[1] = _aid_src_aux_vel.innovation_variance[1];
|
|
||||||
}
|
|
||||||
#endif // CONFIG_EKF2_AUXVEL
|
|
||||||
|
|
||||||
// get the state vector at the delayed time horizon
|
// get the state vector at the delayed time horizon
|
||||||
matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
|
matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
|
||||||
{
|
{
|
||||||
|
|
|
@ -40,11 +40,11 @@
|
||||||
* @author Siddharth B Purohit <siddharthbharatpurohit@gmail.com>
|
* @author Siddharth B Purohit <siddharthbharatpurohit@gmail.com>
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "estimator_interface.h"
|
#include "ekf.h"
|
||||||
|
|
||||||
#include <mathlib/mathlib.h>
|
#include <mathlib/mathlib.h>
|
||||||
|
|
||||||
EstimatorInterface::~EstimatorInterface()
|
Ekf::~Ekf()
|
||||||
{
|
{
|
||||||
delete _gps_buffer;
|
delete _gps_buffer;
|
||||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||||
|
@ -74,7 +74,7 @@ EstimatorInterface::~EstimatorInterface()
|
||||||
}
|
}
|
||||||
|
|
||||||
// Accumulate imu data and store to buffer at desired rate
|
// Accumulate imu data and store to buffer at desired rate
|
||||||
void EstimatorInterface::setIMUData(const imuSample &imu_sample)
|
void Ekf::setIMUData(const imuSample &imu_sample)
|
||||||
{
|
{
|
||||||
// TODO: resolve misplaced responsibility
|
// TODO: resolve misplaced responsibility
|
||||||
if (!_initialised) {
|
if (!_initialised) {
|
||||||
|
@ -84,7 +84,9 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
|
||||||
_time_latest_us = imu_sample.time_us;
|
_time_latest_us = imu_sample.time_us;
|
||||||
|
|
||||||
// the output observer always runs
|
// the output observer always runs
|
||||||
_output_predictor.calculateOutputStates(imu_sample.time_us, imu_sample.delta_ang, imu_sample.delta_ang_dt, imu_sample.delta_vel, imu_sample.delta_vel_dt);
|
_output_predictor.calculateOutputStates(imu_sample.time_us,
|
||||||
|
imu_sample.delta_ang, imu_sample.delta_ang_dt,
|
||||||
|
imu_sample.delta_vel, imu_sample.delta_vel_dt);
|
||||||
|
|
||||||
// accumulate and down-sample imu data and push to the buffer when new downsampled data becomes available
|
// accumulate and down-sample imu data and push to the buffer when new downsampled data becomes available
|
||||||
if (_imu_down_sampler.update(imu_sample)) {
|
if (_imu_down_sampler.update(imu_sample)) {
|
||||||
|
@ -106,350 +108,7 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
|
||||||
#endif // CONFIG_EKF2_DRAG_FUSION
|
#endif // CONFIG_EKF2_DRAG_FUSION
|
||||||
}
|
}
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
void Ekf::setSystemFlagData(const systemFlagUpdate &system_flags)
|
||||||
void EstimatorInterface::setMagData(const magSample &mag_sample)
|
|
||||||
{
|
|
||||||
if (!_initialised) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Allocate the required buffer size if not previously done
|
|
||||||
if (_mag_buffer == nullptr) {
|
|
||||||
_mag_buffer = new RingBuffer<magSample>(_obs_buffer_length);
|
|
||||||
|
|
||||||
if (_mag_buffer == nullptr || !_mag_buffer->valid()) {
|
|
||||||
delete _mag_buffer;
|
|
||||||
_mag_buffer = nullptr;
|
|
||||||
printBufferAllocationFailed("mag");
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
const int64_t time_us = mag_sample.time_us
|
|
||||||
- static_cast<int64_t>(_params.mag_delay_ms * 1000)
|
|
||||||
- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
|
|
||||||
|
|
||||||
// limit data rate to prevent data being lost
|
|
||||||
if (time_us >= static_cast<int64_t>(_mag_buffer->get_newest().time_us + _min_obs_interval_us)) {
|
|
||||||
|
|
||||||
magSample mag_sample_new{mag_sample};
|
|
||||||
mag_sample_new.time_us = time_us;
|
|
||||||
|
|
||||||
_mag_buffer->push(mag_sample_new);
|
|
||||||
_time_last_mag_buffer_push = _time_latest_us;
|
|
||||||
|
|
||||||
} else {
|
|
||||||
ECL_WARN("mag data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _mag_buffer->get_newest().time_us, _min_obs_interval_us);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
|
||||||
|
|
||||||
void EstimatorInterface::setGpsData(const gpsMessage &gps)
|
|
||||||
{
|
|
||||||
if (!_initialised) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Allocate the required buffer size if not previously done
|
|
||||||
if (_gps_buffer == nullptr) {
|
|
||||||
_gps_buffer = new RingBuffer<gpsSample>(_obs_buffer_length);
|
|
||||||
|
|
||||||
if (_gps_buffer == nullptr || !_gps_buffer->valid()) {
|
|
||||||
delete _gps_buffer;
|
|
||||||
_gps_buffer = nullptr;
|
|
||||||
printBufferAllocationFailed("GPS");
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
const int64_t time_us = gps.time_usec
|
|
||||||
- static_cast<int64_t>(_params.gps_delay_ms * 1000)
|
|
||||||
- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
|
|
||||||
|
|
||||||
if (time_us >= static_cast<int64_t>(_gps_buffer->get_newest().time_us + _min_obs_interval_us)) {
|
|
||||||
|
|
||||||
if (!gps.vel_ned_valid || (gps.fix_type == 0)) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
gpsSample gps_sample_new;
|
|
||||||
|
|
||||||
gps_sample_new.time_us = time_us;
|
|
||||||
|
|
||||||
gps_sample_new.vel = gps.vel_ned;
|
|
||||||
|
|
||||||
gps_sample_new.sacc = gps.sacc;
|
|
||||||
gps_sample_new.hacc = gps.eph;
|
|
||||||
gps_sample_new.vacc = gps.epv;
|
|
||||||
|
|
||||||
gps_sample_new.hgt = (float)gps.alt * 1e-3f;
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_GNSS_YAW)
|
|
||||||
|
|
||||||
if (PX4_ISFINITE(gps.yaw)) {
|
|
||||||
_time_last_gps_yaw_buffer_push = _time_latest_us;
|
|
||||||
gps_sample_new.yaw = gps.yaw;
|
|
||||||
gps_sample_new.yaw_acc = PX4_ISFINITE(gps.yaw_accuracy) ? gps.yaw_accuracy : 0.f;
|
|
||||||
|
|
||||||
} else {
|
|
||||||
gps_sample_new.yaw = NAN;
|
|
||||||
gps_sample_new.yaw_acc = 0.f;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (PX4_ISFINITE(gps.yaw_offset)) {
|
|
||||||
_gps_yaw_offset = gps.yaw_offset;
|
|
||||||
|
|
||||||
} else {
|
|
||||||
_gps_yaw_offset = 0.0f;
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif // CONFIG_EKF2_GNSS_YAW
|
|
||||||
|
|
||||||
// Only calculate the relative position if the WGS-84 location of the origin is set
|
|
||||||
if (collect_gps(gps)) {
|
|
||||||
gps_sample_new.pos = _pos_ref.project((gps.lat / 1.0e7), (gps.lon / 1.0e7));
|
|
||||||
|
|
||||||
} else {
|
|
||||||
gps_sample_new.pos(0) = 0.0f;
|
|
||||||
gps_sample_new.pos(1) = 0.0f;
|
|
||||||
}
|
|
||||||
|
|
||||||
_gps_buffer->push(gps_sample_new);
|
|
||||||
_time_last_gps_buffer_push = _time_latest_us;
|
|
||||||
|
|
||||||
|
|
||||||
} else {
|
|
||||||
ECL_WARN("GPS data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _gps_buffer->get_newest().time_us, _min_obs_interval_us);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_BAROMETER)
|
|
||||||
void EstimatorInterface::setBaroData(const baroSample &baro_sample)
|
|
||||||
{
|
|
||||||
if (!_initialised) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Allocate the required buffer size if not previously done
|
|
||||||
if (_baro_buffer == nullptr) {
|
|
||||||
_baro_buffer = new RingBuffer<baroSample>(_obs_buffer_length);
|
|
||||||
|
|
||||||
if (_baro_buffer == nullptr || !_baro_buffer->valid()) {
|
|
||||||
delete _baro_buffer;
|
|
||||||
_baro_buffer = nullptr;
|
|
||||||
printBufferAllocationFailed("baro");
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
const int64_t time_us = baro_sample.time_us
|
|
||||||
- static_cast<int64_t>(_params.baro_delay_ms * 1000)
|
|
||||||
- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
|
|
||||||
|
|
||||||
// limit data rate to prevent data being lost
|
|
||||||
if (time_us >= static_cast<int64_t>(_baro_buffer->get_newest().time_us + _min_obs_interval_us)) {
|
|
||||||
|
|
||||||
baroSample baro_sample_new{baro_sample};
|
|
||||||
baro_sample_new.time_us = time_us;
|
|
||||||
|
|
||||||
_baro_buffer->push(baro_sample_new);
|
|
||||||
_time_last_baro_buffer_push = _time_latest_us;
|
|
||||||
|
|
||||||
} else {
|
|
||||||
ECL_WARN("baro data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _baro_buffer->get_newest().time_us, _min_obs_interval_us);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif // CONFIG_EKF2_BAROMETER
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
|
||||||
void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
|
|
||||||
{
|
|
||||||
if (!_initialised) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Allocate the required buffer size if not previously done
|
|
||||||
if (_airspeed_buffer == nullptr) {
|
|
||||||
_airspeed_buffer = new RingBuffer<airspeedSample>(_obs_buffer_length);
|
|
||||||
|
|
||||||
if (_airspeed_buffer == nullptr || !_airspeed_buffer->valid()) {
|
|
||||||
delete _airspeed_buffer;
|
|
||||||
_airspeed_buffer = nullptr;
|
|
||||||
printBufferAllocationFailed("airspeed");
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
const int64_t time_us = airspeed_sample.time_us
|
|
||||||
- static_cast<int64_t>(_params.airspeed_delay_ms * 1000)
|
|
||||||
- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
|
|
||||||
|
|
||||||
// limit data rate to prevent data being lost
|
|
||||||
if (time_us >= static_cast<int64_t>(_airspeed_buffer->get_newest().time_us + _min_obs_interval_us)) {
|
|
||||||
|
|
||||||
airspeedSample airspeed_sample_new{airspeed_sample};
|
|
||||||
airspeed_sample_new.time_us = time_us;
|
|
||||||
|
|
||||||
_airspeed_buffer->push(airspeed_sample_new);
|
|
||||||
|
|
||||||
} else {
|
|
||||||
ECL_WARN("airspeed data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _airspeed_buffer->get_newest().time_us, _min_obs_interval_us);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif // CONFIG_EKF2_AIRSPEED
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
|
||||||
void EstimatorInterface::setRangeData(const rangeSample &range_sample)
|
|
||||||
{
|
|
||||||
if (!_initialised) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Allocate the required buffer size if not previously done
|
|
||||||
if (_range_buffer == nullptr) {
|
|
||||||
_range_buffer = new RingBuffer<rangeSample>(_obs_buffer_length);
|
|
||||||
|
|
||||||
if (_range_buffer == nullptr || !_range_buffer->valid()) {
|
|
||||||
delete _range_buffer;
|
|
||||||
_range_buffer = nullptr;
|
|
||||||
printBufferAllocationFailed("range");
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
const int64_t time_us = range_sample.time_us
|
|
||||||
- static_cast<int64_t>(_params.range_delay_ms * 1000)
|
|
||||||
- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
|
|
||||||
|
|
||||||
// limit data rate to prevent data being lost
|
|
||||||
if (time_us >= static_cast<int64_t>(_range_buffer->get_newest().time_us + _min_obs_interval_us)) {
|
|
||||||
|
|
||||||
rangeSample range_sample_new{range_sample};
|
|
||||||
range_sample_new.time_us = time_us;
|
|
||||||
|
|
||||||
_range_buffer->push(range_sample_new);
|
|
||||||
_time_last_range_buffer_push = _time_latest_us;
|
|
||||||
|
|
||||||
} else {
|
|
||||||
ECL_WARN("range data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _range_buffer->get_newest().time_us, _min_obs_interval_us);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
|
||||||
void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
|
|
||||||
{
|
|
||||||
if (!_initialised) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Allocate the required buffer size if not previously done
|
|
||||||
if (_flow_buffer == nullptr) {
|
|
||||||
_flow_buffer = new RingBuffer<flowSample>(_imu_buffer_length);
|
|
||||||
|
|
||||||
if (_flow_buffer == nullptr || !_flow_buffer->valid()) {
|
|
||||||
delete _flow_buffer;
|
|
||||||
_flow_buffer = nullptr;
|
|
||||||
printBufferAllocationFailed("flow");
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
const int64_t time_us = flow.time_us
|
|
||||||
- static_cast<int64_t>(_params.flow_delay_ms * 1000)
|
|
||||||
- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
|
|
||||||
|
|
||||||
// limit data rate to prevent data being lost
|
|
||||||
if (time_us >= static_cast<int64_t>(_flow_buffer->get_newest().time_us + _min_obs_interval_us)) {
|
|
||||||
|
|
||||||
flowSample optflow_sample_new{flow};
|
|
||||||
optflow_sample_new.time_us = time_us;
|
|
||||||
|
|
||||||
_flow_buffer->push(optflow_sample_new);
|
|
||||||
|
|
||||||
} else {
|
|
||||||
ECL_WARN("optical flow data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _flow_buffer->get_newest().time_us, _min_obs_interval_us);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
|
||||||
void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
|
|
||||||
{
|
|
||||||
if (!_initialised) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Allocate the required buffer size if not previously done
|
|
||||||
if (_ext_vision_buffer == nullptr) {
|
|
||||||
_ext_vision_buffer = new RingBuffer<extVisionSample>(_obs_buffer_length);
|
|
||||||
|
|
||||||
if (_ext_vision_buffer == nullptr || !_ext_vision_buffer->valid()) {
|
|
||||||
delete _ext_vision_buffer;
|
|
||||||
_ext_vision_buffer = nullptr;
|
|
||||||
printBufferAllocationFailed("vision");
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// calculate the system time-stamp for the mid point of the integration period
|
|
||||||
const int64_t time_us = evdata.time_us
|
|
||||||
- static_cast<int64_t>(_params.ev_delay_ms * 1000)
|
|
||||||
- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
|
|
||||||
|
|
||||||
// limit data rate to prevent data being lost
|
|
||||||
if (time_us >= static_cast<int64_t>(_ext_vision_buffer->get_newest().time_us + _min_obs_interval_us)) {
|
|
||||||
|
|
||||||
extVisionSample ev_sample_new{evdata};
|
|
||||||
ev_sample_new.time_us = time_us;
|
|
||||||
|
|
||||||
_ext_vision_buffer->push(ev_sample_new);
|
|
||||||
_time_last_ext_vision_buffer_push = _time_latest_us;
|
|
||||||
|
|
||||||
} else {
|
|
||||||
ECL_WARN("EV data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _ext_vision_buffer->get_newest().time_us, _min_obs_interval_us);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_AUXVEL)
|
|
||||||
void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)
|
|
||||||
{
|
|
||||||
if (!_initialised) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Allocate the required buffer size if not previously done
|
|
||||||
if (_auxvel_buffer == nullptr) {
|
|
||||||
_auxvel_buffer = new RingBuffer<auxVelSample>(_obs_buffer_length);
|
|
||||||
|
|
||||||
if (_auxvel_buffer == nullptr || !_auxvel_buffer->valid()) {
|
|
||||||
delete _auxvel_buffer;
|
|
||||||
_auxvel_buffer = nullptr;
|
|
||||||
printBufferAllocationFailed("aux vel");
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
const int64_t time_us = auxvel_sample.time_us
|
|
||||||
- static_cast<int64_t>(_params.auxvel_delay_ms * 1000)
|
|
||||||
- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
|
|
||||||
|
|
||||||
// limit data rate to prevent data being lost
|
|
||||||
if (time_us >= static_cast<int64_t>(_auxvel_buffer->get_newest().time_us + _min_obs_interval_us)) {
|
|
||||||
|
|
||||||
auxVelSample auxvel_sample_new{auxvel_sample};
|
|
||||||
auxvel_sample_new.time_us = time_us;
|
|
||||||
|
|
||||||
_auxvel_buffer->push(auxvel_sample_new);
|
|
||||||
|
|
||||||
} else {
|
|
||||||
ECL_WARN("aux velocity data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _auxvel_buffer->get_newest().time_us, _min_obs_interval_us);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif // CONFIG_EKF2_AUXVEL
|
|
||||||
|
|
||||||
void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags)
|
|
||||||
{
|
{
|
||||||
if (!_initialised) {
|
if (!_initialised) {
|
||||||
return;
|
return;
|
||||||
|
@ -479,75 +138,12 @@ void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags)
|
||||||
_system_flag_buffer->push(system_flags_new);
|
_system_flag_buffer->push(system_flags_new);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
ECL_DEBUG("system flag update too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _system_flag_buffer->get_newest().time_us, _min_obs_interval_us);
|
ECL_DEBUG("system flag update too fast %" PRIi64 " < %" PRIu64 " + %d",
|
||||||
|
time_us, _system_flag_buffer->get_newest().time_us, _min_obs_interval_us);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
bool Ekf::initialise_interface(uint64_t timestamp)
|
||||||
void EstimatorInterface::setDragData(const imuSample &imu)
|
|
||||||
{
|
|
||||||
// down-sample the drag specific force data by accumulating and calculating the mean when
|
|
||||||
// sufficient samples have been collected
|
|
||||||
if (_params.drag_ctrl > 0) {
|
|
||||||
|
|
||||||
// Allocate the required buffer size if not previously done
|
|
||||||
if (_drag_buffer == nullptr) {
|
|
||||||
_drag_buffer = new RingBuffer<dragSample>(_obs_buffer_length);
|
|
||||||
|
|
||||||
if (_drag_buffer == nullptr || !_drag_buffer->valid()) {
|
|
||||||
delete _drag_buffer;
|
|
||||||
_drag_buffer = nullptr;
|
|
||||||
printBufferAllocationFailed("drag");
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// don't use any accel samples that are clipping
|
|
||||||
if (imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2]) {
|
|
||||||
// reset accumulators
|
|
||||||
_drag_sample_count = 0;
|
|
||||||
_drag_down_sampled.accelXY.zero();
|
|
||||||
_drag_down_sampled.time_us = 0;
|
|
||||||
_drag_sample_time_dt = 0.0f;
|
|
||||||
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
_drag_sample_count++;
|
|
||||||
// note acceleration is accumulated as a delta velocity
|
|
||||||
_drag_down_sampled.accelXY(0) += imu.delta_vel(0);
|
|
||||||
_drag_down_sampled.accelXY(1) += imu.delta_vel(1);
|
|
||||||
_drag_down_sampled.time_us += imu.time_us;
|
|
||||||
_drag_sample_time_dt += imu.delta_vel_dt;
|
|
||||||
|
|
||||||
// calculate the downsample ratio for drag specific force data
|
|
||||||
uint8_t min_sample_ratio = (uint8_t) ceilf((float)_imu_buffer_length / _obs_buffer_length);
|
|
||||||
|
|
||||||
if (min_sample_ratio < 5) {
|
|
||||||
min_sample_ratio = 5;
|
|
||||||
}
|
|
||||||
|
|
||||||
// calculate and store means from accumulated values
|
|
||||||
if (_drag_sample_count >= min_sample_ratio) {
|
|
||||||
// note conversion from accumulated delta velocity to acceleration
|
|
||||||
_drag_down_sampled.accelXY(0) /= _drag_sample_time_dt;
|
|
||||||
_drag_down_sampled.accelXY(1) /= _drag_sample_time_dt;
|
|
||||||
_drag_down_sampled.time_us /= _drag_sample_count;
|
|
||||||
|
|
||||||
// write to buffer
|
|
||||||
_drag_buffer->push(_drag_down_sampled);
|
|
||||||
|
|
||||||
// reset accumulators
|
|
||||||
_drag_sample_count = 0;
|
|
||||||
_drag_down_sampled.accelXY.zero();
|
|
||||||
_drag_down_sampled.time_us = 0;
|
|
||||||
_drag_sample_time_dt = 0.0f;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif // CONFIG_EKF2_DRAG_FUSION
|
|
||||||
|
|
||||||
bool EstimatorInterface::initialise_interface(uint64_t timestamp)
|
|
||||||
{
|
{
|
||||||
// find the maximum time delay the buffers are required to handle
|
// find the maximum time delay the buffers are required to handle
|
||||||
|
|
||||||
|
@ -565,10 +161,12 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
|
||||||
}
|
}
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||||
|
|
||||||
// using airspeed
|
// using airspeed
|
||||||
if (_params.arsp_thr > FLT_EPSILON) {
|
if (_params.arsp_thr > FLT_EPSILON) {
|
||||||
max_time_delay_ms = math::max(_params.airspeed_delay_ms, max_time_delay_ms);
|
max_time_delay_ms = math::max(_params.airspeed_delay_ms, max_time_delay_ms);
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // CONFIG_EKF2_AIRSPEED
|
#endif // CONFIG_EKF2_AIRSPEED
|
||||||
|
|
||||||
// mag mode
|
// mag mode
|
||||||
|
@ -577,10 +175,12 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
|
||||||
}
|
}
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||||
|
|
||||||
// using range finder
|
// using range finder
|
||||||
if ((_params.rng_ctrl != RngCtrl::DISABLED)) {
|
if ((_params.rng_ctrl != RngCtrl::DISABLED)) {
|
||||||
max_time_delay_ms = math::max(_params.range_delay_ms, max_time_delay_ms);
|
max_time_delay_ms = math::max(_params.range_delay_ms, max_time_delay_ms);
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||||
|
|
||||||
if (_params.gnss_ctrl > 0) {
|
if (_params.gnss_ctrl > 0) {
|
||||||
|
@ -588,15 +188,19 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
|
||||||
}
|
}
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||||
|
|
||||||
if (_params.flow_ctrl > 0) {
|
if (_params.flow_ctrl > 0) {
|
||||||
max_time_delay_ms = math::max(_params.flow_delay_ms, max_time_delay_ms);
|
max_time_delay_ms = math::max(_params.flow_delay_ms, max_time_delay_ms);
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||||
|
|
||||||
if (_params.ev_ctrl > 0) {
|
if (_params.ev_ctrl > 0) {
|
||||||
max_time_delay_ms = math::max(_params.ev_delay_ms, max_time_delay_ms);
|
max_time_delay_ms = math::max(_params.ev_delay_ms, max_time_delay_ms);
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||||
|
|
||||||
const float filter_update_period_ms = _params.filter_update_interval_us / 1000.f;
|
const float filter_update_period_ms = _params.filter_update_interval_us / 1000.f;
|
||||||
|
@ -629,18 +233,13 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool EstimatorInterface::isOnlyActiveSourceOfHorizontalAiding(const bool aiding_flag) const
|
bool Ekf::isOtherSourceOfHorizontalAidingThan(const bool aiding_flag) const
|
||||||
{
|
|
||||||
return aiding_flag && !isOtherSourceOfHorizontalAidingThan(aiding_flag);
|
|
||||||
}
|
|
||||||
|
|
||||||
bool EstimatorInterface::isOtherSourceOfHorizontalAidingThan(const bool aiding_flag) const
|
|
||||||
{
|
{
|
||||||
const int nb_sources = getNumberOfActiveHorizontalAidingSources();
|
const int nb_sources = getNumberOfActiveHorizontalAidingSources();
|
||||||
return aiding_flag ? nb_sources > 1 : nb_sources > 0;
|
return aiding_flag ? nb_sources > 1 : nb_sources > 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
int EstimatorInterface::getNumberOfActiveHorizontalAidingSources() const
|
int Ekf::getNumberOfActiveHorizontalAidingSources() const
|
||||||
{
|
{
|
||||||
return int(_control_status.flags.gps)
|
return int(_control_status.flags.gps)
|
||||||
+ int(_control_status.flags.opt_flow)
|
+ int(_control_status.flags.opt_flow)
|
||||||
|
@ -651,28 +250,13 @@ int EstimatorInterface::getNumberOfActiveHorizontalAidingSources() const
|
||||||
+ int(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta);
|
+ int(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool EstimatorInterface::isHorizontalAidingActive() const
|
bool Ekf::isOtherSourceOfVerticalPositionAidingThan(const bool aiding_flag) const
|
||||||
{
|
|
||||||
return getNumberOfActiveHorizontalAidingSources() > 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool EstimatorInterface::isOtherSourceOfVerticalPositionAidingThan(const bool aiding_flag) const
|
|
||||||
{
|
{
|
||||||
const int nb_sources = getNumberOfActiveVerticalPositionAidingSources();
|
const int nb_sources = getNumberOfActiveVerticalPositionAidingSources();
|
||||||
return aiding_flag ? nb_sources > 1 : nb_sources > 0;
|
return aiding_flag ? nb_sources > 1 : nb_sources > 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool EstimatorInterface::isVerticalPositionAidingActive() const
|
int Ekf::getNumberOfActiveVerticalPositionAidingSources() const
|
||||||
{
|
|
||||||
return getNumberOfActiveVerticalPositionAidingSources() > 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool EstimatorInterface::isOnlyActiveSourceOfVerticalPositionAiding(const bool aiding_flag) const
|
|
||||||
{
|
|
||||||
return aiding_flag && !isOtherSourceOfVerticalPositionAidingThan(aiding_flag);
|
|
||||||
}
|
|
||||||
|
|
||||||
int EstimatorInterface::getNumberOfActiveVerticalPositionAidingSources() const
|
|
||||||
{
|
{
|
||||||
return int(_control_status.flags.gps_hgt)
|
return int(_control_status.flags.gps_hgt)
|
||||||
+ int(_control_status.flags.baro_hgt)
|
+ int(_control_status.flags.baro_hgt)
|
||||||
|
@ -680,30 +264,14 @@ int EstimatorInterface::getNumberOfActiveVerticalPositionAidingSources() const
|
||||||
+ int(_control_status.flags.ev_hgt);
|
+ int(_control_status.flags.ev_hgt);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool EstimatorInterface::isVerticalAidingActive() const
|
void Ekf::printBufferAllocationFailed(const char *buffer_name)
|
||||||
{
|
|
||||||
return isVerticalPositionAidingActive() || isVerticalVelocityAidingActive();
|
|
||||||
}
|
|
||||||
|
|
||||||
bool EstimatorInterface::isVerticalVelocityAidingActive() const
|
|
||||||
{
|
|
||||||
return getNumberOfActiveVerticalVelocityAidingSources() > 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
int EstimatorInterface::getNumberOfActiveVerticalVelocityAidingSources() const
|
|
||||||
{
|
|
||||||
return int(_control_status.flags.gps)
|
|
||||||
+ int(_control_status.flags.ev_vel);
|
|
||||||
}
|
|
||||||
|
|
||||||
void EstimatorInterface::printBufferAllocationFailed(const char *buffer_name)
|
|
||||||
{
|
{
|
||||||
if (buffer_name) {
|
if (buffer_name) {
|
||||||
ECL_ERR("%s buffer allocation failed", buffer_name);
|
ECL_ERR("%s buffer allocation failed", buffer_name);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void EstimatorInterface::print_status()
|
void Ekf::print_status()
|
||||||
{
|
{
|
||||||
printf("EKF average dt: %.6f seconds\n", (double)_dt_ekf_avg);
|
printf("EKF average dt: %.6f seconds\n", (double)_dt_ekf_avg);
|
||||||
|
|
||||||
|
@ -712,49 +280,71 @@ void EstimatorInterface::print_status()
|
||||||
printf("minimum observation interval %d us\n", _min_obs_interval_us);
|
printf("minimum observation interval %d us\n", _min_obs_interval_us);
|
||||||
|
|
||||||
if (_gps_buffer) {
|
if (_gps_buffer) {
|
||||||
printf("gps buffer: %d/%d (%d Bytes)\n", _gps_buffer->entries(), _gps_buffer->get_length(), _gps_buffer->get_total_size());
|
printf("gps buffer: %d/%d (%d Bytes)\n",
|
||||||
|
_gps_buffer->entries(), _gps_buffer->get_length(), _gps_buffer->get_total_size());
|
||||||
}
|
}
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||||
|
|
||||||
if (_mag_buffer) {
|
if (_mag_buffer) {
|
||||||
printf("mag buffer: %d/%d (%d Bytes)\n", _mag_buffer->entries(), _mag_buffer->get_length(), _mag_buffer->get_total_size());
|
printf("mag buffer: %d/%d (%d Bytes)\n",
|
||||||
|
_mag_buffer->entries(), _mag_buffer->get_length(), _mag_buffer->get_total_size());
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_BAROMETER)
|
#if defined(CONFIG_EKF2_BAROMETER)
|
||||||
|
|
||||||
if (_baro_buffer) {
|
if (_baro_buffer) {
|
||||||
printf("baro buffer: %d/%d (%d Bytes)\n", _baro_buffer->entries(), _baro_buffer->get_length(), _baro_buffer->get_total_size());
|
printf("baro buffer: %d/%d (%d Bytes)\n",
|
||||||
|
_baro_buffer->entries(), _baro_buffer->get_length(), _baro_buffer->get_total_size());
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // CONFIG_EKF2_BAROMETER
|
#endif // CONFIG_EKF2_BAROMETER
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||||
|
|
||||||
if (_range_buffer) {
|
if (_range_buffer) {
|
||||||
printf("range buffer: %d/%d (%d Bytes)\n", _range_buffer->entries(), _range_buffer->get_length(), _range_buffer->get_total_size());
|
printf("range buffer: %d/%d (%d Bytes)\n", _range_buffer->entries(), _range_buffer->get_length(),
|
||||||
|
_range_buffer->get_total_size());
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||||
|
|
||||||
if (_airspeed_buffer) {
|
if (_airspeed_buffer) {
|
||||||
printf("airspeed buffer: %d/%d (%d Bytes)\n", _airspeed_buffer->entries(), _airspeed_buffer->get_length(), _airspeed_buffer->get_total_size());
|
printf("airspeed buffer: %d/%d (%d Bytes)\n",
|
||||||
|
_airspeed_buffer->entries(), _airspeed_buffer->get_length(), _airspeed_buffer->get_total_size());
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // CONFIG_EKF2_AIRSPEED
|
#endif // CONFIG_EKF2_AIRSPEED
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||||
|
|
||||||
if (_flow_buffer) {
|
if (_flow_buffer) {
|
||||||
printf("flow buffer: %d/%d (%d Bytes)\n", _flow_buffer->entries(), _flow_buffer->get_length(), _flow_buffer->get_total_size());
|
printf("flow buffer: %d/%d (%d Bytes)\n",
|
||||||
|
_flow_buffer->entries(), _flow_buffer->get_length(), _flow_buffer->get_total_size());
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||||
|
|
||||||
if (_ext_vision_buffer) {
|
if (_ext_vision_buffer) {
|
||||||
printf("vision buffer: %d/%d (%d Bytes)\n", _ext_vision_buffer->entries(), _ext_vision_buffer->get_length(), _ext_vision_buffer->get_total_size());
|
printf("vision buffer: %d/%d (%d Bytes)\n",
|
||||||
|
_ext_vision_buffer->entries(), _ext_vision_buffer->get_length(), _ext_vision_buffer->get_total_size());
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
||||||
|
|
||||||
if (_drag_buffer) {
|
if (_drag_buffer) {
|
||||||
printf("drag buffer: %d/%d (%d Bytes)\n", _drag_buffer->entries(), _drag_buffer->get_length(), _drag_buffer->get_total_size());
|
printf("drag buffer: %d/%d (%d Bytes)\n",
|
||||||
|
_drag_buffer->entries(), _drag_buffer->get_length(), _drag_buffer->get_total_size());
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // CONFIG_EKF2_DRAG_FUSION
|
#endif // CONFIG_EKF2_DRAG_FUSION
|
||||||
|
|
||||||
_output_predictor.print_status();
|
_output_predictor.print_status();
|
||||||
|
|
|
@ -1,490 +0,0 @@
|
||||||
/****************************************************************************
|
|
||||||
*
|
|
||||||
* Copyright (c) 2015-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.
|
|
||||||
*
|
|
||||||
****************************************************************************/
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @file estimator_interface.h
|
|
||||||
* Definition of base class for attitude estimators
|
|
||||||
*
|
|
||||||
* @author Roman Bast <bapstroman@gmail.com>
|
|
||||||
*
|
|
||||||
*/
|
|
||||||
|
|
||||||
#ifndef EKF_ESTIMATOR_INTERFACE_H
|
|
||||||
#define EKF_ESTIMATOR_INTERFACE_H
|
|
||||||
|
|
||||||
#if defined(MODULE_NAME)
|
|
||||||
#include <px4_platform_common/log.h>
|
|
||||||
# define ECL_INFO PX4_DEBUG
|
|
||||||
# define ECL_WARN PX4_DEBUG
|
|
||||||
# define ECL_ERR PX4_DEBUG
|
|
||||||
# define ECL_DEBUG PX4_DEBUG
|
|
||||||
#else
|
|
||||||
# define ECL_INFO(X, ...) printf(X "\n", ##__VA_ARGS__)
|
|
||||||
# define ECL_WARN(X, ...) fprintf(stderr, X "\n", ##__VA_ARGS__)
|
|
||||||
# define ECL_ERR(X, ...) fprintf(stderr, X "\n", ##__VA_ARGS__)
|
|
||||||
|
|
||||||
# if defined(DEBUG_BUILD)
|
|
||||||
# define ECL_DEBUG(X, ...) fprintf(stderr, X "\n", ##__VA_ARGS__)
|
|
||||||
# else
|
|
||||||
# define ECL_DEBUG(X, ...)
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#include "common.h"
|
|
||||||
#include "RingBuffer.h"
|
|
||||||
#include "imu_down_sampler.hpp"
|
|
||||||
#include "output_predictor.h"
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
|
||||||
# include "range_finder_consistency_check.hpp"
|
|
||||||
# include "sensor_range_finder.hpp"
|
|
||||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
|
||||||
|
|
||||||
#include <lib/geo/geo.h>
|
|
||||||
#include <matrix/math.hpp>
|
|
||||||
#include <mathlib/mathlib.h>
|
|
||||||
#include <mathlib/math/filter/AlphaFilter.hpp>
|
|
||||||
|
|
||||||
using namespace estimator;
|
|
||||||
|
|
||||||
class EstimatorInterface
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
|
|
||||||
virtual bool collect_gps(const gpsMessage &gps) = 0;
|
|
||||||
|
|
||||||
void setIMUData(const imuSample &imu_sample);
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
|
||||||
void setMagData(const magSample &mag_sample);
|
|
||||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
|
||||||
|
|
||||||
void setGpsData(const gpsMessage &gps);
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_BAROMETER)
|
|
||||||
void setBaroData(const baroSample &baro_sample);
|
|
||||||
#endif // CONFIG_EKF2_BAROMETER
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
|
||||||
void setAirspeedData(const airspeedSample &airspeed_sample);
|
|
||||||
#endif // CONFIG_EKF2_AIRSPEED
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
|
||||||
void setRangeData(const rangeSample &range_sample);
|
|
||||||
|
|
||||||
// set sensor limitations reported by the rangefinder
|
|
||||||
void set_rangefinder_limits(float min_distance, float max_distance)
|
|
||||||
{
|
|
||||||
_range_sensor.setLimits(min_distance, max_distance);
|
|
||||||
}
|
|
||||||
|
|
||||||
const rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); }
|
|
||||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
|
||||||
// if optical flow sensor gyro delta angles are not available, set gyro_xyz vector fields to NaN and the EKF will use its internal delta angle data instead
|
|
||||||
void setOpticalFlowData(const flowSample &flow);
|
|
||||||
|
|
||||||
// set sensor limitations reported by the optical flow sensor
|
|
||||||
void set_optical_flow_limits(float max_flow_rate, float min_distance, float max_distance)
|
|
||||||
{
|
|
||||||
_flow_max_rate = max_flow_rate;
|
|
||||||
_flow_min_distance = min_distance;
|
|
||||||
_flow_max_distance = max_distance;
|
|
||||||
}
|
|
||||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
|
||||||
// set external vision position and attitude data
|
|
||||||
void setExtVisionData(const extVisionSample &evdata);
|
|
||||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_AUXVEL)
|
|
||||||
void setAuxVelData(const auxVelSample &auxvel_sample);
|
|
||||||
#endif // CONFIG_EKF2_AUXVEL
|
|
||||||
|
|
||||||
void setSystemFlagData(const systemFlagUpdate &system_flags);
|
|
||||||
|
|
||||||
// return a address to the parameters struct
|
|
||||||
// in order to give access to the application
|
|
||||||
parameters *getParamHandle() { return &_params; }
|
|
||||||
|
|
||||||
// set vehicle landed status data
|
|
||||||
void set_in_air_status(bool in_air)
|
|
||||||
{
|
|
||||||
if (!in_air) {
|
|
||||||
if (_control_status.flags.in_air) {
|
|
||||||
ECL_DEBUG("no longer in air");
|
|
||||||
}
|
|
||||||
|
|
||||||
_time_last_on_ground_us = _time_delayed_us;
|
|
||||||
|
|
||||||
} else {
|
|
||||||
if (!_control_status.flags.in_air) {
|
|
||||||
ECL_DEBUG("in air");
|
|
||||||
}
|
|
||||||
|
|
||||||
_time_last_in_air = _time_delayed_us;
|
|
||||||
}
|
|
||||||
|
|
||||||
_control_status.flags.in_air = in_air;
|
|
||||||
}
|
|
||||||
|
|
||||||
void set_vehicle_at_rest(bool at_rest)
|
|
||||||
{
|
|
||||||
if (!_control_status.flags.vehicle_at_rest && at_rest) {
|
|
||||||
ECL_DEBUG("at rest");
|
|
||||||
|
|
||||||
} else if (_control_status.flags.vehicle_at_rest && !at_rest) {
|
|
||||||
ECL_DEBUG("no longer at rest");
|
|
||||||
}
|
|
||||||
|
|
||||||
_control_status.flags.vehicle_at_rest = at_rest;
|
|
||||||
}
|
|
||||||
|
|
||||||
// return true if the attitude is usable
|
|
||||||
bool attitude_valid() const { return _control_status.flags.tilt_align; }
|
|
||||||
|
|
||||||
// get vehicle landed status data
|
|
||||||
bool get_in_air_status() const { return _control_status.flags.in_air; }
|
|
||||||
|
|
||||||
// get wind estimation status
|
|
||||||
bool get_wind_status() const { return _control_status.flags.wind; }
|
|
||||||
|
|
||||||
// set vehicle is fixed wing status
|
|
||||||
void set_is_fixed_wing(bool is_fixed_wing) { _control_status.flags.fixed_wing = is_fixed_wing; }
|
|
||||||
|
|
||||||
// set flag if static pressure rise due to ground effect is expected
|
|
||||||
// use _params.gnd_effect_deadzone to adjust for expected rise in static pressure
|
|
||||||
// flag will clear after GNDEFFECT_TIMEOUT uSec
|
|
||||||
void set_gnd_effect()
|
|
||||||
{
|
|
||||||
_control_status.flags.gnd_effect = true;
|
|
||||||
_time_last_gnd_effect_on = _time_delayed_us;
|
|
||||||
}
|
|
||||||
|
|
||||||
// set air density used by the multi-rotor specific drag force fusion
|
|
||||||
void set_air_density(float air_density) { _air_density = air_density; }
|
|
||||||
|
|
||||||
// the flags considered are opt_flow, gps, ev_vel and ev_pos
|
|
||||||
bool isOnlyActiveSourceOfHorizontalAiding(bool aiding_flag) const;
|
|
||||||
|
|
||||||
/*
|
|
||||||
* Check if there are any other active source of horizontal aiding
|
|
||||||
* Warning: does not tell if the selected source is
|
|
||||||
* active, use isOnlyActiveSourceOfHorizontalAiding() for this
|
|
||||||
*
|
|
||||||
* The flags considered are opt_flow, gps, ev_vel and ev_pos
|
|
||||||
*
|
|
||||||
* @param aiding_flag a flag in _control_status.flags
|
|
||||||
* @return true if an other source than aiding_flag is active
|
|
||||||
*/
|
|
||||||
bool isOtherSourceOfHorizontalAidingThan(bool aiding_flag) const;
|
|
||||||
|
|
||||||
// Return true if at least one source of horizontal aiding is active
|
|
||||||
// the flags considered are opt_flow, gps, ev_vel and ev_pos
|
|
||||||
bool isHorizontalAidingActive() const;
|
|
||||||
bool isVerticalAidingActive() const;
|
|
||||||
|
|
||||||
int getNumberOfActiveHorizontalAidingSources() const;
|
|
||||||
|
|
||||||
bool isOtherSourceOfVerticalPositionAidingThan(bool aiding_flag) const;
|
|
||||||
bool isVerticalPositionAidingActive() const;
|
|
||||||
bool isOnlyActiveSourceOfVerticalPositionAiding(const bool aiding_flag) const;
|
|
||||||
int getNumberOfActiveVerticalPositionAidingSources() const;
|
|
||||||
|
|
||||||
bool isVerticalVelocityAidingActive() const;
|
|
||||||
int getNumberOfActiveVerticalVelocityAidingSources() const;
|
|
||||||
|
|
||||||
const matrix::Quatf &getQuaternion() const { return _output_predictor.getQuaternion(); }
|
|
||||||
float getUnaidedYaw() const { return _output_predictor.getUnaidedYaw(); }
|
|
||||||
Vector3f getVelocity() const { return _output_predictor.getVelocity(); }
|
|
||||||
const Vector3f &getVelocityDerivative() const { return _output_predictor.getVelocityDerivative(); }
|
|
||||||
float getVerticalPositionDerivative() const { return _output_predictor.getVerticalPositionDerivative(); }
|
|
||||||
Vector3f getPosition() const { return _output_predictor.getPosition(); }
|
|
||||||
const Vector3f &getOutputTrackingError() const { return _output_predictor.getOutputTrackingError(); }
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
|
||||||
// Get the value of magnetic declination in degrees to be saved for use at the next startup
|
|
||||||
// Returns true when the declination can be saved
|
|
||||||
// At the next startup, set param.mag_declination_deg to the value saved
|
|
||||||
bool get_mag_decl_deg(float &val) const
|
|
||||||
{
|
|
||||||
if (_NED_origin_initialised && (_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)) {
|
|
||||||
val = math::degrees(_mag_declination_gps);
|
|
||||||
return true;
|
|
||||||
|
|
||||||
} else {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
bool get_mag_inc_deg(float &val) const
|
|
||||||
{
|
|
||||||
if (_NED_origin_initialised) {
|
|
||||||
val = math::degrees(_mag_inclination_gps);
|
|
||||||
return true;
|
|
||||||
|
|
||||||
} else {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void get_mag_checks(float &inc_deg, float &inc_ref_deg, float &strength_gs, float &strength_ref_gs) const
|
|
||||||
{
|
|
||||||
inc_deg = math::degrees(_mag_inclination);
|
|
||||||
inc_ref_deg = math::degrees(_mag_inclination_gps);
|
|
||||||
strength_gs = _mag_strength;
|
|
||||||
strength_ref_gs = _mag_strength_gps;
|
|
||||||
}
|
|
||||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
|
||||||
|
|
||||||
// get EKF mode status
|
|
||||||
const filter_control_status_u &control_status() const { return _control_status; }
|
|
||||||
const decltype(filter_control_status_u::flags) &control_status_flags() const { return _control_status.flags; }
|
|
||||||
|
|
||||||
const filter_control_status_u &control_status_prev() const { return _control_status_prev; }
|
|
||||||
const decltype(filter_control_status_u::flags) &control_status_prev_flags() const { return _control_status_prev.flags; }
|
|
||||||
|
|
||||||
// get EKF internal fault status
|
|
||||||
const fault_status_u &fault_status() const { return _fault_status; }
|
|
||||||
const decltype(fault_status_u::flags) &fault_status_flags() const { return _fault_status.flags; }
|
|
||||||
|
|
||||||
const innovation_fault_status_u &innov_check_fail_status() const { return _innov_check_fail_status; }
|
|
||||||
const decltype(innovation_fault_status_u::flags) &innov_check_fail_status_flags() const { return _innov_check_fail_status.flags; }
|
|
||||||
|
|
||||||
const warning_event_status_u &warning_event_status() const { return _warning_events; }
|
|
||||||
const decltype(warning_event_status_u::flags) &warning_event_flags() const { return _warning_events.flags; }
|
|
||||||
void clear_warning_events() { _warning_events.value = 0; }
|
|
||||||
|
|
||||||
const information_event_status_u &information_event_status() const { return _information_events; }
|
|
||||||
const decltype(information_event_status_u::flags) &information_event_flags() const { return _information_events.flags; }
|
|
||||||
void clear_information_events() { _information_events.value = 0; }
|
|
||||||
|
|
||||||
// Getter for the average EKF update period in s
|
|
||||||
float get_dt_ekf_avg() const { return _dt_ekf_avg; }
|
|
||||||
|
|
||||||
// Getters for samples on the delayed time horizon
|
|
||||||
const imuSample &get_imu_sample_delayed() const { return _imu_buffer.get_oldest(); }
|
|
||||||
const uint64_t &time_delayed_us() const { return _time_delayed_us; }
|
|
||||||
|
|
||||||
const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; }
|
|
||||||
|
|
||||||
const bool &global_origin_valid() const { return _NED_origin_initialised; }
|
|
||||||
const MapProjection &global_origin() const { return _pos_ref; }
|
|
||||||
|
|
||||||
void print_status();
|
|
||||||
|
|
||||||
float gps_horizontal_position_drift_rate_m_s() const { return _gps_horizontal_position_drift_rate_m_s; }
|
|
||||||
float gps_vertical_position_drift_rate_m_s() const { return _gps_vertical_position_drift_rate_m_s; }
|
|
||||||
float gps_filtered_horizontal_velocity_m_s() const { return _gps_filtered_horizontal_velocity_m_s; }
|
|
||||||
|
|
||||||
OutputPredictor &output_predictor() { return _output_predictor; };
|
|
||||||
|
|
||||||
protected:
|
|
||||||
|
|
||||||
EstimatorInterface() = default;
|
|
||||||
virtual ~EstimatorInterface();
|
|
||||||
|
|
||||||
virtual bool init(uint64_t timestamp) = 0;
|
|
||||||
|
|
||||||
parameters _params{}; // filter parameters
|
|
||||||
|
|
||||||
/*
|
|
||||||
OBS_BUFFER_LENGTH defines how many observations (non-IMU measurements) we can buffer
|
|
||||||
which sets the maximum frequency at which we can process non-IMU measurements. Measurements that
|
|
||||||
arrive too soon after the previous measurement will not be processed.
|
|
||||||
max freq (Hz) = (OBS_BUFFER_LENGTH - 1) / (IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_S)
|
|
||||||
This can be adjusted to match the max sensor data rate plus some margin for jitter.
|
|
||||||
*/
|
|
||||||
uint8_t _obs_buffer_length{0};
|
|
||||||
|
|
||||||
/*
|
|
||||||
IMU_BUFFER_LENGTH defines how many IMU samples we buffer which sets the time delay from current time to the
|
|
||||||
EKF fusion time horizon and therefore the maximum sensor time offset relative to the IMU that we can compensate for.
|
|
||||||
max sensor time offet (msec) = IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_MS
|
|
||||||
This can be adjusted to a value that is FILTER_UPDATE_PERIOD_MS longer than the maximum observation time delay.
|
|
||||||
*/
|
|
||||||
uint8_t _imu_buffer_length{0};
|
|
||||||
|
|
||||||
float _dt_ekf_avg{0.010f}; ///< average update rate of the ekf in s
|
|
||||||
|
|
||||||
uint64_t _time_delayed_us{0}; // captures the imu sample on the delayed time horizon
|
|
||||||
uint64_t _time_latest_us{0}; // imu sample capturing the newest imu data
|
|
||||||
|
|
||||||
OutputPredictor _output_predictor{};
|
|
||||||
|
|
||||||
// measurement samples capturing measurements on the delayed time horizon
|
|
||||||
gpsSample _gps_sample_delayed{};
|
|
||||||
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
|
||||||
airspeedSample _airspeed_sample_delayed{};
|
|
||||||
#endif // CONFIG_EKF2_AIRSPEED
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
|
||||||
extVisionSample _ev_sample_prev{};
|
|
||||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
|
||||||
RingBuffer<rangeSample> *_range_buffer{nullptr};
|
|
||||||
uint64_t _time_last_range_buffer_push{0};
|
|
||||||
|
|
||||||
sensor::SensorRangeFinder _range_sensor{};
|
|
||||||
RangeFinderConsistencyCheck _rng_consistency_check;
|
|
||||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
|
||||||
RingBuffer<flowSample> *_flow_buffer{nullptr};
|
|
||||||
|
|
||||||
flowSample _flow_sample_delayed{};
|
|
||||||
|
|
||||||
// Sensor limitations
|
|
||||||
float _flow_max_rate{1.0f}; ///< maximum angular flow rate that the optical flow sensor can measure (rad/s)
|
|
||||||
float _flow_min_distance{0.0f}; ///< minimum distance that the optical flow sensor can operate at (m)
|
|
||||||
float _flow_max_distance{10.f}; ///< maximum distance that the optical flow sensor can operate at (m)
|
|
||||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
|
||||||
|
|
||||||
float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // air density (kg/m**3)
|
|
||||||
|
|
||||||
bool _imu_updated{false}; // true if the ekf should update (completed downsampling process)
|
|
||||||
bool _initialised{false}; // true if the ekf interface instance (data buffering) is initialized
|
|
||||||
|
|
||||||
bool _NED_origin_initialised{false};
|
|
||||||
float _gpos_origin_eph{0.0f}; // horizontal position uncertainty of the global origin
|
|
||||||
float _gpos_origin_epv{0.0f}; // vertical position uncertainty of the global origin
|
|
||||||
MapProjection _pos_ref{}; // Contains WGS-84 position latitude and longitude of the EKF origin
|
|
||||||
MapProjection _gps_pos_prev{}; // Contains WGS-84 position latitude and longitude of the previous GPS message
|
|
||||||
float _gps_alt_prev{0.0f}; // height from the previous GPS message (m)
|
|
||||||
#if defined(CONFIG_EKF2_GNSS_YAW)
|
|
||||||
float _gps_yaw_offset{0.0f}; // Yaw offset angle for dual GPS antennas used for yaw estimation (radians).
|
|
||||||
// innovation consistency check monitoring ratios
|
|
||||||
AlphaFilter<float> _gnss_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state
|
|
||||||
uint64_t _time_last_gps_yaw_buffer_push{0};
|
|
||||||
#endif // CONFIG_EKF2_GNSS_YAW
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
|
||||||
RingBuffer<dragSample> *_drag_buffer{nullptr};
|
|
||||||
dragSample _drag_down_sampled{}; // down sampled drag specific force data (filter prediction rate -> observation rate)
|
|
||||||
Vector2f _drag_test_ratio{}; // drag innovation consistency check ratio
|
|
||||||
#endif // CONFIG_EKF2_DRAG_FUSION
|
|
||||||
|
|
||||||
innovation_fault_status_u _innov_check_fail_status{};
|
|
||||||
|
|
||||||
bool _horizontal_deadreckon_time_exceeded{true};
|
|
||||||
bool _vertical_position_deadreckon_time_exceeded{true};
|
|
||||||
bool _vertical_velocity_deadreckon_time_exceeded{true};
|
|
||||||
|
|
||||||
float _gps_horizontal_position_drift_rate_m_s{NAN}; // Horizontal position drift rate (m/s)
|
|
||||||
float _gps_vertical_position_drift_rate_m_s{NAN}; // Vertical position drift rate (m/s)
|
|
||||||
float _gps_filtered_horizontal_velocity_m_s{NAN}; // Filtered horizontal velocity (m/s)
|
|
||||||
|
|
||||||
uint64_t _time_last_on_ground_us{0}; ///< last time we were on the ground (uSec)
|
|
||||||
uint64_t _time_last_in_air{0}; ///< last time we were in air (uSec)
|
|
||||||
|
|
||||||
// data buffer instances
|
|
||||||
static constexpr uint8_t kBufferLengthDefault = 12;
|
|
||||||
RingBuffer<imuSample> _imu_buffer{kBufferLengthDefault};
|
|
||||||
|
|
||||||
RingBuffer<gpsSample> *_gps_buffer{nullptr};
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
|
||||||
RingBuffer<magSample> *_mag_buffer{nullptr};
|
|
||||||
uint64_t _time_last_mag_buffer_push{0};
|
|
||||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
|
||||||
RingBuffer<airspeedSample> *_airspeed_buffer{nullptr};
|
|
||||||
#endif // CONFIG_EKF2_AIRSPEED
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
|
||||||
RingBuffer<extVisionSample> *_ext_vision_buffer{nullptr};
|
|
||||||
uint64_t _time_last_ext_vision_buffer_push{0};
|
|
||||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
|
||||||
#if defined(CONFIG_EKF2_AUXVEL)
|
|
||||||
RingBuffer<auxVelSample> *_auxvel_buffer{nullptr};
|
|
||||||
#endif // CONFIG_EKF2_AUXVEL
|
|
||||||
RingBuffer<systemFlagUpdate> *_system_flag_buffer{nullptr};
|
|
||||||
|
|
||||||
uint64_t _time_last_gps_buffer_push{0};
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_BAROMETER)
|
|
||||||
RingBuffer<baroSample> *_baro_buffer{nullptr};
|
|
||||||
uint64_t _time_last_baro_buffer_push{0};
|
|
||||||
#endif // CONFIG_EKF2_BAROMETER
|
|
||||||
|
|
||||||
uint64_t _time_last_gnd_effect_on{0};
|
|
||||||
|
|
||||||
fault_status_u _fault_status{};
|
|
||||||
|
|
||||||
// allocate data buffers and initialize interface variables
|
|
||||||
bool initialise_interface(uint64_t timestamp);
|
|
||||||
|
|
||||||
uint64_t _wmm_gps_time_last_checked{0}; // time WMM last checked
|
|
||||||
uint64_t _wmm_gps_time_last_set{0}; // time WMM last set
|
|
||||||
float _mag_declination_gps{NAN}; // magnetic declination returned by the geo library using the last valid GPS position (rad)
|
|
||||||
float _mag_inclination_gps{NAN}; // magnetic inclination returned by the geo library using the last valid GPS position (rad)
|
|
||||||
float _mag_strength_gps{NAN}; // magnetic strength returned by the geo library using the last valid GPS position (T)
|
|
||||||
|
|
||||||
float _mag_inclination{NAN};
|
|
||||||
float _mag_strength{NAN};
|
|
||||||
|
|
||||||
// this is the current status of the filter control modes
|
|
||||||
filter_control_status_u _control_status{};
|
|
||||||
|
|
||||||
// this is the previous status of the filter control modes - used to detect mode transitions
|
|
||||||
filter_control_status_u _control_status_prev{};
|
|
||||||
|
|
||||||
// these are used to record single frame events for external monitoring and should NOT be used for
|
|
||||||
// state logic becasue they will be cleared externally after being read.
|
|
||||||
warning_event_status_u _warning_events{};
|
|
||||||
information_event_status_u _information_events{};
|
|
||||||
|
|
||||||
private:
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
|
||||||
void setDragData(const imuSample &imu);
|
|
||||||
|
|
||||||
// Used by the multi-rotor specific drag force fusion
|
|
||||||
uint8_t _drag_sample_count{0}; // number of drag specific force samples assumulated at the filter prediction rate
|
|
||||||
float _drag_sample_time_dt{0.0f}; // time integral across all samples used to form _drag_down_sampled (sec)
|
|
||||||
#endif // CONFIG_EKF2_DRAG_FUSION
|
|
||||||
|
|
||||||
void printBufferAllocationFailed(const char *buffer_name);
|
|
||||||
|
|
||||||
ImuDownSampler _imu_down_sampler{_params.filter_update_interval_us};
|
|
||||||
|
|
||||||
unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec)
|
|
||||||
};
|
|
||||||
#endif // !EKF_ESTIMATOR_INTERFACE_H
|
|
|
@ -38,6 +38,43 @@
|
||||||
|
|
||||||
#include "ekf.h"
|
#include "ekf.h"
|
||||||
|
|
||||||
|
void Ekf::setExtVisionData(const extVisionSample &evdata)
|
||||||
|
{
|
||||||
|
if (!_initialised) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Allocate the required buffer size if not previously done
|
||||||
|
if (_ext_vision_buffer == nullptr) {
|
||||||
|
_ext_vision_buffer = new RingBuffer<extVisionSample>(_obs_buffer_length);
|
||||||
|
|
||||||
|
if (_ext_vision_buffer == nullptr || !_ext_vision_buffer->valid()) {
|
||||||
|
delete _ext_vision_buffer;
|
||||||
|
_ext_vision_buffer = nullptr;
|
||||||
|
printBufferAllocationFailed("vision");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// calculate the system time-stamp for the mid point of the integration period
|
||||||
|
const int64_t time_us = evdata.time_us
|
||||||
|
- static_cast<int64_t>(_params.ev_delay_ms * 1000)
|
||||||
|
- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
|
||||||
|
|
||||||
|
// limit data rate to prevent data being lost
|
||||||
|
if (time_us >= static_cast<int64_t>(_ext_vision_buffer->get_newest().time_us + _min_obs_interval_us)) {
|
||||||
|
|
||||||
|
extVisionSample ev_sample_new{evdata};
|
||||||
|
ev_sample_new.time_us = time_us;
|
||||||
|
|
||||||
|
_ext_vision_buffer->push(ev_sample_new);
|
||||||
|
_time_last_ext_vision_buffer_push = _time_latest_us;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
ECL_WARN("EV data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _ext_vision_buffer->get_newest().time_us, _min_obs_interval_us);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
void Ekf::controlExternalVisionFusion()
|
void Ekf::controlExternalVisionFusion()
|
||||||
{
|
{
|
||||||
_ev_pos_b_est.predict(_dt_ekf_avg);
|
_ev_pos_b_est.predict(_dt_ekf_avg);
|
||||||
|
|
|
@ -39,6 +39,85 @@
|
||||||
#include "ekf.h"
|
#include "ekf.h"
|
||||||
#include <mathlib/mathlib.h>
|
#include <mathlib/mathlib.h>
|
||||||
|
|
||||||
|
void Ekf::setGpsData(const gpsMessage &gps)
|
||||||
|
{
|
||||||
|
if (!_initialised) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Allocate the required buffer size if not previously done
|
||||||
|
if (_gps_buffer == nullptr) {
|
||||||
|
_gps_buffer = new RingBuffer<gpsSample>(_obs_buffer_length);
|
||||||
|
|
||||||
|
if (_gps_buffer == nullptr || !_gps_buffer->valid()) {
|
||||||
|
delete _gps_buffer;
|
||||||
|
_gps_buffer = nullptr;
|
||||||
|
printBufferAllocationFailed("GPS");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
const int64_t time_us = gps.time_usec
|
||||||
|
- static_cast<int64_t>(_params.gps_delay_ms * 1000)
|
||||||
|
- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
|
||||||
|
|
||||||
|
if (time_us >= static_cast<int64_t>(_gps_buffer->get_newest().time_us + _min_obs_interval_us)) {
|
||||||
|
|
||||||
|
if (!gps.vel_ned_valid || (gps.fix_type == 0)) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
gpsSample gps_sample_new;
|
||||||
|
|
||||||
|
gps_sample_new.time_us = time_us;
|
||||||
|
|
||||||
|
gps_sample_new.vel = gps.vel_ned;
|
||||||
|
|
||||||
|
gps_sample_new.sacc = gps.sacc;
|
||||||
|
gps_sample_new.hacc = gps.eph;
|
||||||
|
gps_sample_new.vacc = gps.epv;
|
||||||
|
|
||||||
|
gps_sample_new.hgt = (float)gps.alt * 1e-3f;
|
||||||
|
|
||||||
|
#if defined(CONFIG_EKF2_GNSS_YAW)
|
||||||
|
|
||||||
|
if (PX4_ISFINITE(gps.yaw)) {
|
||||||
|
_time_last_gps_yaw_buffer_push = _time_latest_us;
|
||||||
|
gps_sample_new.yaw = gps.yaw;
|
||||||
|
gps_sample_new.yaw_acc = PX4_ISFINITE(gps.yaw_accuracy) ? gps.yaw_accuracy : 0.f;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
gps_sample_new.yaw = NAN;
|
||||||
|
gps_sample_new.yaw_acc = 0.f;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (PX4_ISFINITE(gps.yaw_offset)) {
|
||||||
|
_gps_yaw_offset = gps.yaw_offset;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
_gps_yaw_offset = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif // CONFIG_EKF2_GNSS_YAW
|
||||||
|
|
||||||
|
// Only calculate the relative position if the WGS-84 location of the origin is set
|
||||||
|
if (collect_gps(gps)) {
|
||||||
|
gps_sample_new.pos = _pos_ref.project((gps.lat / 1.0e7), (gps.lon / 1.0e7));
|
||||||
|
|
||||||
|
} else {
|
||||||
|
gps_sample_new.pos(0) = 0.0f;
|
||||||
|
gps_sample_new.pos(1) = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
_gps_buffer->push(gps_sample_new);
|
||||||
|
_time_last_gps_buffer_push = _time_latest_us;
|
||||||
|
|
||||||
|
|
||||||
|
} else {
|
||||||
|
ECL_WARN("GPS data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _gps_buffer->get_newest().time_us, _min_obs_interval_us);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
void Ekf::controlGpsFusion(const imuSample &imu_delayed)
|
void Ekf::controlGpsFusion(const imuSample &imu_delayed)
|
||||||
{
|
{
|
||||||
if (!_gps_buffer || (_params.gnss_ctrl == 0)) {
|
if (!_gps_buffer || (_params.gnss_ctrl == 0)) {
|
||||||
|
|
|
@ -39,6 +39,42 @@
|
||||||
#include "ekf.h"
|
#include "ekf.h"
|
||||||
#include <mathlib/mathlib.h>
|
#include <mathlib/mathlib.h>
|
||||||
|
|
||||||
|
void Ekf::setMagData(const magSample &mag_sample)
|
||||||
|
{
|
||||||
|
if (!_initialised) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Allocate the required buffer size if not previously done
|
||||||
|
if (_mag_buffer == nullptr) {
|
||||||
|
_mag_buffer = new RingBuffer<magSample>(_obs_buffer_length);
|
||||||
|
|
||||||
|
if (_mag_buffer == nullptr || !_mag_buffer->valid()) {
|
||||||
|
delete _mag_buffer;
|
||||||
|
_mag_buffer = nullptr;
|
||||||
|
printBufferAllocationFailed("mag");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
const int64_t time_us = mag_sample.time_us
|
||||||
|
- static_cast<int64_t>(_params.mag_delay_ms * 1000)
|
||||||
|
- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
|
||||||
|
|
||||||
|
// limit data rate to prevent data being lost
|
||||||
|
if (time_us >= static_cast<int64_t>(_mag_buffer->get_newest().time_us + _min_obs_interval_us)) {
|
||||||
|
|
||||||
|
magSample mag_sample_new{mag_sample};
|
||||||
|
mag_sample_new.time_us = time_us;
|
||||||
|
|
||||||
|
_mag_buffer->push(mag_sample_new);
|
||||||
|
_time_last_mag_buffer_push = _time_latest_us;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
ECL_WARN("mag data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _mag_buffer->get_newest().time_us, _min_obs_interval_us);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
void Ekf::controlMagFusion()
|
void Ekf::controlMagFusion()
|
||||||
{
|
{
|
||||||
// reset the flight alignment flag so that the mag fields will be
|
// reset the flight alignment flag so that the mag fields will be
|
||||||
|
|
|
@ -38,6 +38,41 @@
|
||||||
|
|
||||||
#include "ekf.h"
|
#include "ekf.h"
|
||||||
|
|
||||||
|
void Ekf::setOpticalFlowData(const flowSample &flow)
|
||||||
|
{
|
||||||
|
if (!_initialised) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Allocate the required buffer size if not previously done
|
||||||
|
if (_flow_buffer == nullptr) {
|
||||||
|
_flow_buffer = new RingBuffer<flowSample>(_imu_buffer_length);
|
||||||
|
|
||||||
|
if (_flow_buffer == nullptr || !_flow_buffer->valid()) {
|
||||||
|
delete _flow_buffer;
|
||||||
|
_flow_buffer = nullptr;
|
||||||
|
printBufferAllocationFailed("flow");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
const int64_t time_us = flow.time_us
|
||||||
|
- static_cast<int64_t>(_params.flow_delay_ms * 1000)
|
||||||
|
- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
|
||||||
|
|
||||||
|
// limit data rate to prevent data being lost
|
||||||
|
if (time_us >= static_cast<int64_t>(_flow_buffer->get_newest().time_us + _min_obs_interval_us)) {
|
||||||
|
|
||||||
|
flowSample optflow_sample_new{flow};
|
||||||
|
optflow_sample_new.time_us = time_us;
|
||||||
|
|
||||||
|
_flow_buffer->push(optflow_sample_new);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
ECL_WARN("optical flow data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _flow_buffer->get_newest().time_us, _min_obs_interval_us);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
|
void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
|
||||||
{
|
{
|
||||||
if (_flow_buffer) {
|
if (_flow_buffer) {
|
||||||
|
|
|
@ -38,6 +38,42 @@
|
||||||
|
|
||||||
#include "ekf.h"
|
#include "ekf.h"
|
||||||
|
|
||||||
|
void Ekf::setRangeData(const rangeSample &range_sample)
|
||||||
|
{
|
||||||
|
if (!_initialised) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Allocate the required buffer size if not previously done
|
||||||
|
if (_range_buffer == nullptr) {
|
||||||
|
_range_buffer = new RingBuffer<rangeSample>(_obs_buffer_length);
|
||||||
|
|
||||||
|
if (_range_buffer == nullptr || !_range_buffer->valid()) {
|
||||||
|
delete _range_buffer;
|
||||||
|
_range_buffer = nullptr;
|
||||||
|
printBufferAllocationFailed("range");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
const int64_t time_us = range_sample.time_us
|
||||||
|
- static_cast<int64_t>(_params.range_delay_ms * 1000)
|
||||||
|
- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
|
||||||
|
|
||||||
|
// limit data rate to prevent data being lost
|
||||||
|
if (time_us >= static_cast<int64_t>(_range_buffer->get_newest().time_us + _min_obs_interval_us)) {
|
||||||
|
|
||||||
|
rangeSample range_sample_new{range_sample};
|
||||||
|
range_sample_new.time_us = time_us;
|
||||||
|
|
||||||
|
_range_buffer->push(range_sample_new);
|
||||||
|
_time_last_range_buffer_push = _time_latest_us;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
ECL_WARN("range data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _range_buffer->get_newest().time_us, _min_obs_interval_us);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
void Ekf::controlRangeHeightFusion()
|
void Ekf::controlRangeHeightFusion()
|
||||||
{
|
{
|
||||||
static constexpr const char *HGT_SRC_NAME = "RNG";
|
static constexpr const char *HGT_SRC_NAME = "RNG";
|
||||||
|
|
|
@ -1002,6 +1002,11 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp)
|
||||||
PublishAidSourceStatus(_ekf.aid_src_baro_hgt(), _status_baro_hgt_pub_last, _estimator_aid_src_baro_hgt_pub);
|
PublishAidSourceStatus(_ekf.aid_src_baro_hgt(), _status_baro_hgt_pub_last, _estimator_aid_src_baro_hgt_pub);
|
||||||
#endif // CONFIG_EKF2_BAROMETER
|
#endif // CONFIG_EKF2_BAROMETER
|
||||||
|
|
||||||
|
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
||||||
|
// drag
|
||||||
|
PublishAidSourceStatus(_ekf.aid_src_drag(), _status_drag_pub_last, _estimator_aid_src_drag_pub);
|
||||||
|
#endif // CONFIG_EKF2_DRAG_FUSION
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||||
// RNG height
|
// RNG height
|
||||||
PublishAidSourceStatus(_ekf.aid_src_rng_hgt(), _status_rng_hgt_pub_last, _estimator_aid_src_rng_hgt_pub);
|
PublishAidSourceStatus(_ekf.aid_src_rng_hgt(), _status_rng_hgt_pub_last, _estimator_aid_src_rng_hgt_pub);
|
||||||
|
@ -1344,49 +1349,89 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp)
|
||||||
// publish estimator innovation data
|
// publish estimator innovation data
|
||||||
estimator_innovations_s innovations{};
|
estimator_innovations_s innovations{};
|
||||||
innovations.timestamp_sample = _ekf.time_delayed_us();
|
innovations.timestamp_sample = _ekf.time_delayed_us();
|
||||||
_ekf.getGpsVelPosInnov(innovations.gps_hvel, innovations.gps_vvel, innovations.gps_hpos, innovations.gps_vpos);
|
|
||||||
|
// GPS
|
||||||
|
innovations.gps_hvel[0] = _ekf.aid_src_gnss_vel().innovation[0];
|
||||||
|
innovations.gps_hvel[1] = _ekf.aid_src_gnss_vel().innovation[1];
|
||||||
|
innovations.gps_vvel = _ekf.aid_src_gnss_vel().innovation[2];
|
||||||
|
innovations.gps_hpos[0] = _ekf.aid_src_gnss_pos().innovation[0];
|
||||||
|
innovations.gps_hpos[1] = _ekf.aid_src_gnss_pos().innovation[1];
|
||||||
|
innovations.gps_vpos = _ekf.aid_src_gnss_hgt().innovation;
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||||
_ekf.getEvVelPosInnov(innovations.ev_hvel, innovations.ev_vvel, innovations.ev_hpos, innovations.ev_vpos);
|
// External Vision
|
||||||
|
innovations.ev_hvel[0] = _ekf.aid_src_ev_vel().innovation[0];
|
||||||
|
innovations.ev_hvel[1] = _ekf.aid_src_ev_vel().innovation[1];
|
||||||
|
innovations.ev_vvel = _ekf.aid_src_ev_vel().innovation[2];
|
||||||
|
innovations.ev_hpos[0] = _ekf.aid_src_ev_pos().innovation[0];
|
||||||
|
innovations.ev_hpos[1] = _ekf.aid_src_ev_pos().innovation[1];
|
||||||
|
innovations.ev_vpos = _ekf.aid_src_ev_hgt().innovation;
|
||||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||||
#if defined(CONFIG_EKF2_BAROMETER)
|
|
||||||
_ekf.getBaroHgtInnov(innovations.baro_vpos);
|
// Height sensors
|
||||||
#endif // CONFIG_EKF2_BAROMETER
|
|
||||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||||
_ekf.getRngHgtInnov(innovations.rng_vpos);
|
innovations.rng_vpos = _ekf.aid_src_rng_hgt().innovation;
|
||||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||||
|
#if defined(CONFIG_EKF2_BAROMETER)
|
||||||
|
innovations.baro_vpos = _ekf.aid_src_baro_hgt().innovation;
|
||||||
|
#endif // CONFIG_EKF2_BAROMETER
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_AUXVEL)
|
#if defined(CONFIG_EKF2_AUXVEL)
|
||||||
_ekf.getAuxVelInnov(innovations.aux_hvel);
|
// Auxiliary velocity
|
||||||
|
innovations.aux_hvel[0] = _ekf.aid_src_aux_vel().innovation[0];
|
||||||
|
innovations.aux_hvel[1] = _ekf.aid_src_aux_vel().innovation[1];
|
||||||
#endif // CONFIG_EKF2_AUXVEL
|
#endif // CONFIG_EKF2_AUXVEL
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||||
_ekf.getFlowInnov(innovations.flow);
|
// Optical flow
|
||||||
|
innovations.flow[0] = _ekf.aid_src_optical_flow().innovation[0];
|
||||||
|
innovations.flow[1] = _ekf.aid_src_optical_flow().innovation[1];
|
||||||
|
# if defined(CONFIG_EKF2_TERRAIN)
|
||||||
|
innovations.terr_flow[0] = _ekf.aid_src_terrain_optical_flow().innovation[0];
|
||||||
|
innovations.terr_flow[1] = _ekf.aid_src_terrain_optical_flow().innovation[1];
|
||||||
|
# endif // CONFIG_EKF2_TERRAIN
|
||||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||||
_ekf.getHeadingInnov(innovations.heading);
|
|
||||||
|
// heading
|
||||||
|
innovations.heading = _ekf.getHeadingInnov();
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||||
_ekf.getMagInnov(innovations.mag_field);
|
// mag_field
|
||||||
|
innovations.mag_field[0] = _ekf.aid_src_mag().innovation[0];
|
||||||
|
innovations.mag_field[1] = _ekf.aid_src_mag().innovation[1];
|
||||||
|
innovations.mag_field[2] = _ekf.aid_src_mag().innovation[2];
|
||||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||||
|
|
||||||
|
// gravity
|
||||||
|
innovations.gravity[0] = _ekf.aid_src_gravity().innovation[0];
|
||||||
|
innovations.gravity[1] = _ekf.aid_src_gravity().innovation[1];
|
||||||
|
innovations.gravity[2] = _ekf.aid_src_gravity().innovation[2];
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
||||||
_ekf.getDragInnov(innovations.drag);
|
// drag
|
||||||
|
innovations.drag[0] = _ekf.aid_src_drag().innovation[0];
|
||||||
|
innovations.drag[1] = _ekf.aid_src_drag().innovation[1];
|
||||||
#endif // CONFIG_EKF2_DRAG_FUSION
|
#endif // CONFIG_EKF2_DRAG_FUSION
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||||
_ekf.getAirspeedInnov(innovations.airspeed);
|
// airspeed
|
||||||
|
innovations.airspeed = _ekf.aid_src_airspeed().innovation;
|
||||||
#endif // CONFIG_EKF2_AIRSPEED
|
#endif // CONFIG_EKF2_AIRSPEED
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_SIDESLIP)
|
#if defined(CONFIG_EKF2_SIDESLIP)
|
||||||
_ekf.getBetaInnov(innovations.beta);
|
// beta
|
||||||
|
innovations.beta = _ekf.aid_src_sideslip().innovation;
|
||||||
#endif // CONFIG_EKF2_SIDESLIP
|
#endif // CONFIG_EKF2_SIDESLIP
|
||||||
|
|
||||||
_ekf.getGravityInnov(innovations.gravity);
|
#if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_RANGE_FINDER)
|
||||||
|
// hagl
|
||||||
|
innovations.hagl = _ekf.aid_src_terrain_range_finder().innovation;
|
||||||
|
#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_TERRAIN)
|
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||||
# if defined(CONFIG_EKF2_RANGE_FINDER)
|
// hagl_rate
|
||||||
_ekf.getHaglInnov(innovations.hagl);
|
innovations.hagl_rate = _ekf.getHaglRateInnov();
|
||||||
# endif //CONFIG_EKF2_RANGE_FINDER
|
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||||
# if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
|
||||||
_ekf.getTerrainFlowInnov(innovations.terr_flow);
|
|
||||||
# endif // CONFIG_EKF2_OPTICAL_FLOW
|
|
||||||
#endif // CONFIG_EKF2_TERRAIN
|
|
||||||
|
|
||||||
// Not yet supported
|
|
||||||
innovations.aux_vvel = NAN;
|
|
||||||
|
|
||||||
innovations.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
|
innovations.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
|
||||||
_estimator_innovations_pub.publish(innovations);
|
_estimator_innovations_pub.publish(innovations);
|
||||||
|
@ -1434,51 +1479,89 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp)
|
||||||
// publish estimator innovation test ratio data
|
// publish estimator innovation test ratio data
|
||||||
estimator_innovations_s test_ratios{};
|
estimator_innovations_s test_ratios{};
|
||||||
test_ratios.timestamp_sample = _ekf.time_delayed_us();
|
test_ratios.timestamp_sample = _ekf.time_delayed_us();
|
||||||
_ekf.getGpsVelPosInnovRatio(test_ratios.gps_hvel[0], test_ratios.gps_vvel, test_ratios.gps_hpos[0],
|
|
||||||
test_ratios.gps_vpos);
|
// GPS
|
||||||
|
test_ratios.gps_hvel[0] = _ekf.aid_src_gnss_vel().test_ratio[0];
|
||||||
|
test_ratios.gps_hvel[1] = _ekf.aid_src_gnss_vel().test_ratio[1];
|
||||||
|
test_ratios.gps_vvel = _ekf.aid_src_gnss_vel().test_ratio[2];
|
||||||
|
test_ratios.gps_hpos[0] = _ekf.aid_src_gnss_pos().test_ratio[0];
|
||||||
|
test_ratios.gps_hpos[1] = _ekf.aid_src_gnss_pos().test_ratio[1];
|
||||||
|
test_ratios.gps_vpos = _ekf.aid_src_gnss_hgt().test_ratio;
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||||
_ekf.getEvVelPosInnovRatio(test_ratios.ev_hvel[0], test_ratios.ev_vvel, test_ratios.ev_hpos[0], test_ratios.ev_vpos);
|
// External Vision
|
||||||
|
test_ratios.ev_hvel[0] = _ekf.aid_src_ev_vel().test_ratio[0];
|
||||||
|
test_ratios.ev_hvel[1] = _ekf.aid_src_ev_vel().test_ratio[1];
|
||||||
|
test_ratios.ev_vvel = _ekf.aid_src_ev_vel().test_ratio[2];
|
||||||
|
test_ratios.ev_hpos[0] = _ekf.aid_src_ev_pos().test_ratio[0];
|
||||||
|
test_ratios.ev_hpos[1] = _ekf.aid_src_ev_pos().test_ratio[1];
|
||||||
|
test_ratios.ev_vpos = _ekf.aid_src_ev_hgt().test_ratio;
|
||||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||||
#if defined(CONFIG_EKF2_BAROMETER)
|
|
||||||
_ekf.getBaroHgtInnovRatio(test_ratios.baro_vpos);
|
// Height sensors
|
||||||
#endif // CONFIG_EKF2_BAROMETER
|
|
||||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||||
_ekf.getRngHgtInnovRatio(test_ratios.rng_vpos);
|
test_ratios.rng_vpos = _ekf.aid_src_rng_hgt().test_ratio;
|
||||||
_ekf.getHaglRateInnovRatio(test_ratios.hagl_rate);
|
|
||||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||||
|
#if defined(CONFIG_EKF2_BAROMETER)
|
||||||
|
test_ratios.baro_vpos = _ekf.aid_src_baro_hgt().test_ratio;
|
||||||
|
#endif // CONFIG_EKF2_BAROMETER
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_AUXVEL)
|
#if defined(CONFIG_EKF2_AUXVEL)
|
||||||
_ekf.getAuxVelInnovRatio(test_ratios.aux_hvel[0]);
|
// Auxiliary velocity
|
||||||
|
test_ratios.aux_hvel[0] = _ekf.aid_src_aux_vel().test_ratio[0];
|
||||||
|
test_ratios.aux_hvel[1] = _ekf.aid_src_aux_vel().test_ratio[1];
|
||||||
#endif // CONFIG_EKF2_AUXVEL
|
#endif // CONFIG_EKF2_AUXVEL
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||||
_ekf.getFlowInnovRatio(test_ratios.flow[0]);
|
// Optical flow
|
||||||
|
test_ratios.flow[0] = _ekf.aid_src_optical_flow().test_ratio[0];
|
||||||
|
test_ratios.flow[1] = _ekf.aid_src_optical_flow().test_ratio[1];
|
||||||
|
# if defined(CONFIG_EKF2_TERRAIN)
|
||||||
|
test_ratios.terr_flow[0] = _ekf.aid_src_terrain_optical_flow().test_ratio[0];
|
||||||
|
test_ratios.terr_flow[1] = _ekf.aid_src_terrain_optical_flow().test_ratio[1];
|
||||||
|
# endif // CONFIG_EKF2_TERRAIN
|
||||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||||
_ekf.getHeadingInnovRatio(test_ratios.heading);
|
|
||||||
|
// heading
|
||||||
|
test_ratios.heading = _ekf.getHeadingInnovRatio();
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||||
_ekf.getMagInnovRatio(test_ratios.mag_field[0]);
|
// mag_field
|
||||||
|
test_ratios.mag_field[0] = _ekf.aid_src_mag().test_ratio[0];
|
||||||
|
test_ratios.mag_field[1] = _ekf.aid_src_mag().test_ratio[1];
|
||||||
|
test_ratios.mag_field[2] = _ekf.aid_src_mag().test_ratio[2];
|
||||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||||
|
|
||||||
|
// gravity
|
||||||
|
test_ratios.gravity[0] = _ekf.aid_src_gravity().test_ratio[0];
|
||||||
|
test_ratios.gravity[1] = _ekf.aid_src_gravity().test_ratio[1];
|
||||||
|
test_ratios.gravity[2] = _ekf.aid_src_gravity().test_ratio[2];
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
||||||
_ekf.getDragInnovRatio(&test_ratios.drag[0]);
|
// drag
|
||||||
|
test_ratios.drag[0] = _ekf.aid_src_drag().test_ratio[0];
|
||||||
|
test_ratios.drag[1] = _ekf.aid_src_drag().test_ratio[1];
|
||||||
#endif // CONFIG_EKF2_DRAG_FUSION
|
#endif // CONFIG_EKF2_DRAG_FUSION
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||||
_ekf.getAirspeedInnovRatio(test_ratios.airspeed);
|
// airspeed
|
||||||
|
test_ratios.airspeed = _ekf.aid_src_airspeed().test_ratio;
|
||||||
#endif // CONFIG_EKF2_AIRSPEED
|
#endif // CONFIG_EKF2_AIRSPEED
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_SIDESLIP)
|
#if defined(CONFIG_EKF2_SIDESLIP)
|
||||||
_ekf.getBetaInnovRatio(test_ratios.beta);
|
// beta
|
||||||
|
test_ratios.beta = _ekf.aid_src_sideslip().test_ratio;
|
||||||
#endif // CONFIG_EKF2_SIDESLIP
|
#endif // CONFIG_EKF2_SIDESLIP
|
||||||
|
|
||||||
_ekf.getGravityInnovRatio(test_ratios.gravity[0]);
|
#if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_RANGE_FINDER)
|
||||||
|
// hagl
|
||||||
|
test_ratios.hagl = _ekf.aid_src_terrain_range_finder().test_ratio;
|
||||||
|
#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_TERRAIN)
|
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||||
# if defined(CONFIG_EKF2_RANGE_FINDER)
|
// hagl_rate
|
||||||
_ekf.getHaglInnovRatio(test_ratios.hagl);
|
test_ratios.hagl_rate = _ekf.getHaglRateInnovRatio();
|
||||||
# endif
|
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||||
# if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
|
||||||
_ekf.getTerrainFlowInnovRatio(test_ratios.terr_flow[0]);
|
|
||||||
# endif // CONFIG_EKF2_OPTICAL_FLOW
|
|
||||||
#endif // CONFIG_EKF2_TERRAIN
|
|
||||||
|
|
||||||
// Not yet supported
|
|
||||||
test_ratios.aux_vvel = NAN;
|
|
||||||
|
|
||||||
test_ratios.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
|
test_ratios.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
|
||||||
_estimator_innovation_test_ratios_pub.publish(test_ratios);
|
_estimator_innovation_test_ratios_pub.publish(test_ratios);
|
||||||
|
@ -1489,50 +1572,89 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp)
|
||||||
// publish estimator innovation variance data
|
// publish estimator innovation variance data
|
||||||
estimator_innovations_s variances{};
|
estimator_innovations_s variances{};
|
||||||
variances.timestamp_sample = _ekf.time_delayed_us();
|
variances.timestamp_sample = _ekf.time_delayed_us();
|
||||||
_ekf.getGpsVelPosInnovVar(variances.gps_hvel, variances.gps_vvel, variances.gps_hpos, variances.gps_vpos);
|
|
||||||
|
// GPS
|
||||||
|
variances.gps_hvel[0] = _ekf.aid_src_gnss_vel().innovation_variance[0];
|
||||||
|
variances.gps_hvel[1] = _ekf.aid_src_gnss_vel().innovation_variance[1];
|
||||||
|
variances.gps_vvel = _ekf.aid_src_gnss_vel().innovation_variance[2];
|
||||||
|
variances.gps_hpos[0] = _ekf.aid_src_gnss_pos().innovation_variance[0];
|
||||||
|
variances.gps_hpos[1] = _ekf.aid_src_gnss_pos().innovation_variance[1];
|
||||||
|
variances.gps_vpos = _ekf.aid_src_gnss_hgt().innovation_variance;
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||||
_ekf.getEvVelPosInnovVar(variances.ev_hvel, variances.ev_vvel, variances.ev_hpos, variances.ev_vpos);
|
// External Vision
|
||||||
|
variances.ev_hvel[0] = _ekf.aid_src_ev_vel().innovation_variance[0];
|
||||||
|
variances.ev_hvel[1] = _ekf.aid_src_ev_vel().innovation_variance[1];
|
||||||
|
variances.ev_vvel = _ekf.aid_src_ev_vel().innovation_variance[2];
|
||||||
|
variances.ev_hpos[0] = _ekf.aid_src_ev_pos().innovation_variance[0];
|
||||||
|
variances.ev_hpos[1] = _ekf.aid_src_ev_pos().innovation_variance[1];
|
||||||
|
variances.ev_vpos = _ekf.aid_src_ev_hgt().innovation_variance;
|
||||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||||
#if defined(CONFIG_EKF2_BAROMETER)
|
|
||||||
_ekf.getBaroHgtInnovVar(variances.baro_vpos);
|
// Height sensors
|
||||||
#endif // CONFIG_EKF2_BAROMETER
|
|
||||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||||
_ekf.getRngHgtInnovVar(variances.rng_vpos);
|
variances.rng_vpos = _ekf.aid_src_rng_hgt().innovation_variance;
|
||||||
_ekf.getHaglRateInnovVar(variances.hagl_rate);
|
|
||||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||||
|
#if defined(CONFIG_EKF2_BAROMETER)
|
||||||
|
variances.baro_vpos = _ekf.aid_src_baro_hgt().innovation_variance;
|
||||||
|
#endif // CONFIG_EKF2_BAROMETER
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_AUXVEL)
|
#if defined(CONFIG_EKF2_AUXVEL)
|
||||||
_ekf.getAuxVelInnovVar(variances.aux_hvel);
|
// Auxiliary velocity
|
||||||
|
variances.aux_hvel[0] = _ekf.aid_src_aux_vel().innovation_variance[0];
|
||||||
|
variances.aux_hvel[1] = _ekf.aid_src_aux_vel().innovation_variance[1];
|
||||||
#endif // CONFIG_EKF2_AUXVEL
|
#endif // CONFIG_EKF2_AUXVEL
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||||
_ekf.getFlowInnovVar(variances.flow);
|
// Optical flow
|
||||||
|
variances.flow[0] = _ekf.aid_src_optical_flow().innovation_variance[0];
|
||||||
|
variances.flow[1] = _ekf.aid_src_optical_flow().innovation_variance[1];
|
||||||
|
# if defined(CONFIG_EKF2_TERRAIN)
|
||||||
|
variances.terr_flow[0] = _ekf.aid_src_terrain_optical_flow().innovation_variance[0];
|
||||||
|
variances.terr_flow[1] = _ekf.aid_src_terrain_optical_flow().innovation_variance[1];
|
||||||
|
# endif // CONFIG_EKF2_TERRAIN
|
||||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||||
_ekf.getHeadingInnovVar(variances.heading);
|
|
||||||
|
// heading
|
||||||
|
variances.heading = _ekf.getHeadingInnovVar();
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||||
_ekf.getMagInnovVar(variances.mag_field);
|
// mag_field
|
||||||
|
variances.mag_field[0] = _ekf.aid_src_mag().innovation_variance[0];
|
||||||
|
variances.mag_field[1] = _ekf.aid_src_mag().innovation_variance[1];
|
||||||
|
variances.mag_field[2] = _ekf.aid_src_mag().innovation_variance[2];
|
||||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||||
|
|
||||||
|
// gravity
|
||||||
|
variances.gravity[0] = _ekf.aid_src_gravity().innovation_variance[0];
|
||||||
|
variances.gravity[1] = _ekf.aid_src_gravity().innovation_variance[1];
|
||||||
|
variances.gravity[2] = _ekf.aid_src_gravity().innovation_variance[2];
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
||||||
_ekf.getDragInnovVar(variances.drag);
|
// drag
|
||||||
|
variances.drag[0] = _ekf.aid_src_drag().innovation_variance[0];
|
||||||
|
variances.drag[1] = _ekf.aid_src_drag().innovation_variance[1];
|
||||||
#endif // CONFIG_EKF2_DRAG_FUSION
|
#endif // CONFIG_EKF2_DRAG_FUSION
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||||
_ekf.getAirspeedInnovVar(variances.airspeed);
|
// airspeed
|
||||||
|
variances.airspeed = _ekf.aid_src_airspeed().innovation_variance;
|
||||||
#endif // CONFIG_EKF2_AIRSPEED
|
#endif // CONFIG_EKF2_AIRSPEED
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_SIDESLIP)
|
#if defined(CONFIG_EKF2_SIDESLIP)
|
||||||
_ekf.getBetaInnovVar(variances.beta);
|
// beta
|
||||||
|
variances.beta = _ekf.aid_src_sideslip().innovation_variance;
|
||||||
#endif // CONFIG_EKF2_SIDESLIP
|
#endif // CONFIG_EKF2_SIDESLIP
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_TERRAIN)
|
#if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_RANGE_FINDER)
|
||||||
# if defined(CONFIG_EKF2_RANGE_FINDER)
|
// hagl
|
||||||
_ekf.getHaglInnovVar(variances.hagl);
|
variances.hagl = _ekf.aid_src_terrain_range_finder().innovation_variance;
|
||||||
# endif // CONFIG_EKF2_RANGE_FINDER
|
|
||||||
# if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
|
||||||
_ekf.getTerrainFlowInnovVar(variances.terr_flow);
|
|
||||||
# endif // CONFIG_EKF2_OPTICAL_FLOW
|
|
||||||
#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER
|
#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER
|
||||||
|
|
||||||
_ekf.getGravityInnovVar(variances.gravity);
|
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||||
|
// hagl_rate
|
||||||
// Not yet supported
|
variances.hagl_rate = _ekf.getHaglRateInnovVar();
|
||||||
variances.aux_vvel = NAN;
|
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||||
|
|
||||||
variances.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
|
variances.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
|
||||||
_estimator_innovation_variances_pub.publish(variances);
|
_estimator_innovation_variances_pub.publish(variances);
|
||||||
|
@ -1955,7 +2077,7 @@ void EKF2::PublishYawEstimatorStatus(const hrt_abstime ×tamp)
|
||||||
|
|
||||||
void EKF2::PublishWindEstimate(const hrt_abstime ×tamp)
|
void EKF2::PublishWindEstimate(const hrt_abstime ×tamp)
|
||||||
{
|
{
|
||||||
if (_ekf.get_wind_status()) {
|
if (_ekf.control_status_flags().wind) {
|
||||||
// Publish wind estimate only if ekf declares them valid
|
// Publish wind estimate only if ekf declares them valid
|
||||||
wind_s wind{};
|
wind_s wind{};
|
||||||
wind.timestamp_sample = _ekf.time_delayed_us();
|
wind.timestamp_sample = _ekf.time_delayed_us();
|
||||||
|
@ -1964,12 +2086,12 @@ void EKF2::PublishWindEstimate(const hrt_abstime ×tamp)
|
||||||
const Vector2f wind_vel_var = _ekf.getWindVelocityVariance();
|
const Vector2f wind_vel_var = _ekf.getWindVelocityVariance();
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||||
_ekf.getAirspeedInnov(wind.tas_innov);
|
wind.tas_innov = _ekf.aid_src_airspeed().innovation;
|
||||||
_ekf.getAirspeedInnovVar(wind.tas_innov_var);
|
wind.tas_innov_var = _ekf.aid_src_airspeed().innovation_variance;
|
||||||
#endif // CONFIG_EKF2_AIRSPEED
|
#endif // CONFIG_EKF2_AIRSPEED
|
||||||
#if defined(CONFIG_EKF2_SIDESLIP)
|
#if defined(CONFIG_EKF2_SIDESLIP)
|
||||||
_ekf.getBetaInnov(wind.beta_innov);
|
wind.beta_innov = _ekf.aid_src_sideslip().innovation;
|
||||||
_ekf.getBetaInnovVar(wind.beta_innov_var);
|
wind.beta_innov = _ekf.aid_src_sideslip().innovation_variance;
|
||||||
#endif // CONFIG_EKF2_SIDESLIP
|
#endif // CONFIG_EKF2_SIDESLIP
|
||||||
|
|
||||||
wind.windspeed_north = wind_vel(0);
|
wind.windspeed_north = wind_vel(0);
|
||||||
|
|
|
@ -404,6 +404,11 @@ private:
|
||||||
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_baro_hgt_pub {ORB_ID(estimator_aid_src_baro_hgt)};
|
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_baro_hgt_pub {ORB_ID(estimator_aid_src_baro_hgt)};
|
||||||
#endif // CONFIG_EKF2_BAROMETER
|
#endif // CONFIG_EKF2_BAROMETER
|
||||||
|
|
||||||
|
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
||||||
|
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_drag_pub {ORB_ID(estimator_aid_src_drag)};
|
||||||
|
hrt_abstime _status_drag_pub_last{0};
|
||||||
|
#endif // CONFIG_EKF2_DRAG_FUSION
|
||||||
|
|
||||||
float _last_gnss_hgt_bias_published{};
|
float _last_gnss_hgt_bias_published{};
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||||
|
|
|
@ -39,7 +39,6 @@
|
||||||
#define EKF_EKF_LOGGER_H
|
#define EKF_EKF_LOGGER_H
|
||||||
|
|
||||||
#include "EKF/ekf.h"
|
#include "EKF/ekf.h"
|
||||||
#include "EKF/estimator_interface.h"
|
|
||||||
#include "ekf_wrapper.h"
|
#include "ekf_wrapper.h"
|
||||||
#include <fstream>
|
#include <fstream>
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
|
|
|
@ -40,7 +40,6 @@
|
||||||
|
|
||||||
#include <memory>
|
#include <memory>
|
||||||
#include "EKF/ekf.h"
|
#include "EKF/ekf.h"
|
||||||
#include "EKF/estimator_interface.h"
|
|
||||||
|
|
||||||
class EkfWrapper
|
class EkfWrapper
|
||||||
{
|
{
|
||||||
|
|
|
@ -245,22 +245,16 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_initialized)
|
||||||
|
|
||||||
_sensor_simulator.runSeconds(1);
|
_sensor_simulator.runSeconds(1);
|
||||||
|
|
||||||
float hpos = 0.f;
|
|
||||||
float vpos = 0.f;
|
|
||||||
float hvel = 0.f;
|
|
||||||
float vvel = 0.f;
|
|
||||||
float baro_vpos = 0.f;
|
|
||||||
|
|
||||||
// After the change of origin, the pos and vel innovations should stay small
|
// After the change of origin, the pos and vel innovations should stay small
|
||||||
_ekf->getGpsVelPosInnovRatio(hvel, vvel, hpos, vpos);
|
EXPECT_NEAR(_ekf->aid_src_gnss_pos().test_ratio[0], 0.f, 0.05f);
|
||||||
_ekf->getBaroHgtInnovRatio(baro_vpos);
|
EXPECT_NEAR(_ekf->aid_src_gnss_pos().test_ratio[1], 0.f, 0.05f);
|
||||||
|
EXPECT_NEAR(_ekf->aid_src_gnss_hgt().test_ratio, 0.f, 0.05f);
|
||||||
|
|
||||||
EXPECT_NEAR(hpos, 0.f, 0.05f);
|
EXPECT_NEAR(_ekf->aid_src_baro_hgt().test_ratio, 0.f, 0.05f);
|
||||||
EXPECT_NEAR(vpos, 0.f, 0.05f);
|
|
||||||
EXPECT_NEAR(baro_vpos, 0.f, 0.05f);
|
|
||||||
|
|
||||||
EXPECT_NEAR(hvel, 0.f, 0.02f);
|
EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[0], 0.f, 0.02f);
|
||||||
EXPECT_NEAR(vvel, 0.f, 0.02f);
|
EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[1], 0.f, 0.02f);
|
||||||
|
EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[2], 0.f, 0.02f);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized)
|
TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized)
|
||||||
|
@ -291,19 +285,14 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized)
|
||||||
|
|
||||||
_sensor_simulator.runSeconds(1);
|
_sensor_simulator.runSeconds(1);
|
||||||
|
|
||||||
float hpos = 0.f;
|
|
||||||
float vpos = 0.f;
|
|
||||||
float hvel = 0.f;
|
|
||||||
float vvel = 0.f;
|
|
||||||
|
|
||||||
// After the change of origin, the pos and vel innovations should stay small
|
// After the change of origin, the pos and vel innovations should stay small
|
||||||
_ekf->getGpsVelPosInnovRatio(hvel, vvel, hpos, vpos);
|
EXPECT_NEAR(_ekf->aid_src_gnss_pos().test_ratio[0], 0.f, 0.05f);
|
||||||
|
EXPECT_NEAR(_ekf->aid_src_gnss_pos().test_ratio[1], 0.f, 0.05f);
|
||||||
|
EXPECT_NEAR(_ekf->aid_src_gnss_hgt().test_ratio, 0.f, 0.05f);
|
||||||
|
|
||||||
EXPECT_NEAR(hpos, 0.f, 0.05f);
|
EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[0], 0.f, 0.02f);
|
||||||
EXPECT_NEAR(vpos, 0.f, 0.05f);
|
EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[1], 0.f, 0.02f);
|
||||||
|
EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[2], 0.f, 0.02f);
|
||||||
EXPECT_NEAR(hvel, 0.f, 0.02f);
|
|
||||||
EXPECT_NEAR(vvel, 0.f, 0.02f);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(EkfBasicsTest, global_position_from_local_ev)
|
TEST_F(EkfBasicsTest, global_position_from_local_ev)
|
||||||
|
|
|
@ -222,8 +222,7 @@ TEST_F(EkfGpsTest, altitudeDrift)
|
||||||
_sensor_simulator.runSeconds(dt);
|
_sensor_simulator.runSeconds(dt);
|
||||||
}
|
}
|
||||||
|
|
||||||
float baro_innov;
|
float baro_innov = _ekf->aid_src_baro_hgt().innovation;
|
||||||
_ekf->getBaroHgtInnov(baro_innov);
|
|
||||||
BiasEstimator::status status = _ekf->getBaroBiasEstimatorStatus();
|
BiasEstimator::status status = _ekf->getBaroBiasEstimatorStatus();
|
||||||
|
|
||||||
printf("baro innov = %f\n", (double)baro_innov);
|
printf("baro innov = %f\n", (double)baro_innov);
|
||||||
|
|
|
@ -198,13 +198,8 @@ TEST_F(EkfHeightFusionTest, gpsRef)
|
||||||
EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, baro_initial + baro_increment - gps_step, 0.2f);
|
EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, baro_initial + baro_increment - gps_step, 0.2f);
|
||||||
|
|
||||||
// and the innovations are close to zero
|
// and the innovations are close to zero
|
||||||
float baro_innov = NAN;
|
EXPECT_NEAR(_ekf->aid_src_baro_hgt().innovation, 0.f, 0.2f);
|
||||||
_ekf->getBaroHgtInnov(baro_innov);
|
EXPECT_NEAR(_ekf->aid_src_rng_hgt().innovation, 0.f, 0.2f);
|
||||||
EXPECT_NEAR(baro_innov, 0.f, 0.2f);
|
|
||||||
|
|
||||||
float rng_innov = NAN;
|
|
||||||
_ekf->getRngHgtInnov(rng_innov);
|
|
||||||
EXPECT_NEAR(rng_innov, 0.f, 0.2f);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(EkfHeightFusionTest, baroRefFailOver)
|
TEST_F(EkfHeightFusionTest, baroRefFailOver)
|
||||||
|
|
Loading…
Reference in New Issue