mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-28 02:20:34 +08:00
ECL: Clean velPos logging, deprecate ekf2_innovations msg
This commit is contained in:
kamilritz
@@ -38,7 +38,7 @@
|
||||
|
||||
#include "PreFlightChecker.hpp"
|
||||
|
||||
void PreFlightChecker::update(const float dt, const ekf2_innovations_s &innov)
|
||||
void PreFlightChecker::update(const float dt, const estimator_innovations_s &innov)
|
||||
{
|
||||
const float alpha = InnovationLpf::computeAlphaFromDtAndTauInv(dt, _innov_lpf_tau_inv);
|
||||
|
||||
@@ -48,14 +48,14 @@ void PreFlightChecker::update(const float dt, const ekf2_innovations_s &innov)
|
||||
_has_height_failed = preFlightCheckHeightFailed(innov, alpha);
|
||||
}
|
||||
|
||||
bool PreFlightChecker::preFlightCheckHeadingFailed(const ekf2_innovations_s &innov, const float alpha)
|
||||
bool PreFlightChecker::preFlightCheckHeadingFailed(const estimator_innovations_s &innov, const float alpha)
|
||||
{
|
||||
const float heading_test_limit = selectHeadingTestLimit();
|
||||
const float heading_innov_spike_lim = 2.0f * heading_test_limit;
|
||||
|
||||
const float heading_innov_lpf = _filter_heading_innov.update(innov.heading_innov, alpha, heading_innov_spike_lim);
|
||||
const float heading_innov_lpf = _filter_heading_innov.update(innov.heading, alpha, heading_innov_spike_lim);
|
||||
|
||||
return checkInnovFailed(innov.heading_innov, heading_innov_lpf, heading_test_limit);
|
||||
return checkInnovFailed(innov.heading, heading_innov_lpf, heading_test_limit);
|
||||
}
|
||||
|
||||
float PreFlightChecker::selectHeadingTestLimit()
|
||||
@@ -69,12 +69,13 @@ float PreFlightChecker::selectHeadingTestLimit()
|
||||
: _heading_innov_test_lim; // less restrictive test limit
|
||||
}
|
||||
|
||||
bool PreFlightChecker::preFlightCheckHorizVelFailed(const ekf2_innovations_s &innov, const float alpha)
|
||||
bool PreFlightChecker::preFlightCheckHorizVelFailed(const estimator_innovations_s &innov, const float alpha)
|
||||
{
|
||||
bool has_failed = false;
|
||||
|
||||
if (_is_using_gps_aiding || _is_using_ev_pos_aiding) {
|
||||
const Vector2f vel_ne_innov = Vector2f(innov.vel_pos_innov);
|
||||
if (_is_using_gps_aiding || _is_using_ev_vel_aiding) {
|
||||
const Vector2f vel_ne_innov = Vector2f(fmaxf(fabsf(innov.gps_hvel[0]), fabsf(innov.ev_hvel[0])),
|
||||
fmaxf(fabsf(innov.gps_hvel[1]), fabsf(innov.ev_hvel[1])));
|
||||
Vector2f vel_ne_innov_lpf;
|
||||
vel_ne_innov_lpf(0) = _filter_vel_n_innov.update(vel_ne_innov(0), alpha, _vel_innov_spike_lim);
|
||||
vel_ne_innov_lpf(1) = _filter_vel_n_innov.update(vel_ne_innov(1), alpha, _vel_innov_spike_lim);
|
||||
@@ -82,7 +83,7 @@ bool PreFlightChecker::preFlightCheckHorizVelFailed(const ekf2_innovations_s &in
|
||||
}
|
||||
|
||||
if (_is_using_flow_aiding) {
|
||||
const Vector2f flow_innov = Vector2f(innov.flow_innov);
|
||||
const Vector2f flow_innov = Vector2f(innov.flow);
|
||||
Vector2f flow_innov_lpf;
|
||||
flow_innov_lpf(0) = _filter_flow_x_innov.update(flow_innov(0), alpha, _flow_innov_spike_lim);
|
||||
flow_innov_lpf(1) = _filter_flow_x_innov.update(flow_innov(1), alpha, _flow_innov_spike_lim);
|
||||
@@ -92,16 +93,16 @@ bool PreFlightChecker::preFlightCheckHorizVelFailed(const ekf2_innovations_s &in
|
||||
return has_failed;
|
||||
}
|
||||
|
||||
bool PreFlightChecker::preFlightCheckVertVelFailed(const ekf2_innovations_s &innov, const float alpha)
|
||||
bool PreFlightChecker::preFlightCheckVertVelFailed(const estimator_innovations_s &innov, const float alpha)
|
||||
{
|
||||
const float vel_d_innov = innov.vel_pos_innov[2];
|
||||
const float vel_d_innov = fmaxf(fabsf(innov.gps_vvel), fabs(innov.ev_vvel));
|
||||
const float vel_d_innov_lpf = _filter_vel_d_innov.update(vel_d_innov, alpha, _vel_innov_spike_lim);
|
||||
return checkInnovFailed(vel_d_innov, vel_d_innov_lpf, _vel_innov_test_lim);
|
||||
}
|
||||
|
||||
bool PreFlightChecker::preFlightCheckHeightFailed(const ekf2_innovations_s &innov, const float alpha)
|
||||
bool PreFlightChecker::preFlightCheckHeightFailed(const estimator_innovations_s &innov, const float alpha)
|
||||
{
|
||||
const float hgt_innov = innov.vel_pos_innov[5];
|
||||
const float hgt_innov = innov.gps_vpos; // HACK: height innovation independent of sensor is published on gps_vpos
|
||||
const float hgt_innov_lpf = _filter_hgt_innov.update(hgt_innov, alpha, _hgt_innov_spike_lim);
|
||||
return checkInnovFailed(hgt_innov, hgt_innov_lpf, _hgt_innov_test_lim);
|
||||
}
|
||||
@@ -122,6 +123,7 @@ void PreFlightChecker::reset()
|
||||
_is_using_gps_aiding = false;
|
||||
_is_using_flow_aiding = false;
|
||||
_is_using_ev_pos_aiding = false;
|
||||
_is_using_ev_vel_aiding = false;
|
||||
_has_heading_failed = false;
|
||||
_has_horiz_vel_failed = false;
|
||||
_has_vert_vel_failed = false;
|
||||
|
||||
@@ -42,7 +42,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/ekf2_innovations.h>
|
||||
#include <uORB/topics/estimator_innovations.h>
|
||||
|
||||
#include <matrix/matrix/math.hpp>
|
||||
|
||||
@@ -66,7 +66,7 @@ public:
|
||||
* @param dt the sampling time
|
||||
* @param innov the ekf2_innovation_s struct containing the current innovations
|
||||
*/
|
||||
void update(float dt, const ekf2_innovations_s &innov);
|
||||
void update(float dt, const estimator_innovations_s &innov);
|
||||
|
||||
/*
|
||||
* If set to true, the checker will use a less conservative heading innovation check
|
||||
@@ -76,6 +76,7 @@ public:
|
||||
void setUsingGpsAiding(bool val) { _is_using_gps_aiding = val; }
|
||||
void setUsingFlowAiding(bool val) { _is_using_flow_aiding = val; }
|
||||
void setUsingEvPosAiding(bool val) { _is_using_ev_pos_aiding = val; }
|
||||
void setUsingEvVelAiding(bool val) { _is_using_ev_vel_aiding = val; }
|
||||
|
||||
bool hasHeadingFailed() const { return _has_heading_failed; }
|
||||
bool hasHorizVelFailed() const { return _has_horiz_vel_failed; }
|
||||
@@ -127,12 +128,12 @@ public:
|
||||
static constexpr float sq(float var) { return var * var; }
|
||||
|
||||
private:
|
||||
bool preFlightCheckHeadingFailed(const ekf2_innovations_s &innov, float alpha);
|
||||
bool preFlightCheckHeadingFailed(const estimator_innovations_s &innov, float alpha);
|
||||
float selectHeadingTestLimit();
|
||||
|
||||
bool preFlightCheckHorizVelFailed(const ekf2_innovations_s &innov, float alpha);
|
||||
bool preFlightCheckVertVelFailed(const ekf2_innovations_s &innov, float alpha);
|
||||
bool preFlightCheckHeightFailed(const ekf2_innovations_s &innov, float alpha);
|
||||
bool preFlightCheckHorizVelFailed(const estimator_innovations_s &innov, float alpha);
|
||||
bool preFlightCheckVertVelFailed(const estimator_innovations_s &innov, float alpha);
|
||||
bool preFlightCheckHeightFailed(const estimator_innovations_s &innov, float alpha);
|
||||
|
||||
void resetPreFlightChecks();
|
||||
|
||||
@@ -145,6 +146,7 @@ private:
|
||||
bool _is_using_gps_aiding{};
|
||||
bool _is_using_flow_aiding{};
|
||||
bool _is_using_ev_pos_aiding{};
|
||||
bool _is_using_ev_vel_aiding{};
|
||||
|
||||
// Low-pass filters for innovation pre-flight checks
|
||||
InnovationLpf _filter_vel_n_innov; ///< Preflight low pass filter N axis velocity innovations (m/sec)
|
||||
|
||||
@@ -56,7 +56,7 @@
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/distance_sensor.h>
|
||||
#include <uORB/topics/ekf2_innovations.h>
|
||||
#include <uORB/topics/estimator_innovations.h>
|
||||
#include <uORB/topics/ekf2_timestamps.h>
|
||||
#include <uORB/topics/ekf_gps_position.h>
|
||||
#include <uORB/topics/estimator_status.h>
|
||||
@@ -120,7 +120,7 @@ private:
|
||||
PreFlightChecker _preflt_checker;
|
||||
void runPreFlightChecks(float dt, const filter_control_status_u &control_status,
|
||||
const vehicle_status_s &vehicle_status,
|
||||
const ekf2_innovations_s &innov);
|
||||
const estimator_innovations_s &innov);
|
||||
void resetPreFlightChecks();
|
||||
|
||||
template<typename Param>
|
||||
@@ -267,7 +267,9 @@ private:
|
||||
vehicle_land_detected_s _vehicle_land_detected{};
|
||||
vehicle_status_s _vehicle_status{};
|
||||
|
||||
uORB::Publication<ekf2_innovations_s> _estimator_innovations_pub{ORB_ID(ekf2_innovations)};
|
||||
uORB::Publication<estimator_innovations_s> _estimator_innovations_pub{ORB_ID(estimator_innovations)};
|
||||
uORB::Publication<estimator_innovations_s> _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
|
||||
uORB::Publication<estimator_innovations_s> _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
|
||||
uORB::Publication<ekf2_timestamps_s> _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
|
||||
uORB::Publication<ekf_gps_drift_s> _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)};
|
||||
uORB::Publication<ekf_gps_position_s> _blended_gps_pub{ORB_ID(ekf_gps_position)};
|
||||
@@ -1484,16 +1486,17 @@ void Ekf2::Run()
|
||||
_ekf.get_state_delayed(status.states);
|
||||
status.n_states = 24;
|
||||
_ekf.covariances_diagonal().copyTo(status.covariances);
|
||||
_ekf.get_output_tracking_error(&status.output_tracking_error[0]);
|
||||
_ekf.get_gps_check_status(&status.gps_check_fail_flags);
|
||||
// only report enabled GPS check failures (the param indexes are shifted by 1 bit, because they don't include
|
||||
// the GPS Fix bit, which is always checked)
|
||||
status.gps_check_fail_flags &= ((uint16_t)_params->gps_check_mask << 1) | 1;
|
||||
status.control_mode_flags = control_status.value;
|
||||
_ekf.get_filter_fault_status(&status.filter_fault_flags);
|
||||
_ekf.get_innovation_test_status(&status.innovation_check_flags, &status.mag_test_ratio,
|
||||
&status.vel_test_ratio, &status.pos_test_ratio,
|
||||
&status.hgt_test_ratio, &status.tas_test_ratio,
|
||||
&status.hagl_test_ratio, &status.beta_test_ratio);
|
||||
_ekf.get_innovation_test_status(status.innovation_check_flags, status.mag_test_ratio,
|
||||
status.vel_test_ratio, status.pos_test_ratio,
|
||||
status.hgt_test_ratio, status.tas_test_ratio,
|
||||
status.hagl_test_ratio, status.beta_test_ratio);
|
||||
|
||||
status.pos_horiz_accuracy = _vehicle_local_position_pub.get().eph;
|
||||
status.pos_vert_accuracy = _vehicle_local_position_pub.get().epv;
|
||||
@@ -1602,28 +1605,51 @@ void Ekf2::Run()
|
||||
|
||||
{
|
||||
// publish estimator innovation data
|
||||
ekf2_innovations_s innovations;
|
||||
estimator_innovations_s innovations;
|
||||
innovations.timestamp = now;
|
||||
_ekf.get_vel_pos_innov(&innovations.vel_pos_innov[0]);
|
||||
_ekf.get_aux_vel_innov(&innovations.aux_vel_innov[0]);
|
||||
_ekf.get_mag_innov(&innovations.mag_innov[0]);
|
||||
_ekf.get_heading_innov(&innovations.heading_innov);
|
||||
_ekf.get_airspeed_innov(&innovations.airspeed_innov);
|
||||
_ekf.get_beta_innov(&innovations.beta_innov);
|
||||
_ekf.get_flow_innov(&innovations.flow_innov[0]);
|
||||
_ekf.get_hagl_innov(&innovations.hagl_innov);
|
||||
_ekf.get_drag_innov(&innovations.drag_innov[0]);
|
||||
_ekf.getGpsVelPosInnov(&innovations.gps_hvel[0], innovations.gps_vvel, &innovations.gps_hpos[0],
|
||||
innovations.gps_vpos);
|
||||
_ekf.getEvVelPosInnov(&innovations.ev_hvel[0], innovations.ev_vvel, &innovations.ev_hpos[0], innovations.ev_vpos);
|
||||
_ekf.getAuxVelInnov(&innovations.aux_hvel[0]);
|
||||
_ekf.getFlowInnov(&innovations.flow[0]);
|
||||
_ekf.getHeadingInnov(innovations.heading);
|
||||
_ekf.getMagInnov(innovations.mag_field);
|
||||
_ekf.getDragInnov(&innovations.drag[0]);
|
||||
_ekf.getAirspeedInnov(innovations.airspeed);
|
||||
_ekf.getBetaInnov(innovations.beta);
|
||||
_ekf.getHaglInnov(innovations.hagl);
|
||||
|
||||
_ekf.get_vel_pos_innov_var(&innovations.vel_pos_innov_var[0]);
|
||||
_ekf.get_mag_innov_var(&innovations.mag_innov_var[0]);
|
||||
_ekf.get_heading_innov_var(&innovations.heading_innov_var);
|
||||
_ekf.get_airspeed_innov_var(&innovations.airspeed_innov_var);
|
||||
_ekf.get_beta_innov_var(&innovations.beta_innov_var);
|
||||
_ekf.get_flow_innov_var(&innovations.flow_innov_var[0]);
|
||||
_ekf.get_hagl_innov_var(&innovations.hagl_innov_var);
|
||||
_ekf.get_drag_innov_var(&innovations.drag_innov_var[0]);
|
||||
// publish estimator innovation variance data
|
||||
estimator_innovations_s innovation_var;
|
||||
innovation_var.timestamp = now;
|
||||
_ekf.getGpsVelPosInnovVar(&innovation_var.gps_hvel[0], innovation_var.gps_vvel, &innovation_var.gps_hpos[0],
|
||||
innovation_var.gps_vpos);
|
||||
_ekf.getEvVelPosInnovVar(&innovation_var.ev_hvel[0], innovation_var.ev_vvel, &innovation_var.ev_hpos[0],
|
||||
innovation_var.ev_vpos);
|
||||
_ekf.getAuxVelInnovVar(&innovation_var.aux_hvel[0]);
|
||||
_ekf.getFlowInnovVar(&innovation_var.flow[0]);
|
||||
_ekf.getHeadingInnovVar(innovation_var.heading);
|
||||
_ekf.getMagInnovVar(&innovation_var.mag_field[0]);
|
||||
_ekf.getDragInnovVar(&innovation_var.drag[0]);
|
||||
_ekf.getAirspeedInnovVar(innovation_var.airspeed);
|
||||
_ekf.getBetaInnovVar(innovation_var.beta);
|
||||
_ekf.getHaglInnovVar(innovation_var.hagl);
|
||||
|
||||
_ekf.get_output_tracking_error(&innovations.output_tracking_error[0]);
|
||||
// publish estimator innovation test ratio data
|
||||
estimator_innovations_s test_ratios;
|
||||
test_ratios.timestamp = now;
|
||||
_ekf.getGpsVelPosInnovRatio(test_ratios.gps_hvel[0], test_ratios.gps_vvel, test_ratios.gps_hpos[0],
|
||||
test_ratios.gps_vpos);
|
||||
_ekf.getEvVelPosInnovRatio(test_ratios.ev_hvel[0], test_ratios.ev_vvel, test_ratios.ev_hpos[0],
|
||||
test_ratios.ev_vpos);
|
||||
_ekf.getAuxVelInnovRatio(test_ratios.aux_hvel[0]);
|
||||
_ekf.getFlowInnovRatio(test_ratios.flow[0]);
|
||||
_ekf.getHeadingInnovRatio(test_ratios.heading);
|
||||
_ekf.getMagInnovRatio(test_ratios.mag_field[0]);
|
||||
_ekf.getDragInnovRatio(&test_ratios.drag[0]);
|
||||
_ekf.getAirspeedInnovRatio(test_ratios.airspeed);
|
||||
_ekf.getBetaInnovRatio(test_ratios.beta);
|
||||
_ekf.getHaglInnovRatio(test_ratios.hagl);
|
||||
|
||||
// calculate noise filtered velocity innovations which are used for pre-flight checking
|
||||
if (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
|
||||
@@ -1635,6 +1661,9 @@ void Ekf2::Run()
|
||||
}
|
||||
|
||||
_estimator_innovations_pub.publish(innovations);
|
||||
_estimator_innovation_variances_pub.publish(innovation_var);
|
||||
_estimator_innovation_test_ratios_pub.publish(test_ratios);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1667,7 +1696,7 @@ void Ekf2::fillGpsMsgWithVehicleGpsPosData(gps_message &msg, const vehicle_gps_p
|
||||
void Ekf2::runPreFlightChecks(const float dt,
|
||||
const filter_control_status_u &control_status,
|
||||
const vehicle_status_s &vehicle_status,
|
||||
const ekf2_innovations_s &innov)
|
||||
const estimator_innovations_s &innov)
|
||||
{
|
||||
const bool can_observe_heading_in_flight = (vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
|
||||
|
||||
@@ -1675,6 +1704,7 @@ void Ekf2::runPreFlightChecks(const float dt,
|
||||
_preflt_checker.setUsingGpsAiding(control_status.flags.gps);
|
||||
_preflt_checker.setUsingFlowAiding(control_status.flags.opt_flow);
|
||||
_preflt_checker.setUsingEvPosAiding(control_status.flags.ev_pos);
|
||||
_preflt_checker.setUsingEvVelAiding(control_status.flags.ev_vel);
|
||||
|
||||
_preflt_checker.update(dt, innov);
|
||||
}
|
||||
@@ -1736,10 +1766,10 @@ bool Ekf2::publish_wind_estimate(const hrt_abstime ×tamp)
|
||||
float wind_var[2];
|
||||
_ekf.get_wind_velocity(velNE_wind);
|
||||
_ekf.get_wind_velocity_var(wind_var);
|
||||
_ekf.get_airspeed_innov(&wind_estimate.tas_innov);
|
||||
_ekf.get_airspeed_innov_var(&wind_estimate.tas_innov_var);
|
||||
_ekf.get_beta_innov(&wind_estimate.beta_innov);
|
||||
_ekf.get_beta_innov_var(&wind_estimate.beta_innov_var);
|
||||
_ekf.getAirspeedInnov(wind_estimate.tas_innov);
|
||||
_ekf.getAirspeedInnovVar(wind_estimate.tas_innov_var);
|
||||
_ekf.getBetaInnov(wind_estimate.beta_innov);
|
||||
_ekf.getBetaInnovVar(wind_estimate.beta_innov_var);
|
||||
wind_estimate.timestamp = timestamp;
|
||||
wind_estimate.windspeed_north = velNE_wind[0];
|
||||
wind_estimate.windspeed_east = velNE_wind[1];
|
||||
|
||||
@@ -493,6 +493,8 @@ void BlockLocalPositionEstimator::Run()
|
||||
publishEstimatorStatus();
|
||||
_pub_innov.get().timestamp = _timeStamp;
|
||||
_pub_innov.update();
|
||||
_pub_innov_var.get().timestamp = _timeStamp;
|
||||
_pub_innov_var.update();
|
||||
|
||||
if ((_estimatorInitialized & EST_XY) && (_map_ref.init_done || _param_lpe_fake_origin.get())) {
|
||||
publishGlobalPos();
|
||||
|
||||
@@ -34,7 +34,7 @@
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/estimator_status.h>
|
||||
#include <uORB/topics/ekf2_innovations.h>
|
||||
#include <uORB/topics/estimator_innovations.h>
|
||||
|
||||
using namespace matrix;
|
||||
using namespace control;
|
||||
@@ -285,7 +285,8 @@ private:
|
||||
uORB::PublicationData<vehicle_global_position_s> _pub_gpos{ORB_ID(vehicle_global_position)};
|
||||
uORB::PublicationData<vehicle_odometry_s> _pub_odom{ORB_ID(vehicle_odometry)};
|
||||
uORB::PublicationData<estimator_status_s> _pub_est_status{ORB_ID(estimator_status)};
|
||||
uORB::PublicationData<ekf2_innovations_s> _pub_innov{ORB_ID(ekf2_innovations)};
|
||||
uORB::PublicationData<estimator_innovations_s> _pub_innov{ORB_ID(estimator_innovations)};
|
||||
uORB::PublicationData<estimator_innovations_s> _pub_innov_var{ORB_ID(estimator_innovation_variances)};
|
||||
|
||||
// map projection
|
||||
struct map_projection_reference_s _map_ref;
|
||||
|
||||
@@ -172,10 +172,10 @@ void BlockLocalPositionEstimator::flowCorrect()
|
||||
Matrix<float, n_y_flow, n_y_flow> S = C * m_P * C.transpose() + R;
|
||||
|
||||
// publish innovations
|
||||
_pub_innov.get().flow_innov[0] = r(0);
|
||||
_pub_innov.get().flow_innov[1] = r(1);
|
||||
_pub_innov.get().flow_innov_var[0] = S(0, 0);
|
||||
_pub_innov.get().flow_innov_var[1] = S(1, 1);
|
||||
_pub_innov.get().flow[0] = r(0);
|
||||
_pub_innov.get().flow[1] = r(1);
|
||||
_pub_innov_var.get().flow[0] = S(0, 0);
|
||||
_pub_innov_var.get().flow[1] = S(1, 1);
|
||||
|
||||
// residual covariance, (inverse)
|
||||
Matrix<float, n_y_flow, n_y_flow> S_I = inv<float, n_y_flow>(S);
|
||||
|
||||
@@ -189,10 +189,20 @@ void BlockLocalPositionEstimator::gpsCorrect()
|
||||
Matrix<float, n_y_gps, n_y_gps> S = C * m_P * C.transpose() + R;
|
||||
|
||||
// publish innovations
|
||||
for (size_t i = 0; i < 6; i++) {
|
||||
_pub_innov.get().vel_pos_innov[i] = r(i);
|
||||
_pub_innov.get().vel_pos_innov_var[i] = S(i, i);
|
||||
}
|
||||
_pub_innov.get().gps_hvel[0] = r(3);
|
||||
_pub_innov.get().gps_hvel[1] = r(4);
|
||||
_pub_innov.get().gps_vvel = r(5);
|
||||
_pub_innov.get().gps_hpos[0] = r(0);
|
||||
_pub_innov.get().gps_hpos[1] = r(1);
|
||||
_pub_innov.get().gps_vpos = r(2);
|
||||
|
||||
// publish innovation variances
|
||||
_pub_innov_var.get().gps_hvel[0] = S(3, 3);
|
||||
_pub_innov_var.get().gps_hvel[1] = S(4, 4);
|
||||
_pub_innov_var.get().gps_vvel = S(5, 5);
|
||||
_pub_innov_var.get().gps_hpos[0] = S(0, 0);
|
||||
_pub_innov_var.get().gps_hpos[1] = S(1, 1);
|
||||
_pub_innov_var.get().gps_vpos = S(2, 2);
|
||||
|
||||
// residual covariance, (inverse)
|
||||
Matrix<float, n_y_gps, n_y_gps> S_I = inv<float, n_y_gps>(S);
|
||||
|
||||
@@ -61,8 +61,8 @@ void BlockLocalPositionEstimator::landCorrect()
|
||||
// residual
|
||||
Matrix<float, n_y_land, n_y_land> S_I = inv<float, n_y_land>((C * m_P * C.transpose()) + R);
|
||||
Vector<float, n_y_land> r = y - C * _x;
|
||||
_pub_innov.get().hagl_innov = r(Y_land_agl);
|
||||
_pub_innov.get().hagl_innov_var = R(Y_land_agl, Y_land_agl);
|
||||
_pub_innov.get().hagl = r(Y_land_agl);
|
||||
_pub_innov_var.get().hagl = R(Y_land_agl, Y_land_agl);
|
||||
|
||||
// fault detection
|
||||
float beta = (r.transpose() * (S_I * r))(0, 0);
|
||||
|
||||
@@ -92,8 +92,8 @@ void BlockLocalPositionEstimator::lidarCorrect()
|
||||
Matrix<float, n_y_lidar, n_y_lidar> S = C * m_P * C.transpose() + R;
|
||||
|
||||
// publish innovations
|
||||
_pub_innov.get().hagl_innov = r(0);
|
||||
_pub_innov.get().hagl_innov_var = S(0, 0);
|
||||
_pub_innov.get().hagl = r(0);
|
||||
_pub_innov_var.get().hagl = S(0, 0);
|
||||
|
||||
// residual covariance, (inverse)
|
||||
Matrix<float, n_y_lidar, n_y_lidar> S_I = inv<float, n_y_lidar>(S);
|
||||
|
||||
@@ -145,15 +145,20 @@ void BlockLocalPositionEstimator::mocapCorrect()
|
||||
Matrix<float, n_y_mocap, n_y_mocap> S = C * m_P * C.transpose() + R;
|
||||
|
||||
// publish innovations
|
||||
for (size_t i = 0; i < 3; i++) {
|
||||
_pub_innov.get().vel_pos_innov[i] = r(i);
|
||||
_pub_innov.get().vel_pos_innov_var[i] = S(i, i);
|
||||
}
|
||||
_pub_innov.get().ev_hvel[0] = NAN;
|
||||
_pub_innov.get().ev_hvel[1] = NAN;
|
||||
_pub_innov.get().ev_vvel = NAN;
|
||||
_pub_innov.get().ev_hpos[0] = r(0);
|
||||
_pub_innov.get().ev_hpos[1] = r(1);
|
||||
_pub_innov.get().ev_vpos = r(2);
|
||||
|
||||
for (size_t i = 3; i < 6; i++) {
|
||||
_pub_innov.get().vel_pos_innov[i] = 0;
|
||||
_pub_innov.get().vel_pos_innov_var[i] = 1;
|
||||
}
|
||||
// publish innovation variances
|
||||
_pub_innov_var.get().ev_hvel[0] = NAN;
|
||||
_pub_innov_var.get().ev_hvel[1] = NAN;
|
||||
_pub_innov_var.get().ev_vvel = NAN;
|
||||
_pub_innov_var.get().ev_hpos[0] = S(0, 0);
|
||||
_pub_innov_var.get().ev_hpos[1] = S(1, 1);
|
||||
_pub_innov_var.get().ev_vpos = S(2, 2);
|
||||
|
||||
// residual covariance, (inverse)
|
||||
Matrix<float, n_y_mocap, n_y_mocap> S_I = inv<float, n_y_mocap>(S);
|
||||
|
||||
@@ -112,8 +112,8 @@ void BlockLocalPositionEstimator::sonarCorrect()
|
||||
Matrix<float, n_y_sonar, n_y_sonar> S = C * m_P * C.transpose() + R;
|
||||
|
||||
// publish innovations
|
||||
_pub_innov.get().hagl_innov = r(0);
|
||||
_pub_innov.get().hagl_innov_var = S(0, 0);
|
||||
_pub_innov.get().hagl = r(0);
|
||||
_pub_innov_var.get().hagl = S(0, 0);
|
||||
|
||||
// residual covariance, (inverse)
|
||||
Matrix<float, n_y_sonar, n_y_sonar> S_I = inv<float, n_y_sonar>(S);
|
||||
|
||||
@@ -162,15 +162,20 @@ void BlockLocalPositionEstimator::visionCorrect()
|
||||
Matrix<float, n_y_vision, n_y_vision> S = C * m_P * C.transpose() + R;
|
||||
|
||||
// publish innovations
|
||||
for (size_t i = 0; i < 3; i++) {
|
||||
_pub_innov.get().vel_pos_innov[i] = r(i, 0);
|
||||
_pub_innov.get().vel_pos_innov_var[i] = S(i, i);
|
||||
}
|
||||
_pub_innov.get().ev_hvel[0] = NAN;
|
||||
_pub_innov.get().ev_hvel[1] = NAN;
|
||||
_pub_innov.get().ev_vvel = NAN;
|
||||
_pub_innov.get().ev_hpos[0] = r(0, 0);
|
||||
_pub_innov.get().ev_hpos[1] = r(1, 0);
|
||||
_pub_innov.get().ev_vpos = r(2, 0);
|
||||
|
||||
for (size_t i = 3; i < 6; i++) {
|
||||
_pub_innov.get().vel_pos_innov[i] = 0;
|
||||
_pub_innov.get().vel_pos_innov_var[i] = 1;
|
||||
}
|
||||
// publish innovation variances
|
||||
_pub_innov_var.get().ev_hvel[0] = NAN;
|
||||
_pub_innov_var.get().ev_hvel[1] = NAN;
|
||||
_pub_innov_var.get().ev_vvel = NAN;
|
||||
_pub_innov_var.get().ev_hpos[0] = S(0, 0);
|
||||
_pub_innov_var.get().ev_hpos[1] = S(1, 1);
|
||||
_pub_innov_var.get().ev_vpos = S(2, 2);
|
||||
|
||||
// residual covariance, (inverse)
|
||||
Matrix<float, n_y_vision, n_y_vision> S_I = inv<float, n_y_vision>(S);
|
||||
|
||||
@@ -516,7 +516,9 @@ void Logger::add_default_topics()
|
||||
add_topic("camera_trigger");
|
||||
add_topic("camera_trigger_secondary");
|
||||
add_topic("cpuload");
|
||||
add_topic("ekf2_innovations", 200);
|
||||
add_topic("estimator_innovations", 200);
|
||||
add_topic("estimator_innovation_variances", 200);
|
||||
add_topic("estimator_innovation_test_ratios", 200);
|
||||
add_topic("ekf_gps_drift");
|
||||
add_topic("esc_status", 250);
|
||||
add_topic("estimator_status", 200);
|
||||
|
||||
Reference in New Issue
Block a user