dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

WIP

This commit is contained in:
Daniel Agar 2023-10-26 12:49:41 -04:00
parent 3503d8388a
commit 877a82328c
5 changed files with 242 additions and 96 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/modules/ekf2/EKF/ekf.cpp
View File

@ -217,6 +217,27 @@ bool Ekf::update()
runTerrainEstimator(imu_sample_delayed);
#endif // CONFIG_EKF2_TERRAIN
if (_state_reset_status.reset_count.quat != _state_reset_count_prev.quat) {
_output_predictor.resetQuaternion();
}
if (_state_reset_status.reset_count.velNE != _state_reset_count_prev.velNE) {
_output_predictor.resetHorizontalVelocity();
}
if (_state_reset_status.reset_count.velD != _state_reset_count_prev.velD) {
_output_predictor.resetVerticalVelocity();
}
if (_state_reset_status.reset_count.posNE != _state_reset_count_prev.posNE) {
_output_predictor.resetHorizontalPosition();
}
if (_state_reset_status.reset_count.posD != _state_reset_count_prev.posD) {
_output_predictor.resetVerticalPosition();
}
updated = true;
}

13
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -74,8 +74,6 @@ void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f
P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 1, math::max(sq(0.01f), new_horz_vel_var(1)));
}
_output_predictor.resetHorizontalVelocityTo(_time_delayed_us, _state.vel.xy());
// record the state change
if (_state_reset_status.reset_count.velNE == _state_reset_count_prev.velNE) {
_state_reset_status.velNE_change = delta_horz_vel;
@ -100,8 +98,6 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var)
P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 2, math::max(sq(0.01f), new_vert_vel_var));
}
_output_predictor.resetVerticalVelocityTo(_time_delayed_us, _state.vel(2));
// record the state change
if (_state_reset_status.reset_count.velD == _state_reset_count_prev.velD) {
_state_reset_status.velD_change = delta_vert_vel;
@ -140,8 +136,6 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f
P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 1, math::max(sq(0.01f), new_horz_pos_var(1)));
}
_output_predictor.resetHorizontalPositionTo(_time_delayed_us, _state.pos.xy());
// record the state change
if (_state_reset_status.reset_count.posNE == _state_reset_count_prev.posNE) {
_state_reset_status.posNE_change = delta_horz_pos;
@ -185,10 +179,6 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v
const float delta_z = new_vert_pos - old_vert_pos;
// apply the change in height / height rate to our newest height / height rate estimate
// which have already been taken out from the output buffer
_output_predictor.resetVerticalPositionTo(_time_delayed_us, _state.pos(2));
// record the state change
if (_state_reset_status.reset_count.posD == _state_reset_count_prev.posD) {
_state_reset_status.posD_change = delta_z;
@ -922,9 +912,6 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
// restore covariance
resetQuatCov(rot_var_ned_before_reset);
// add the reset amount to the output observer buffered data
_output_predictor.resetQuaternion(_time_delayed_us, _state.quat_nominal);
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
// update EV attitude error filter
if (_ev_q_error_initialized) {

233
src/modules/ekf2/EKF/output_predictor.cpp
View File

@ -39,6 +39,8 @@ using matrix::Quatf;
using matrix::Vector2f;
using matrix::Vector3f;
//#define OUTPUT_PREDICTOR_DEBUG_VEROSE
void OutputPredictor::print_status()
{
printf("output predictor: IMU dt: %.4f, EKF dt: %.4f\n", (double)_dt_update_states_avg, (double)_dt_correct_states_avg);
@ -57,7 +59,6 @@ void OutputPredictor::reset()
_accel_bias.zero();
_gyro_bias.zero();
_time_last_update_states_us = 0;
_time_last_correct_states_us = 0;
_output_new = {};
@ -72,14 +73,25 @@ void OutputPredictor::reset()
_output_tracking_error.zero();
_reset_quaternion = true;
_reset_velocity_xy = true;
_reset_velocity_z = true;
_reset_position_xy = true;
_reset_position_z = true;
_output_filter_aligned = false;
}
void OutputPredictor::resetQuaternion(const uint64_t &time_delayed_us, const Quatf &new_quat)
void OutputPredictor::resetQuaternionTo(const uint64_t &time_delayed_us, const Quatf &new_quat)
{
// find the output observer state corresponding to the reset time
const outputSample &delayed_sample = findOutputSample(time_delayed_us);
#if defined(OUTPUT_PREDICTOR_DEBUG_VEROSE)
const outputSample delayed_orig = delayed_sample;
const outputSample newest_orig = _output_new;
#endif // OUTPUT_PREDICTOR_DEBUG_VEROSE
const Quatf quat_change = (new_quat * delayed_sample.quat_nominal.inversed()).normalized();
// add the reset amount to the output observer buffered data
@ -90,6 +102,30 @@ void OutputPredictor::resetQuaternion(const uint64_t &time_delayed_us, const Qua
// apply change to latest
_output_new.quat_nominal = (quat_change * _output_new.quat_nominal).normalized();
_R_to_earth_now = Dcmf(_output_new.quat_nominal);
_delta_angle_corr.zero();
_output_tracking_error(0) = 0.f;
_reset_quaternion = false;
#if defined(OUTPUT_PREDICTOR_DEBUG_VEROSE)
printf("output predictor: reset q delayed (%.1f,%.1f,%.1f)->(%.1f,%.1f,%.1f)\n",
(double)matrix::Eulerf(delayed_orig.quat_nominal).phi(), (double)matrix::Eulerf(delayed_orig.quat_nominal).theta(),
(double)matrix::Eulerf(delayed_orig.quat_nominal).psi(),
(double)matrix::Eulerf(delayed_sample.quat_nominal).phi(), (double)matrix::Eulerf(delayed_sample.quat_nominal).theta(),
(double)matrix::Eulerf(delayed_sample.quat_nominal).psi()
);
printf("output predictor: reset q newest (%.1f,%.1f,%.1f)->(%.1f,%.1f,%.1f)\n",
(double)matrix::Eulerf(newest_orig.quat_nominal).phi(), (double)matrix::Eulerf(newest_orig.quat_nominal).theta(),
(double)matrix::Eulerf(newest_orig.quat_nominal).psi(),
(double)matrix::Eulerf(_output_new.quat_nominal).phi(), (double)matrix::Eulerf(_output_new.quat_nominal).theta(),
(double)matrix::Eulerf(_output_new.quat_nominal).psi()
);
#endif // OUTPUT_PREDICTOR_DEBUG_VEROSE
}
void OutputPredictor::resetHorizontalVelocityTo(const uint64_t &time_delayed_us, const Vector2f &new_horz_vel)
@ -97,6 +133,11 @@ void OutputPredictor::resetHorizontalVelocityTo(const uint64_t &time_delayed_us,
// find the output observer state corresponding to the reset time
const outputSample &delayed_sample = findOutputSample(time_delayed_us);
#if defined(OUTPUT_PREDICTOR_DEBUG_VEROSE)
const outputSample delayed_orig = delayed_sample;
const outputSample newest_orig = _output_new;
#endif // OUTPUT_PREDICTOR_DEBUG_VEROSE
const Vector2f delta_vxy = new_horz_vel - delayed_sample.vel.xy(); // horizontal velocity
// add the reset amount to the output observer buffered data
@ -106,6 +147,24 @@ void OutputPredictor::resetHorizontalVelocityTo(const uint64_t &time_delayed_us,
// apply change to latest velocity
_output_new.vel.xy() += delta_vxy;
_vel_err_integ(0) = 0.f;
_vel_err_integ(1) = 0.f;
_reset_velocity_xy = false;
#if defined(OUTPUT_PREDICTOR_DEBUG_VEROSE)
printf("output predictor: reset v_xy delayed (%.1f,%.1f)->(%.1f,%.1f)\n",
(double)delayed_orig.vel(0), (double)delayed_orig.vel(1),
(double)delayed_sample.vel(0), (double)delayed_sample.vel(1)
);
printf("output predictor: reset v_xy newest (%.1f,%.1f)->(%.1f,%.1f)\n",
(double)newest_orig.vel(0), (double)newest_orig.vel(1),
(double)_output_new.vel(0), (double)_output_new.vel(1)
);
#endif // OUTPUT_PREDICTOR_DEBUG_VEROSE
}
void OutputPredictor::resetVerticalVelocityTo(const uint64_t &time_delayed_us, const float new_vert_vel)
@ -113,6 +172,11 @@ void OutputPredictor::resetVerticalVelocityTo(const uint64_t &time_delayed_us, c
// find the output observer state corresponding to the reset time
const outputSample &delayed_sample = findOutputSample(time_delayed_us);
#if defined(OUTPUT_PREDICTOR_DEBUG_VEROSE)
const outputSample delayed_orig = delayed_sample;
const outputSample newest_orig = _output_new;
#endif // OUTPUT_PREDICTOR_DEBUG_VEROSE
const float delta_vz = new_vert_vel - delayed_sample.vel(2); // vertical velocity
const float delta_vz_alt = new_vert_vel - delayed_sample.vert_vel_alt; // vertical velocity (alternative)
@ -125,6 +189,16 @@ void OutputPredictor::resetVerticalVelocityTo(const uint64_t &time_delayed_us, c
// apply change to latest
_output_new.vel(2) += delta_vz;
_output_new.vert_vel_alt += delta_vz_alt;
_vel_err_integ(2) = 0.f;
_reset_velocity_z = false;
#if defined(OUTPUT_PREDICTOR_DEBUG_VEROSE)
printf("output predictor: reset v_z delayed %.1f->%.1f\n", (double)delayed_orig.vel(2), (double)delayed_sample.vel(2));
printf("output predictor: reset v_z newest %.1f->%.1f\n", (double)newest_orig.vel(2), (double)_output_new.vel(2));
#endif // OUTPUT_PREDICTOR_DEBUG_VEROSE
}
void OutputPredictor::resetHorizontalPositionTo(const uint64_t &time_delayed_us, const Vector2f &new_horz_pos)
@ -132,6 +206,11 @@ void OutputPredictor::resetHorizontalPositionTo(const uint64_t &time_delayed_us,
// find the output observer state corresponding to the reset time
const outputSample &delayed_sample = findOutputSample(time_delayed_us);
#if defined(OUTPUT_PREDICTOR_DEBUG_VEROSE)
const outputSample delayed_orig = delayed_sample;
const outputSample newest_orig = _output_new;
#endif // OUTPUT_PREDICTOR_DEBUG_VEROSE
const Vector2f delta_xy = new_horz_pos - delayed_sample.pos.xy(); // horizontal position
// add the reset amount to the output observer buffered data
@ -141,6 +220,23 @@ void OutputPredictor::resetHorizontalPositionTo(const uint64_t &time_delayed_us,
// apply change to latest
_output_new.pos.xy() += delta_xy;
_pos_err_integ(0) = 0.f;
_pos_err_integ(1) = 0.f;
_reset_position_xy = false;
#if defined(OUTPUT_PREDICTOR_DEBUG_VEROSE)
printf("output predictor: reset xy delayed (%.1f,%.1f)->(%.1f,%.1f)\n",
(double)delayed_orig.pos(0), (double)delayed_orig.pos(1),
(double)delayed_sample.pos(0), (double)delayed_sample.pos(1)
);
printf("output predictor: reset xy newest (%.1f,%.1f)->(%.1f,%.1f)\n",
(double)newest_orig.pos(0), (double)newest_orig.pos(1),
(double)_output_new.pos(0), (double)_output_new.pos(1)
);
#endif // OUTPUT_PREDICTOR_DEBUG_VEROSE
}
void OutputPredictor::resetVerticalPositionTo(const uint64_t &time_delayed_us, const float new_vert_pos)
@ -148,6 +244,16 @@ void OutputPredictor::resetVerticalPositionTo(const uint64_t &time_delayed_us, c
// find the output observer state corresponding to the reset time
const outputSample &delayed_sample = findOutputSample(time_delayed_us);
#if defined(OUTPUT_PREDICTOR_DEBUG_VEROSE)
const outputSample delayed_orig = delayed_sample;
const outputSample newest_orig = _output_new;
if (time_delayed_us != delayed_sample.time_us) {
printf("output predictor: BAD RESET TIME!\n");
}
#endif // OUTPUT_PREDICTOR_DEBUG_VEROSE
const float delta_z = new_vert_pos - delayed_sample.pos(2); // vertical position
const float delta_z_alt = new_vert_pos - delayed_sample.vert_vel_integ; // vertical position (alternative)
@ -160,23 +266,36 @@ void OutputPredictor::resetVerticalPositionTo(const uint64_t &time_delayed_us, c
// apply change to latest
_output_new.pos(2) += delta_z;
_output_new.vert_vel_integ += delta_z_alt;
_pos_err_integ(2) = 0.f;
_reset_position_z = false;
#if defined(OUTPUT_PREDICTOR_DEBUG_VEROSE)
printf("output predictor: reset z, delayed (%llu):%.1f->%.1f, newest (%llu):%.1f->%.1f\n",
time_delayed_us, (double)delayed_orig.pos(2), (double)delayed_sample.pos(2),
_output_new.time_us, (double)newest_orig.pos(2), (double)_output_new.pos(2)
);
//printf("output predictor: %llu reset z newest %.1f->%.1f\n", _output_new.time_us, (double)newest_orig.pos(2), (double)_output_new.pos(2));
#endif // OUTPUT_PREDICTOR_DEBUG_VEROSE
}
bool OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector3f &delta_angle,
const float delta_angle_dt, const Vector3f &delta_velocity, const float delta_velocity_dt)
{
if (time_us <= _time_last_update_states_us) {
if (time_us <= _output_new.time_us) {
reset();
return false;
}
// Use full rate IMU data at the current time horizon
if (_time_last_update_states_us != 0) {
const float dt = math::constrain((time_us - _time_last_update_states_us) * 1e-6f, 0.0001f, 0.03f);
if (_output_new.time_us != 0) {
const float dt = math::constrain((time_us - _output_new.time_us) * 1e-6f, 0.0001f, 0.03f);
_dt_update_states_avg = 0.8f * _dt_update_states_avg + 0.2f * dt;
}
_time_last_update_states_us = time_us;
_output_new.time_us = time_us;
// correct delta angle and delta velocity for bias offsets
// Apply corrections to the delta angle required to track the quaternion states at the EKF fusion time horizon
@ -186,8 +305,6 @@ bool OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector
const Vector3f delta_vel_bias_scaled = _accel_bias * delta_velocity_dt;
const Vector3f delta_velocity_corrected(delta_velocity - delta_vel_bias_scaled);
_output_new.time_us = time_us;
const Quatf dq(AxisAnglef{delta_angle_corrected});
// rotate the previous INS quaternion by the delta quaternions
@ -276,23 +393,29 @@ bool OutputPredictor::correctOutputStates(const uint64_t &time_delayed_us,
if (!_output_filter_aligned) {
if (delayed_index_found) {
resetQuaternion(time_delayed_us, quat_state);
resetQuaternionTo(time_delayed_us, quat_state);
resetHorizontalVelocityTo(time_delayed_us, vel_state.xy());
resetVerticalVelocityTo(time_delayed_us, vel_state(2));
resetHorizontalPositionTo(time_delayed_us, pos_state.xy());
resetVerticalPositionTo(time_delayed_us, pos_state(2));
_delta_angle_corr.zero();
_vel_err_integ.zero();
_pos_err_integ.zero();
_output_tracking_error.zero();
_output_filter_aligned = true;
#if defined(OUTPUT_PREDICTOR_DEBUG_VEROSE)
printf("output predictor: ALIGNED, IMU dt: %.4f, EKF dt: %.4f\n",
(double)_dt_update_states_avg, (double)_dt_correct_states_avg);
#endif // OUTPUT_PREDICTOR_DEBUG_VEROSE
}
return false;
}
if (_reset_quaternion) {
resetQuaternionTo(time_delayed_us, quat_state);
_output_tracking_error(0) = 0.f;
} else {
// calculate the quaternion delta between the INS and EKF quaternions at the EKF fusion time horizon
const Quatf q_error((quat_state.inversed() * output_delayed.quat_nominal).normalized());
@ -310,18 +433,54 @@ bool OutputPredictor::correctOutputStates(const uint64_t &time_delayed_us,
// that will cause the INS to track the EKF quaternions
_delta_angle_corr = delta_ang_error * att_gain;
_output_tracking_error(0) = delta_ang_error.norm();
}
/*
* Loop through the output filter state history and apply the corrections to the velocity and position states.
* This method is too expensive to use for the attitude states due to the quaternion operations required
* but because it eliminates the time delay in the 'correction loop' it allows higher tracking gains
* to be used and reduces tracking error relative to EKF states.
*/
// Complementary filter gains
const float vel_gain = _dt_correct_states_avg / math::constrain(_vel_tau, _dt_correct_states_avg, 10.f);
const float pos_gain = _dt_correct_states_avg / math::constrain(_pos_tau, _dt_correct_states_avg, 10.f);
// velocity
if (_reset_velocity_xy) {
resetHorizontalVelocityTo(time_delayed_us, vel_state.xy());
}
if (_reset_velocity_z) {
resetVerticalVelocityTo(time_delayed_us, vel_state(2));
}
// calculate a velocity correction and apply it to the output state history
const Vector3f vel_err(vel_state - output_delayed.vel);
_vel_err_integ += vel_err;
const float vel_gain = _dt_correct_states_avg / math::constrain(_vel_tau, _dt_correct_states_avg, 10.f); // complementary filter gains
const Vector3f vel_correction = vel_err * vel_gain + _vel_err_integ * sq(vel_gain) * 0.1f;
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
_output_buffer[i].vel += vel_correction;
}
_output_tracking_error(1) = vel_err.norm(); // logging
// position
if (_reset_position_xy) {
resetHorizontalPositionTo(time_delayed_us, pos_state.xy());
}
if (_reset_position_z) {
resetVerticalPositionTo(time_delayed_us, pos_state(2));
}
// calculate a position correction and apply it to the output state history
const Vector3f pos_err(pos_state - output_delayed.pos);
_pos_err_integ += pos_err;
const float pos_gain = _dt_correct_states_avg / math::constrain(_pos_tau, _dt_correct_states_avg, 10.f); // complementary filter gains
const Vector3f pos_correction = pos_err * pos_gain + _pos_err_integ * sq(pos_gain) * 0.1f;
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
_output_buffer[i].pos += pos_correction;
}
_output_tracking_error(2) = pos_err.norm(); // logging
// vertical position alternate
// calculate down velocity and position tracking errors
const float vert_vel_err = (vel_state(2) - output_delayed.vert_vel_alt);
const float vert_vel_integ_err = (pos_state(2) - output_delayed.vert_vel_integ);
@ -332,22 +491,6 @@ bool OutputPredictor::correctOutputStates(const uint64_t &time_delayed_us,
applyCorrectionToVerticalOutputBuffer(vert_vel_correction);
// calculate velocity and position tracking errors
const Vector3f vel_err(vel_state - output_delayed.vel);
const Vector3f pos_err(pos_state - output_delayed.pos);
_output_tracking_error(1) = vel_err.norm();
_output_tracking_error(2) = pos_err.norm();
// calculate a velocity correction that will be applied to the output state history
_vel_err_integ += vel_err;
const Vector3f vel_correction = vel_err * vel_gain + _vel_err_integ * sq(vel_gain) * 0.1f;
// calculate a position correction that will be applied to the output state history
_pos_err_integ += pos_err;
const Vector3f pos_correction = pos_err * pos_gain + _pos_err_integ * sq(pos_gain) * 0.1f;
applyCorrectionToOutputBuffer(vel_correction, pos_correction);
// update output state to corrected values and
@ -386,15 +529,3 @@ void OutputPredictor::applyCorrectionToVerticalOutputBuffer(float vert_vel_corre
index = (index + 1) % size;
}
}
void OutputPredictor::applyCorrectionToOutputBuffer(const Vector3f &vel_correction, const Vector3f &pos_correction)
{
// loop through the output filter state history and apply the corrections to the velocity and position states
for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
// a constant velocity correction is applied
_output_buffer[index].vel += vel_correction;
// a constant position correction is applied
_output_buffer[index].pos += pos_correction;
}
}

22
src/modules/ekf2/EKF/output_predictor.h
View File

@ -69,7 +69,7 @@ public:
const matrix::Vector3f &vel_state, const matrix::Vector3f &pos_state,
const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias);
void resetQuaternion(const uint64_t &time_delayed_us, const matrix::Quatf &new_quat);
void resetQuaternionTo(const uint64_t &time_delayed_us, const matrix::Quatf &new_quat);
void resetHorizontalVelocityTo(const uint64_t &time_delayed_us, const matrix::Vector2f &new_horz_vel);
void resetVerticalVelocityTo(const uint64_t &time_delayed_us, const float new_vert_vel);
@ -77,6 +77,12 @@ public:
void resetHorizontalPositionTo(const uint64_t &time_delayed_us, const matrix::Vector2f &new_horz_pos);
void resetVerticalPositionTo(const uint64_t &time_delayed_us, const float new_vert_pos);
void resetQuaternion() { _reset_quaternion = true; }
void resetHorizontalVelocity() { _reset_velocity_xy = true; }
void resetVerticalVelocity() { _reset_velocity_z = true; }
void resetHorizontalPosition() { _reset_position_xy = true; }
void resetVerticalPosition() { _reset_position_z = true; }
void print_status();
bool allocate(uint8_t size)
@ -136,13 +142,6 @@ private:
*/
void applyCorrectionToVerticalOutputBuffer(float vert_vel_correction);
/*
* Calculate corrections to be applied to vel and pos output state history.
* The vel and pos state history are corrected individually so they track the EKF states at
* the fusion time horizon. This option provides the most accurate tracking of EKF states.
*/
void applyCorrectionToOutputBuffer(const matrix::Vector3f &vel_correction, const matrix::Vector3f &pos_correction);
// return the square of two floating point numbers - used in auto coded sections
static constexpr float sq(float var) { return var * var; }
@ -183,7 +182,6 @@ private:
float _dt_update_states_avg{0.005f}; // average imu update period in s
float _dt_correct_states_avg{0.010f}; // average update rate of the ekf in s
uint64_t _time_last_update_states_us{0}; ///< last time the output states were updated (uSec)
uint64_t _time_last_correct_states_us{0}; ///< last time the output states were updated (uSec)
// Output Predictor
@ -197,6 +195,12 @@ private:
matrix::Vector3f _vel_err_integ{}; ///< integral of velocity tracking error (m)
matrix::Vector3f _pos_err_integ{}; ///< integral of position tracking error (m.s)
bool _reset_quaternion{true};
bool _reset_velocity_xy{true};
bool _reset_velocity_z{true};
bool _reset_position_xy{true};
bool _reset_position_z{true};
matrix::Vector3f _output_tracking_error{}; ///< contains the magnitude of the angle, velocity and position track errors (rad, m/s, m)
bool _output_filter_aligned{false};

23
src/modules/ekf2/EKF2.cpp
View File

@ -758,12 +758,19 @@ void EKF2::Run()
perf_set_elapsed(_ecl_ekf_update_full_perf, hrt_elapsed_time(&ekf_update_start));
if (_ekf.output_predictor().aligned()) {
// publish output predictor output
// publish output predictor
PublishLocalPosition(now);
PublishOdometry(now, imu_sample_new);
PublishGlobalPosition(now);
}
// update sensor calibration (in-run bias) before publishing sensor bias
UpdateAccelCalibration(now);
UpdateGyroCalibration(now);
#if defined(CONFIG_EKF2_MAGNETOMETER)
UpdateMagCalibration(now);
#endif // CONFIG_EKF2_MAGNETOMETER
// publish other state output used by the system not dependent on output predictor
PublishSensorBias(now);
@ -773,13 +780,15 @@ void EKF2::Run()
// publish status/logging messages
PublishEventFlags(now);
PublishStatus(now);
PublishStatusFlags(now);
// verbose logging
PublishAidSourceStatus(now);
PublishInnovations(now);
PublishInnovationTestRatios(now);
PublishInnovationVariances(now);
PublishStates(now);
PublishStatus(now);
PublishStatusFlags(now);
PublishAidSourceStatus(now);
#if defined(CONFIG_EKF2_BAROMETER)
PublishBaroBias(now);
@ -803,12 +812,6 @@ void EKF2::Run()
PublishOpticalFlowVel(now);
#endif // CONFIG_EKF2_OPTICAL_FLOW
UpdateAccelCalibration(now);
UpdateGyroCalibration(now);
#if defined(CONFIG_EKF2_MAGNETOMETER)
UpdateMagCalibration(now);
#endif // CONFIG_EKF2_MAGNETOMETER
} else {
// ekf no update
perf_set_elapsed(_ecl_ekf_update_perf, hrt_elapsed_time(&ekf_update_start));