forked from Archive/PX4-Autopilot
LPE Variance Dependent Publication (#4914)
* Use variance to control publishing for LPE. * Don't stop publishing if we have gps/ baro. * LPE tuning and cleanup. * Added bias saturation to LPE. * Added vector enabled low pass filter block. * Added rk4 integration and pub lowpass to LPE. * Fix std::abs issue on mac/ reset lowpass on state reset. * Don't estimate gyro bias when rotating at high speed att_est_q. * Lowered low pass on position to 5 Hz for LPE. * Streamline state space update for LPE. * Added health flags to est2 log. * Revert to old tuning, more conservative, less faults. * Formatting. * Fix for fault message on LPE. * Added subscription throttling to LPE. * Formatting.
This commit is contained in:
James Goppert
GitHub
parent
2a395c3fec
commit
00dfc99e08
|
|
@ -84,6 +84,7 @@ float BlockLowPass::update(float input)
|
|||
return getState();
|
||||
}
|
||||
|
||||
|
||||
float BlockHighPass::update(float input)
|
||||
{
|
||||
float b = 2 * float(M_PI) * getFCut() * getDt();
|
||||
|
|
|
|||
|
|
@ -127,6 +127,45 @@ protected:
|
|||
control::BlockParamFloat _fCut;
|
||||
};
|
||||
|
||||
template<class Type, size_t M>
|
||||
class __EXPORT BlockLowPassVector: public Block
|
||||
{
|
||||
public:
|
||||
// methods
|
||||
BlockLowPassVector(SuperBlock *parent,
|
||||
const char *name) :
|
||||
Block(parent, name),
|
||||
_state(),
|
||||
_fCut(this, "") // only one parameter, no need to name
|
||||
{
|
||||
for (int i = 0; i < M; i++) {
|
||||
_state(i) = 0.0f / 0.0f;
|
||||
}
|
||||
};
|
||||
virtual ~BlockLowPassVector() {};
|
||||
matrix::Vector<Type, M> update(const matrix::Matrix<Type, M, 1> &input)
|
||||
{
|
||||
for (int i = 0; i < M; i++) {
|
||||
if (!PX4_ISFINITE(getState()(i))) {
|
||||
setState(input);
|
||||
}
|
||||
}
|
||||
|
||||
float b = 2 * float(M_PI) * getFCut() * getDt();
|
||||
float a = b / (1 + b);
|
||||
setState(input * a + getState() * (1 - a));
|
||||
return getState();
|
||||
}
|
||||
// accessors
|
||||
matrix::Vector<Type, M> getState() { return _state; }
|
||||
float getFCut() { return _fCut.get(); }
|
||||
void setState(const matrix::Vector<Type, M> &state) { _state = state; }
|
||||
private:
|
||||
// attributes
|
||||
matrix::Vector<Type, M> _state;
|
||||
control::BlockParamFloat _fCut;
|
||||
};
|
||||
|
||||
/**
|
||||
* A high pass filter as described here:
|
||||
* http://en.wikipedia.org/wiki/High-pass_filter.
|
||||
|
|
|
|||
|
|
@ -797,6 +797,7 @@ bool AttitudeEstimatorQ::update(float dt)
|
|||
|
||||
_q.normalize();
|
||||
|
||||
|
||||
// Accelerometer correction
|
||||
// Project 'k' unit vector of earth frame to body frame
|
||||
// Vector<3> k = _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f));
|
||||
|
|
@ -810,7 +811,9 @@ bool AttitudeEstimatorQ::update(float dt)
|
|||
corr += (k % (_accel - _pos_acc).normalized()) * _w_accel;
|
||||
|
||||
// Gyro bias estimation
|
||||
_gyro_bias += corr * (_w_gyro_bias * dt);
|
||||
if (_gyro.length() < 1.0f) {
|
||||
_gyro_bias += corr * (_w_gyro_bias * dt);
|
||||
}
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
_gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max);
|
||||
|
|
|
|||
|
|
@ -3,40 +3,45 @@
|
|||
#include <fcntl.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <matrix/math.hpp>
|
||||
#include <cstdlib>
|
||||
|
||||
orb_advert_t mavlink_log_pub = nullptr;
|
||||
|
||||
// timeouts for sensors in microseconds
|
||||
static const uint32_t EST_SRC_TIMEOUT = 10000; // 0.01 s
|
||||
|
||||
// required standard deviation of estimate for estimator to publish data
|
||||
static const uint32_t EST_STDDEV_XY_VALID = 2.0; // 2.0 m
|
||||
static const uint32_t EST_STDDEV_Z_VALID = 2.0; // 2.0 m
|
||||
static const uint32_t EST_STDDEV_TZ_VALID = 2.0; // 2.0 m
|
||||
|
||||
// minimum flow altitude
|
||||
static const float flow_min_agl = 0.3;
|
||||
|
||||
BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
|
||||
// this block has no parent, and has name LPE
|
||||
SuperBlock(NULL, "LPE"),
|
||||
|
||||
// subscriptions, set rate, add to list
|
||||
// TODO topic speed limiting?
|
||||
_sub_status(ORB_ID(vehicle_status), 0, 0, &getSubscriptions()),
|
||||
_sub_armed(ORB_ID(actuator_armed), 0, 0, &getSubscriptions()),
|
||||
_sub_control_mode(ORB_ID(vehicle_control_mode),
|
||||
0, 0, &getSubscriptions()),
|
||||
_sub_att(ORB_ID(vehicle_attitude), 0, 0, &getSubscriptions()),
|
||||
_sub_att_sp(ORB_ID(vehicle_attitude_setpoint),
|
||||
0, 0, &getSubscriptions()),
|
||||
_sub_flow(ORB_ID(optical_flow), 0, 0, &getSubscriptions()),
|
||||
_sub_sensor(ORB_ID(sensor_combined), 0, 0, &getSubscriptions()),
|
||||
_sub_param_update(ORB_ID(parameter_update), 0, 0, &getSubscriptions()),
|
||||
_sub_manual(ORB_ID(manual_control_setpoint), 0, 0, &getSubscriptions()),
|
||||
_sub_home(ORB_ID(home_position), 0, 0, &getSubscriptions()),
|
||||
_sub_gps(ORB_ID(vehicle_gps_position), 0, 0, &getSubscriptions()),
|
||||
_sub_vision_pos(ORB_ID(vision_position_estimate), 0, 0, &getSubscriptions()),
|
||||
_sub_mocap(ORB_ID(att_pos_mocap), 0, 0, &getSubscriptions()),
|
||||
_sub_dist0(ORB_ID(distance_sensor), 0, 0, &getSubscriptions()),
|
||||
_sub_dist1(ORB_ID(distance_sensor), 0, 1, &getSubscriptions()),
|
||||
_sub_dist2(ORB_ID(distance_sensor), 0, 2, &getSubscriptions()),
|
||||
_sub_dist3(ORB_ID(distance_sensor), 0, 3, &getSubscriptions()),
|
||||
_sub_armed(ORB_ID(actuator_armed), 1000 / 2, 0, &getSubscriptions()),
|
||||
_sub_att(ORB_ID(vehicle_attitude), 1000 / 100, 0, &getSubscriptions()),
|
||||
// flow 10 hz
|
||||
_sub_flow(ORB_ID(optical_flow), 1000 / 10, 0, &getSubscriptions()),
|
||||
// main prediction loop, 100 hz
|
||||
_sub_sensor(ORB_ID(sensor_combined), 1000 / 100, 0, &getSubscriptions()),
|
||||
// status updates 2 hz
|
||||
_sub_param_update(ORB_ID(parameter_update), 1000 / 2, 0, &getSubscriptions()),
|
||||
_sub_manual(ORB_ID(manual_control_setpoint), 1000 / 2, 0, &getSubscriptions()),
|
||||
_sub_home(ORB_ID(home_position), 1000 / 2, 0, &getSubscriptions()),
|
||||
// gps 10 hz
|
||||
_sub_gps(ORB_ID(vehicle_gps_position), 1000 / 10, 0, &getSubscriptions()),
|
||||
// vision 5 hz
|
||||
_sub_vision_pos(ORB_ID(vision_position_estimate), 1000 / 5, 0, &getSubscriptions()),
|
||||
// all distance sensors, 10 hz
|
||||
_sub_mocap(ORB_ID(att_pos_mocap), 1000 / 10, 0, &getSubscriptions()),
|
||||
_sub_dist0(ORB_ID(distance_sensor), 1000 / 10, 0, &getSubscriptions()),
|
||||
_sub_dist1(ORB_ID(distance_sensor), 1000 / 10, 1, &getSubscriptions()),
|
||||
_sub_dist2(ORB_ID(distance_sensor), 1000 / 10, 2, &getSubscriptions()),
|
||||
_sub_dist3(ORB_ID(distance_sensor), 1000 / 10, 3, &getSubscriptions()),
|
||||
_dist_subs(),
|
||||
_sub_lidar(NULL),
|
||||
_sub_sonar(NULL),
|
||||
|
|
@ -97,7 +102,10 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
|
|||
_mocapStats(this, ""),
|
||||
_gpsStats(this, ""),
|
||||
|
||||
// stats
|
||||
// low pass
|
||||
_xLowPass(this, "X_LP"),
|
||||
|
||||
// delay
|
||||
_xDelay(this, ""),
|
||||
_tDelay(this, ""),
|
||||
|
||||
|
|
@ -141,9 +149,9 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
|
|||
_flowMeanQual(0),
|
||||
|
||||
// status
|
||||
_canEstimateXY(false),
|
||||
_canEstimateZ(false),
|
||||
_canEstimateT(false),
|
||||
_validXY(false),
|
||||
_validZ(false),
|
||||
_validTZ(false),
|
||||
_xyTimeout(true),
|
||||
_zTimeout(true),
|
||||
_tzTimeout(true),
|
||||
|
|
@ -182,12 +190,12 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
|
|||
_polls[POLL_SENSORS].fd = _sub_sensor.getHandle();
|
||||
_polls[POLL_SENSORS].events = POLLIN;
|
||||
|
||||
// initialize P, x, u
|
||||
initP();
|
||||
// initialize A, B, P, x, u
|
||||
_x.setZero();
|
||||
_u.setZero();
|
||||
_flowX = 0;
|
||||
_flowY = 0;
|
||||
initSS();
|
||||
|
||||
// perf counters
|
||||
_loop_perf = perf_alloc(PC_ELAPSED,
|
||||
|
|
@ -207,6 +215,14 @@ BlockLocalPositionEstimator::~BlockLocalPositionEstimator()
|
|||
{
|
||||
}
|
||||
|
||||
Vector<float, BlockLocalPositionEstimator::n_x> BlockLocalPositionEstimator::dynamics(
|
||||
float t,
|
||||
const Vector<float, BlockLocalPositionEstimator::n_x> &x,
|
||||
const Vector<float, BlockLocalPositionEstimator::n_u> &u)
|
||||
{
|
||||
return _A * x + _B * u;
|
||||
}
|
||||
|
||||
void BlockLocalPositionEstimator::update()
|
||||
{
|
||||
|
||||
|
|
@ -254,6 +270,9 @@ void BlockLocalPositionEstimator::update()
|
|||
|
||||
// assume we are on the ground, so terrain alt is local alt
|
||||
_x(X_tz) = _x(X_z);
|
||||
|
||||
// reset lowpass filter as well
|
||||
_xLowPass.setState(_x);
|
||||
}
|
||||
|
||||
_lastArmedState = armedState;
|
||||
|
|
@ -275,6 +294,7 @@ void BlockLocalPositionEstimator::update()
|
|||
// update parameters
|
||||
if (paramsUpdated) {
|
||||
updateParams();
|
||||
updateSSParams();
|
||||
}
|
||||
|
||||
// update home position projection
|
||||
|
|
@ -282,27 +302,60 @@ void BlockLocalPositionEstimator::update()
|
|||
updateHome();
|
||||
}
|
||||
|
||||
// determine if we should start estimating
|
||||
_canEstimateZ =
|
||||
(_baroInitialized && _baroFault < fault_lvl_disable);
|
||||
_canEstimateXY =
|
||||
(_gpsInitialized && _gpsFault < fault_lvl_disable) ||
|
||||
(_flowInitialized && _flowFault < fault_lvl_disable) ||
|
||||
(_visionInitialized && _visionFault < fault_lvl_disable) ||
|
||||
(_mocapInitialized && _mocapFault < fault_lvl_disable);
|
||||
_canEstimateT =
|
||||
(_lidarInitialized && _lidarFault < fault_lvl_disable) ||
|
||||
(_sonarInitialized && _sonarFault < fault_lvl_disable);
|
||||
// is xy valid?
|
||||
bool xy_stddev_ok = sqrt(math::max(_P(X_x, X_x), _P(X_y, X_y))) < EST_STDDEV_XY_VALID;
|
||||
|
||||
if (_canEstimateXY) {
|
||||
if (_validXY) {
|
||||
// if valid and gps has timed out, set to not valid
|
||||
if (!xy_stddev_ok && !_gpsInitialized) {
|
||||
_validXY = false;
|
||||
}
|
||||
|
||||
} else {
|
||||
if (xy_stddev_ok) {
|
||||
_validXY = true;
|
||||
}
|
||||
}
|
||||
|
||||
// is z valid?
|
||||
bool z_stddev_ok = sqrt(_P(X_z, X_z)) < EST_STDDEV_Z_VALID;
|
||||
|
||||
if (_validZ) {
|
||||
// if valid and baro has timed out, set to not valid
|
||||
if (!z_stddev_ok && !_baroInitialized) {
|
||||
_validZ = false;
|
||||
}
|
||||
|
||||
} else {
|
||||
if (z_stddev_ok) {
|
||||
_validZ = true;
|
||||
}
|
||||
}
|
||||
|
||||
// is terrain valid?
|
||||
bool tz_stddev_ok = sqrt(_P(X_tz, X_tz)) < EST_STDDEV_TZ_VALID;
|
||||
|
||||
if (_validTZ) {
|
||||
if (!tz_stddev_ok) {
|
||||
_validTZ = false;
|
||||
}
|
||||
|
||||
} else {
|
||||
if (tz_stddev_ok) {
|
||||
_validTZ = true;
|
||||
}
|
||||
}
|
||||
|
||||
// timeouts
|
||||
if (_validXY) {
|
||||
_time_last_xy = _timeStamp;
|
||||
}
|
||||
|
||||
if (_canEstimateZ) {
|
||||
if (_validZ) {
|
||||
_time_last_z = _timeStamp;
|
||||
}
|
||||
|
||||
if (_canEstimateT) {
|
||||
if (_validTZ) {
|
||||
_time_last_tz = _timeStamp;
|
||||
}
|
||||
|
||||
|
|
@ -310,7 +363,7 @@ void BlockLocalPositionEstimator::update()
|
|||
checkTimeouts();
|
||||
|
||||
// if we have no lat, lon initialize projection at 0,0
|
||||
if (_canEstimateXY && !_map_ref.init_done) {
|
||||
if (_validXY && !_map_ref.init_done) {
|
||||
map_projection_init(&_map_ref,
|
||||
_init_home_lat.get(),
|
||||
_init_home_lon.get());
|
||||
|
|
@ -431,7 +484,7 @@ void BlockLocalPositionEstimator::update()
|
|||
publishLocalPos();
|
||||
publishEstimatorStatus();
|
||||
|
||||
if (_canEstimateXY) {
|
||||
if (_validXY) {
|
||||
publishGlobalPos();
|
||||
}
|
||||
}
|
||||
|
|
@ -495,7 +548,53 @@ void BlockLocalPositionEstimator::checkTimeouts()
|
|||
|
||||
float BlockLocalPositionEstimator::agl()
|
||||
{
|
||||
return _x(X_tz) - _x(X_z);
|
||||
const Vector<float, n_x> &xLP = _xLowPass.getState();
|
||||
return xLP(X_tz) - xLP(X_z);
|
||||
}
|
||||
|
||||
void BlockLocalPositionEstimator::correctionLogic(Vector<float, n_x> &dx)
|
||||
{
|
||||
// don't correct bias when rotating rapidly
|
||||
float ang_speed = sqrt(
|
||||
_sub_att.get().rollspeed * _sub_att.get().rollspeed +
|
||||
_sub_att.get().pitchspeed * _sub_att.get().pitchspeed +
|
||||
_sub_att.get().yawspeed * _sub_att.get().yawspeed);
|
||||
|
||||
if (ang_speed > 1) {
|
||||
dx(X_bx) = 0;
|
||||
dx(X_by) = 0;
|
||||
dx(X_bz) = 0;
|
||||
}
|
||||
|
||||
if (!_validXY) {
|
||||
dx(X_x) = 0;
|
||||
dx(X_y) = 0;
|
||||
dx(X_vx) = 0;
|
||||
dx(X_vy) = 0;
|
||||
dx(X_bx) = 0;
|
||||
dx(X_by) = 0;
|
||||
}
|
||||
|
||||
if (!_validZ) {
|
||||
dx(X_z) = 0;
|
||||
dx(X_vz) = 0;
|
||||
dx(X_bz) = 0;
|
||||
}
|
||||
|
||||
if (!_validTZ) {
|
||||
dx(X_tz) = 0;
|
||||
}
|
||||
|
||||
// saturate bias
|
||||
float bx = dx(X_bx) + _x(X_bx);
|
||||
float by = dx(X_by) + _x(X_by);
|
||||
float bz = dx(X_bz) + _x(X_bz);
|
||||
|
||||
if (std::abs(bx) > BIAS_MAX) { bx = BIAS_MAX * bx / std::abs(bx); }
|
||||
|
||||
if (std::abs(by) > BIAS_MAX) { by = BIAS_MAX * by / std::abs(by); }
|
||||
|
||||
if (std::abs(bz) > BIAS_MAX) { bz = BIAS_MAX * bz / std::abs(bz); }
|
||||
}
|
||||
|
||||
void BlockLocalPositionEstimator::detectDistanceSensors()
|
||||
|
|
@ -551,21 +650,23 @@ void BlockLocalPositionEstimator::updateHome()
|
|||
|
||||
void BlockLocalPositionEstimator::publishLocalPos()
|
||||
{
|
||||
const Vector<float, n_x> &xLP = _xLowPass.getState();
|
||||
|
||||
// publish local position
|
||||
if (PX4_ISFINITE(_x(X_x)) && PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) &&
|
||||
PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy))
|
||||
&& PX4_ISFINITE(_x(X_vz))) {
|
||||
_pub_lpos.get().timestamp = _timeStamp;
|
||||
_pub_lpos.get().xy_valid = _canEstimateXY;
|
||||
_pub_lpos.get().z_valid = _canEstimateZ;
|
||||
_pub_lpos.get().v_xy_valid = _canEstimateXY;
|
||||
_pub_lpos.get().v_z_valid = _canEstimateZ;
|
||||
_pub_lpos.get().x = _x(X_x); // north
|
||||
_pub_lpos.get().y = _x(X_y); // east
|
||||
_pub_lpos.get().z = _x(X_z); // down
|
||||
_pub_lpos.get().vx = _x(X_vx); // north
|
||||
_pub_lpos.get().vy = _x(X_vy); // east
|
||||
_pub_lpos.get().vz = _x(X_vz); // down
|
||||
_pub_lpos.get().xy_valid = _validXY;
|
||||
_pub_lpos.get().z_valid = _validZ;
|
||||
_pub_lpos.get().v_xy_valid = _validXY;
|
||||
_pub_lpos.get().v_z_valid = _validZ;
|
||||
_pub_lpos.get().x = xLP(X_x); // north
|
||||
_pub_lpos.get().y = xLP(X_y); // east
|
||||
_pub_lpos.get().z = xLP(X_z); // down
|
||||
_pub_lpos.get().vx = xLP(X_vx); // north
|
||||
_pub_lpos.get().vy = xLP(X_vy); // east
|
||||
_pub_lpos.get().vz = xLP(X_vz); // down
|
||||
_pub_lpos.get().yaw = _sub_att.get().yaw;
|
||||
_pub_lpos.get().xy_global = _sub_home.get().timestamp != 0; // need home for reference
|
||||
_pub_lpos.get().z_global = _baroInitialized;
|
||||
|
|
@ -574,9 +675,9 @@ void BlockLocalPositionEstimator::publishLocalPos()
|
|||
_pub_lpos.get().ref_lon = _map_ref.lon_rad * 180 / M_PI;
|
||||
_pub_lpos.get().ref_alt = _sub_home.get().alt;
|
||||
_pub_lpos.get().dist_bottom = agl();
|
||||
_pub_lpos.get().dist_bottom_rate = -_x(X_vz);
|
||||
_pub_lpos.get().dist_bottom_rate = - xLP(X_vz);
|
||||
_pub_lpos.get().surface_bottom_timestamp = _timeStamp;
|
||||
_pub_lpos.get().dist_bottom_valid = _canEstimateZ;
|
||||
_pub_lpos.get().dist_bottom_valid = _validTZ && _validZ;
|
||||
_pub_lpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
|
||||
_pub_lpos.get().epv = sqrtf(_P(X_z, X_z));
|
||||
_pub_lpos.update();
|
||||
|
|
@ -595,13 +696,13 @@ void BlockLocalPositionEstimator::publishEstimatorStatus()
|
|||
_pub_est_status.get().n_states = n_x;
|
||||
_pub_est_status.get().nan_flags = 0;
|
||||
_pub_est_status.get().health_flags =
|
||||
((_baroFault > fault_lvl_disable) << SENSOR_BARO)
|
||||
+ ((_gpsFault > fault_lvl_disable) << SENSOR_GPS)
|
||||
+ ((_lidarFault > fault_lvl_disable) << SENSOR_LIDAR)
|
||||
+ ((_flowFault > fault_lvl_disable) << SENSOR_FLOW)
|
||||
+ ((_sonarFault > fault_lvl_disable) << SENSOR_SONAR)
|
||||
+ ((_visionFault > fault_lvl_disable) << SENSOR_VISION)
|
||||
+ ((_mocapFault > fault_lvl_disable) << SENSOR_MOCAP);
|
||||
((_baroFault > FAULT_NONE) << SENSOR_BARO)
|
||||
+ ((_gpsFault > FAULT_NONE) << SENSOR_GPS)
|
||||
+ ((_lidarFault > FAULT_NONE) << SENSOR_LIDAR)
|
||||
+ ((_flowFault > FAULT_NONE) << SENSOR_FLOW)
|
||||
+ ((_sonarFault > FAULT_NONE) << SENSOR_SONAR)
|
||||
+ ((_visionFault > FAULT_NONE) << SENSOR_VISION)
|
||||
+ ((_mocapFault > FAULT_NONE) << SENSOR_MOCAP);
|
||||
_pub_est_status.get().timeout_flags =
|
||||
(_baroInitialized << SENSOR_BARO)
|
||||
+ (_gpsInitialized << SENSOR_GPS)
|
||||
|
|
@ -618,26 +719,27 @@ void BlockLocalPositionEstimator::publishGlobalPos()
|
|||
// publish global position
|
||||
double lat = 0;
|
||||
double lon = 0;
|
||||
map_projection_reproject(&_map_ref, _x(X_x), _x(X_y), &lat, &lon);
|
||||
float alt = -_x(X_z) + _altHome;
|
||||
const Vector<float, n_x> &xLP = _xLowPass.getState();
|
||||
map_projection_reproject(&_map_ref, xLP(X_x), xLP(X_y), &lat, &lon);
|
||||
float alt = -xLP(X_z) + _altHome;
|
||||
|
||||
if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt) &&
|
||||
PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy)) &&
|
||||
PX4_ISFINITE(_x(X_vz))) {
|
||||
PX4_ISFINITE(xLP(X_vx)) && PX4_ISFINITE(xLP(X_vy)) &&
|
||||
PX4_ISFINITE(xLP(X_vz))) {
|
||||
_pub_gpos.get().timestamp = _timeStamp;
|
||||
_pub_gpos.get().time_utc_usec = _sub_gps.get().time_utc_usec;
|
||||
_pub_gpos.get().lat = lat;
|
||||
_pub_gpos.get().lon = lon;
|
||||
_pub_gpos.get().alt = alt;
|
||||
_pub_gpos.get().vel_n = _x(X_vx);
|
||||
_pub_gpos.get().vel_e = _x(X_vy);
|
||||
_pub_gpos.get().vel_d = _x(X_vz);
|
||||
_pub_gpos.get().vel_n = xLP(X_vx);
|
||||
_pub_gpos.get().vel_e = xLP(X_vy);
|
||||
_pub_gpos.get().vel_d = xLP(X_vz);
|
||||
_pub_gpos.get().yaw = _sub_att.get().yaw;
|
||||
_pub_gpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
|
||||
_pub_gpos.get().epv = sqrtf(_P(X_z, X_z));
|
||||
_pub_gpos.get().terrain_alt = _altHome - _x(X_tz);
|
||||
_pub_gpos.get().terrain_alt_valid = _canEstimateT;
|
||||
_pub_gpos.get().dead_reckoning = !_canEstimateXY && !_xyTimeout;
|
||||
_pub_gpos.get().terrain_alt = _altHome - xLP(X_tz);
|
||||
_pub_gpos.get().terrain_alt_valid = _validTZ;
|
||||
_pub_gpos.get().dead_reckoning = !_validXY && !_xyTimeout;
|
||||
_pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter[0];
|
||||
_pub_gpos.update();
|
||||
}
|
||||
|
|
@ -646,23 +748,95 @@ void BlockLocalPositionEstimator::publishGlobalPos()
|
|||
void BlockLocalPositionEstimator::initP()
|
||||
{
|
||||
_P.setZero();
|
||||
_P(X_x, X_x) = 1;
|
||||
_P(X_y, X_y) = 1;
|
||||
_P(X_z, X_z) = 1;
|
||||
_P(X_x, X_x) = 2 * EST_STDDEV_XY_VALID; // initialize to twice valid condition
|
||||
_P(X_y, X_y) = 2 * EST_STDDEV_XY_VALID;
|
||||
_P(X_z, X_z) = 2 * EST_STDDEV_Z_VALID;
|
||||
_P(X_vx, X_vx) = 1;
|
||||
_P(X_vy, X_vy) = 1;
|
||||
_P(X_vz, X_vz) = 1;
|
||||
_P(X_bx, X_bx) = 1e-6;
|
||||
_P(X_by, X_by) = 1e-6;
|
||||
_P(X_bz, X_bz) = 1e-6;
|
||||
_P(X_tz, X_tz) = 1;
|
||||
_P(X_tz, X_tz) = 2 * EST_STDDEV_TZ_VALID;
|
||||
}
|
||||
|
||||
void BlockLocalPositionEstimator::initSS()
|
||||
{
|
||||
initP();
|
||||
|
||||
// dynamics matrix
|
||||
_A.setZero();
|
||||
// derivative of position is velocity
|
||||
_A(X_x, X_vx) = 1;
|
||||
_A(X_y, X_vy) = 1;
|
||||
_A(X_z, X_vz) = 1;
|
||||
|
||||
// input matrix
|
||||
_B.setZero();
|
||||
_B(X_vx, U_ax) = 1;
|
||||
_B(X_vy, U_ay) = 1;
|
||||
_B(X_vz, U_az) = 1;
|
||||
|
||||
// update components that depend on current state
|
||||
updateSSStates();
|
||||
updateSSParams();
|
||||
}
|
||||
|
||||
void BlockLocalPositionEstimator::updateSSStates()
|
||||
{
|
||||
// derivative of velocity is accelerometer acceleration
|
||||
// (in input matrix) - bias (in body frame)
|
||||
Matrix3f R_att(_sub_att.get().R);
|
||||
_A(X_vx, X_bx) = -R_att(0, 0);
|
||||
_A(X_vx, X_by) = -R_att(0, 1);
|
||||
_A(X_vx, X_bz) = -R_att(0, 2);
|
||||
|
||||
_A(X_vy, X_bx) = -R_att(1, 0);
|
||||
_A(X_vy, X_by) = -R_att(1, 1);
|
||||
_A(X_vy, X_bz) = -R_att(1, 2);
|
||||
|
||||
_A(X_vz, X_bx) = -R_att(2, 0);
|
||||
_A(X_vz, X_by) = -R_att(2, 1);
|
||||
_A(X_vz, X_bz) = -R_att(2, 2);
|
||||
}
|
||||
|
||||
void BlockLocalPositionEstimator::updateSSParams()
|
||||
{
|
||||
// input noise covariance matrix
|
||||
_R.setZero();
|
||||
_R(U_ax, U_ax) = _accel_xy_stddev.get() * _accel_xy_stddev.get();
|
||||
_R(U_ay, U_ay) = _accel_xy_stddev.get() * _accel_xy_stddev.get();
|
||||
_R(U_az, U_az) = _accel_z_stddev.get() * _accel_z_stddev.get();
|
||||
|
||||
// process noise power matrix
|
||||
_Q.setZero();
|
||||
float pn_p_sq = _pn_p_noise_density.get() * _pn_p_noise_density.get();
|
||||
float pn_v_sq = _pn_v_noise_density.get() * _pn_v_noise_density.get();
|
||||
_Q(X_x, X_x) = pn_p_sq;
|
||||
_Q(X_y, X_y) = pn_p_sq;
|
||||
_Q(X_z, X_z) = pn_p_sq;
|
||||
_Q(X_vx, X_vx) = pn_v_sq;
|
||||
_Q(X_vy, X_vy) = pn_v_sq;
|
||||
_Q(X_vz, X_vz) = pn_v_sq;
|
||||
|
||||
// technically, the noise is in the body frame,
|
||||
// but the components are all the same, so
|
||||
// ignoring for now
|
||||
float pn_b_sq = _pn_b_noise_density.get() * _pn_b_noise_density.get();
|
||||
_Q(X_bx, X_bx) = pn_b_sq;
|
||||
_Q(X_by, X_by) = pn_b_sq;
|
||||
_Q(X_bz, X_bz) = pn_b_sq;
|
||||
|
||||
// terrain random walk noise
|
||||
float pn_t_sq = _pn_t_noise_density.get() * _pn_t_noise_density.get();
|
||||
_Q(X_tz, X_tz) = pn_t_sq;
|
||||
}
|
||||
|
||||
void BlockLocalPositionEstimator::predict()
|
||||
{
|
||||
// if can't update anything, don't propagate
|
||||
// state or covariance
|
||||
if (!_canEstimateXY && !_canEstimateZ) { return; }
|
||||
if (!_validXY && !_validZ) { return; }
|
||||
|
||||
if (_integrate.get() && _sub_att.get().R_valid) {
|
||||
Matrix3f R_att(_sub_att.get().R);
|
||||
|
|
@ -674,86 +848,26 @@ void BlockLocalPositionEstimator::predict()
|
|||
_u = Vector3f(0, 0, 0);
|
||||
}
|
||||
|
||||
// dynamics matrix
|
||||
Matrix<float, n_x, n_x> A; // state dynamics matrix
|
||||
A.setZero();
|
||||
// derivative of position is velocity
|
||||
A(X_x, X_vx) = 1;
|
||||
A(X_y, X_vy) = 1;
|
||||
A(X_z, X_vz) = 1;
|
||||
|
||||
// derivative of velocity is accelerometer acceleration
|
||||
// (in input matrix) - bias (in body frame)
|
||||
Matrix3f R_att(_sub_att.get().R);
|
||||
A(X_vx, X_bx) = -R_att(0, 0);
|
||||
A(X_vx, X_by) = -R_att(0, 1);
|
||||
A(X_vx, X_bz) = -R_att(0, 2);
|
||||
|
||||
A(X_vy, X_bx) = -R_att(1, 0);
|
||||
A(X_vy, X_by) = -R_att(1, 1);
|
||||
A(X_vy, X_bz) = -R_att(1, 2);
|
||||
|
||||
A(X_vz, X_bx) = -R_att(2, 0);
|
||||
A(X_vz, X_by) = -R_att(2, 1);
|
||||
A(X_vz, X_bz) = -R_att(2, 2);
|
||||
|
||||
// input matrix
|
||||
Matrix<float, n_x, n_u> B; // input matrix
|
||||
B.setZero();
|
||||
B(X_vx, U_ax) = 1;
|
||||
B(X_vy, U_ay) = 1;
|
||||
B(X_vz, U_az) = 1;
|
||||
|
||||
// input noise covariance matrix
|
||||
Matrix<float, n_u, n_u> R;
|
||||
R.setZero();
|
||||
R(U_ax, U_ax) = _accel_xy_stddev.get() * _accel_xy_stddev.get();
|
||||
R(U_ay, U_ay) = _accel_xy_stddev.get() * _accel_xy_stddev.get();
|
||||
R(U_az, U_az) = _accel_z_stddev.get() * _accel_z_stddev.get();
|
||||
|
||||
// process noise power matrix
|
||||
Matrix<float, n_x, n_x> Q;
|
||||
Q.setZero();
|
||||
float pn_p_sq = _pn_p_noise_density.get() * _pn_p_noise_density.get();
|
||||
float pn_v_sq = _pn_v_noise_density.get() * _pn_v_noise_density.get();
|
||||
Q(X_x, X_x) = pn_p_sq;
|
||||
Q(X_y, X_y) = pn_p_sq;
|
||||
Q(X_z, X_z) = pn_p_sq;
|
||||
Q(X_vx, X_vx) = pn_v_sq;
|
||||
Q(X_vy, X_vy) = pn_v_sq;
|
||||
Q(X_vz, X_vz) = pn_v_sq;
|
||||
|
||||
// technically, the noise is in the body frame,
|
||||
// but the components are all the same, so
|
||||
// ignoring for now
|
||||
float pn_b_sq = _pn_b_noise_density.get() * _pn_b_noise_density.get();
|
||||
Q(X_bx, X_bx) = pn_b_sq;
|
||||
Q(X_by, X_by) = pn_b_sq;
|
||||
Q(X_bz, X_bz) = pn_b_sq;
|
||||
|
||||
// terrain random walk noise
|
||||
float pn_t_sq = _pn_t_noise_density.get() * _pn_t_noise_density.get();
|
||||
Q(X_tz, X_tz) = pn_t_sq;
|
||||
// update state space based on new states
|
||||
updateSSStates();
|
||||
|
||||
// continuous time kalman filter prediction
|
||||
Vector<float, n_x> dx = (A * _x + B * _u) * getDt();
|
||||
|
||||
// only predict for components we have
|
||||
// valid measurements for
|
||||
if (!_canEstimateXY) {
|
||||
dx(X_x) = 0;
|
||||
dx(X_y) = 0;
|
||||
dx(X_vx) = 0;
|
||||
dx(X_vy) = 0;
|
||||
}
|
||||
|
||||
if (!_canEstimateZ) {
|
||||
dx(X_z) = 0;
|
||||
dx(X_vz) = 0;
|
||||
}
|
||||
// integrate runge kutta 4th order
|
||||
// TODO move rk4 algorithm to matrixlib
|
||||
// https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods
|
||||
float h = getDt();
|
||||
Vector<float, n_x> k1, k2, k3, k4;
|
||||
k1 = dynamics(0, _x, _u);
|
||||
k2 = dynamics(h / 2, _x + k1 * h / 2, _u);
|
||||
k3 = dynamics(h / 2, _x + k2 * h / 2, _u);
|
||||
k4 = dynamics(h, _x + k3 * h, _u);
|
||||
Vector<float, n_x> dx = (k1 + k2 * 2 + k3 * 2 + k4) * (h / 6);
|
||||
|
||||
// propagate
|
||||
correctionLogic(dx);
|
||||
_x += dx;
|
||||
_P += (A * _P + _P * A.transpose() +
|
||||
B * R * B.transpose() + Q) * getDt();
|
||||
_P += (_A * _P + _P * _A.transpose() +
|
||||
_B * _R * _B.transpose() +
|
||||
_Q) * getDt();
|
||||
_xLowPass.update(_x);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -33,8 +33,9 @@
|
|||
using namespace matrix;
|
||||
using namespace control;
|
||||
|
||||
static const float GPS_DELAY_MAX = 0.5; // seconds
|
||||
static const float HIST_STEP = 0.05; // 20 hz
|
||||
static const float GPS_DELAY_MAX = 0.5f; // seconds
|
||||
static const float HIST_STEP = 0.05f; // 20 hz
|
||||
static const float BIAS_MAX = 1e-1f;
|
||||
static const size_t HIST_LEN = 10; // GPS_DELAY_MAX / HIST_STEP;
|
||||
static const size_t N_DIST_SUBS = 4;
|
||||
|
||||
|
|
@ -134,6 +135,10 @@ public:
|
|||
|
||||
BlockLocalPositionEstimator();
|
||||
void update();
|
||||
Vector<float, n_x> dynamics(
|
||||
float t,
|
||||
const Vector<float, n_x> &x,
|
||||
const Vector<float, n_u> &u);
|
||||
virtual ~BlockLocalPositionEstimator();
|
||||
|
||||
private:
|
||||
|
|
@ -144,6 +149,9 @@ private:
|
|||
// methods
|
||||
// ----------------------------
|
||||
void initP();
|
||||
void initSS();
|
||||
void updateSSStates();
|
||||
void updateSSParams();
|
||||
|
||||
// predict the next state
|
||||
void predict();
|
||||
|
|
@ -195,6 +203,7 @@ private:
|
|||
|
||||
// misc
|
||||
float agl();
|
||||
void correctionLogic(Vector<float, n_x> &dx);
|
||||
void detectDistanceSensors();
|
||||
void updateHome();
|
||||
|
||||
|
|
@ -207,11 +216,8 @@ private:
|
|||
// ----------------------------
|
||||
|
||||
// subscriptions
|
||||
uORB::Subscription<vehicle_status_s> _sub_status;
|
||||
uORB::Subscription<actuator_armed_s> _sub_armed;
|
||||
uORB::Subscription<vehicle_control_mode_s> _sub_control_mode;
|
||||
uORB::Subscription<vehicle_attitude_s> _sub_att;
|
||||
uORB::Subscription<vehicle_attitude_setpoint_s> _sub_att_sp;
|
||||
uORB::Subscription<optical_flow_s> _sub_flow;
|
||||
uORB::Subscription<sensor_combined_s> _sub_sensor;
|
||||
uORB::Subscription<parameter_update_s> _sub_param_update;
|
||||
|
|
@ -302,6 +308,9 @@ private:
|
|||
BlockStats<float, n_y_mocap> _mocapStats;
|
||||
BlockStats<double, n_y_gps> _gpsStats;
|
||||
|
||||
// low pass
|
||||
BlockLowPassVector<float, n_x> _xLowPass;
|
||||
|
||||
// delay blocks
|
||||
BlockDelay<float, n_x, 1, HIST_LEN> _xDelay;
|
||||
BlockDelay<uint64_t, 1, 1, HIST_LEN> _tDelay;
|
||||
|
|
@ -346,9 +355,9 @@ private:
|
|||
float _flowMeanQual;
|
||||
|
||||
// status
|
||||
bool _canEstimateXY;
|
||||
bool _canEstimateZ;
|
||||
bool _canEstimateT;
|
||||
bool _validXY;
|
||||
bool _validZ;
|
||||
bool _validTZ;
|
||||
bool _xyTimeout;
|
||||
bool _zTimeout;
|
||||
bool _tzTimeout;
|
||||
|
|
@ -372,4 +381,8 @@ private:
|
|||
Vector<float, n_x> _x; // state vector
|
||||
Vector<float, n_u> _u; // input vector
|
||||
Matrix<float, n_x, n_x> _P; // state covariance matrix
|
||||
Matrix<float, n_x, n_x> _A; // dynamics matrix
|
||||
Matrix<float, n_x, n_u> _B; // input matrix
|
||||
Matrix<float, n_u, n_u> _R; // input covariance
|
||||
Matrix<float, n_x, n_x> _Q; // process noise covariance
|
||||
};
|
||||
|
|
|
|||
|
|
@ -35,7 +35,7 @@ px4_add_module(
|
|||
MAIN local_position_estimator
|
||||
COMPILE_FLAGS -Os
|
||||
STACK_MAIN 5700
|
||||
STACK_MAX 10000
|
||||
STACK_MAX 13000
|
||||
SRCS
|
||||
local_position_estimator_main.cpp
|
||||
BlockLocalPositionEstimator.cpp
|
||||
|
|
|
|||
|
|
@ -112,7 +112,7 @@ int local_position_estimator_main(int argc, char *argv[])
|
|||
deamon_task = px4_task_spawn_cmd("lp_estimator",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
12000,
|
||||
13000,
|
||||
local_position_estimator_thread_main,
|
||||
(argv && argc > 2) ? (char *const *) &argv[2] : (char *const *) NULL);
|
||||
return 0;
|
||||
|
|
|
|||
|
|
@ -88,34 +88,32 @@ PARAM_DEFINE_FLOAT(LPE_LDR_Z, 0.03f);
|
|||
PARAM_DEFINE_FLOAT(LPE_LDR_OFF_Z, 0.00f);
|
||||
|
||||
/**
|
||||
* Accelerometer xy standard deviation
|
||||
* Accelerometer xy noise density
|
||||
*
|
||||
* Data sheet sqrt(Noise power) = 150ug/sqrt(Hz)
|
||||
* std dev = (150*9.8*1e-6)*sqrt(1000 Hz) m/s^2
|
||||
* Since accels sampled at 1000 Hz.
|
||||
* Data sheet noise density = 150ug/sqrt(Hz) = 0.0015 m/s^2/sqrt(Hz)
|
||||
*
|
||||
* should be 0.0464
|
||||
* Larger than data sheet to account for tilt error.
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit m/s^2
|
||||
* @unit m/s^2/srqt(Hz)
|
||||
* @min 0.00001
|
||||
* @max 2
|
||||
* @decimal 4
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.0454f);
|
||||
PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.0015f);
|
||||
|
||||
/**
|
||||
* Accelerometer z standard deviation
|
||||
* Accelerometer z noise density
|
||||
*
|
||||
* (see Accel x comments)
|
||||
* Data sheet noise density = 150ug/sqrt(Hz) = 0.0015 m/s^2/sqrt(Hz)
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit m/s^2
|
||||
* @unit m/s^2/srqt(Hz)
|
||||
* @min 0.00001
|
||||
* @max 2
|
||||
* @decimal 4
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0454f);
|
||||
PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0015f);
|
||||
|
||||
/**
|
||||
* Barometric presssure altitude z standard deviation.
|
||||
|
|
@ -145,11 +143,11 @@ PARAM_DEFINE_INT32(LPE_GPS_ON, 1);
|
|||
* @max 0.4
|
||||
* @decimal 2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_GPS_DELAY, 0.25f);
|
||||
PARAM_DEFINE_FLOAT(LPE_GPS_DELAY, 0.29f);
|
||||
|
||||
|
||||
/**
|
||||
* GPS xy standard deviation.
|
||||
* Minimum GPS xy standard deviation, uses reported EPH if greater.
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit m
|
||||
|
|
@ -157,10 +155,10 @@ PARAM_DEFINE_FLOAT(LPE_GPS_DELAY, 0.25f);
|
|||
* @max 5
|
||||
* @decimal 2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_GPS_XY, 2.0f);
|
||||
PARAM_DEFINE_FLOAT(LPE_GPS_XY, 1.0f);
|
||||
|
||||
/**
|
||||
* GPS z standard deviation.
|
||||
* Minimum GPS z standard deviation, uses reported EPV if greater.
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit m
|
||||
|
|
@ -168,10 +166,11 @@ PARAM_DEFINE_FLOAT(LPE_GPS_XY, 2.0f);
|
|||
* @max 200
|
||||
* @decimal 2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_GPS_Z, 100.0f);
|
||||
PARAM_DEFINE_FLOAT(LPE_GPS_Z, 3.0f);
|
||||
|
||||
/**
|
||||
* GPS xy velocity standard deviation.
|
||||
* EPV used if greater than this value.
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit m/s
|
||||
|
|
@ -193,7 +192,7 @@ PARAM_DEFINE_FLOAT(LPE_GPS_VXY, 0.25f);
|
|||
PARAM_DEFINE_FLOAT(LPE_GPS_VZ, 0.25f);
|
||||
|
||||
/**
|
||||
* GPS max eph
|
||||
* Max EPH allowed for GPS initialization
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit m
|
||||
|
|
@ -204,7 +203,7 @@ PARAM_DEFINE_FLOAT(LPE_GPS_VZ, 0.25f);
|
|||
PARAM_DEFINE_FLOAT(LPE_EPH_MAX, 3.0f);
|
||||
|
||||
/**
|
||||
* GPS max epv
|
||||
* Max EPV allowed for GPS initialization
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit m
|
||||
|
|
@ -258,24 +257,30 @@ PARAM_DEFINE_FLOAT(LPE_VIC_P, 0.05f);
|
|||
/**
|
||||
* Position propagation noise density
|
||||
*
|
||||
* Increase to trust measurements more.
|
||||
* Decrease to trust model more.
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit m/s/sqrt(Hz)
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @decimal 8
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_PN_P, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(LPE_PN_P, 0.1f);
|
||||
|
||||
/**
|
||||
* Velocity propagation noise density
|
||||
*
|
||||
* Increase to trust measurements more.
|
||||
* Decrease to trust model more.
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit (m/s)/s/sqrt(Hz)
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @decimal 8
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_PN_V, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(LPE_PN_V, 0.1f);
|
||||
|
||||
/**
|
||||
* Accel bias propagation noise density
|
||||
|
|
@ -332,3 +337,14 @@ PARAM_DEFINE_FLOAT(LPE_LAT, 40.430f);
|
|||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_LON, -86.929);
|
||||
|
||||
/**
|
||||
* Cut frequency for state publication
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit Hz
|
||||
* @min 5
|
||||
* @max 1000
|
||||
* @decimal 0
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_X_LP, 5.0);
|
||||
|
||||
|
|
|
|||
|
|
@ -76,8 +76,8 @@ void BlockLocalPositionEstimator::baroCorrect()
|
|||
|
||||
if (beta > BETA_TABLE[n_y_baro]) {
|
||||
if (_baroFault < FAULT_MINOR) {
|
||||
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f",
|
||||
//double(r(0)), double(beta));
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f",
|
||||
double(r(0)), double(beta));
|
||||
_baroFault = FAULT_MINOR;
|
||||
}
|
||||
|
||||
|
|
@ -90,14 +90,7 @@ void BlockLocalPositionEstimator::baroCorrect()
|
|||
if (_baroFault < fault_lvl_disable) {
|
||||
Matrix<float, n_x, n_y_baro> K = _P * C.transpose() * S_I;
|
||||
Vector<float, n_x> dx = K * r;
|
||||
|
||||
if (!_canEstimateXY) {
|
||||
dx(X_x) = 0;
|
||||
dx(X_y) = 0;
|
||||
dx(X_vx) = 0;
|
||||
dx(X_vy) = 0;
|
||||
}
|
||||
|
||||
correctionLogic(dx);
|
||||
_x += dx;
|
||||
_P -= K * C * _P;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -49,7 +49,7 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
|
|||
}
|
||||
|
||||
// calculate range to center of image for flow
|
||||
if (!_canEstimateT) {
|
||||
if (!_validTZ) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
|
@ -153,7 +153,9 @@ void BlockLocalPositionEstimator::flowCorrect()
|
|||
if (_flowFault < fault_lvl_disable) {
|
||||
Matrix<float, n_x, n_y_flow> K =
|
||||
_P * C.transpose() * S_I;
|
||||
_x += K * r;
|
||||
Vector<float, n_x> dx = K * r;
|
||||
correctionLogic(dx);
|
||||
_x += dx;
|
||||
_P -= K * C * _P;
|
||||
|
||||
} else {
|
||||
|
|
|
|||
|
|
@ -8,11 +8,25 @@ extern orb_advert_t mavlink_log_pub;
|
|||
// to initialize
|
||||
static const uint32_t REQ_GPS_INIT_COUNT = 10;
|
||||
static const uint32_t GPS_TIMEOUT = 1000000; // 1.0 s
|
||||
static const float GPS_EPH_MIN = 2.0; // m, min allowed by gps self reporting
|
||||
static const float GPS_EPV_MIN = 2.0; // m, min allowed by gps self reporting
|
||||
|
||||
void BlockLocalPositionEstimator::gpsInit()
|
||||
{
|
||||
// check for good gps signal
|
||||
uint8_t nSat = _sub_gps.get().satellites_used;
|
||||
float eph = _sub_gps.get().eph;
|
||||
float epv = _sub_gps.get().epv;
|
||||
uint8_t fix_type = _sub_gps.get().fix_type;
|
||||
|
||||
if (
|
||||
nSat < 6 ||
|
||||
eph > _gps_eph_max.get() ||
|
||||
epv > _gps_epv_max.get() ||
|
||||
fix_type < 3
|
||||
) {
|
||||
_gpsStats.reset();
|
||||
return;
|
||||
}
|
||||
|
||||
// measure
|
||||
Vector<double, n_y_gps> y;
|
||||
|
||||
|
|
@ -50,21 +64,6 @@ void BlockLocalPositionEstimator::gpsInit()
|
|||
|
||||
int BlockLocalPositionEstimator::gpsMeasure(Vector<double, n_y_gps> &y)
|
||||
{
|
||||
// check for good gps signal
|
||||
uint8_t nSat = _sub_gps.get().satellites_used;
|
||||
float eph = _sub_gps.get().eph;
|
||||
float epv = _sub_gps.get().epv;
|
||||
uint8_t fix_type = _sub_gps.get().fix_type;
|
||||
|
||||
if (
|
||||
nSat < 6 ||
|
||||
eph > _gps_eph_max.get() ||
|
||||
epv > _gps_epv_max.get() ||
|
||||
fix_type < 3
|
||||
) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
// gps measurement
|
||||
y.setZero();
|
||||
y(0) = _sub_gps.get().lat * 1e-7;
|
||||
|
|
@ -125,11 +124,11 @@ void BlockLocalPositionEstimator::gpsCorrect()
|
|||
float var_vz = _gps_vz_stddev.get() * _gps_vz_stddev.get();
|
||||
|
||||
// if field is not below minimum, set it to the value provided
|
||||
if (_sub_gps.get().eph > GPS_EPH_MIN) {
|
||||
if (_sub_gps.get().eph > _gps_xy_stddev.get()) {
|
||||
var_xy = _sub_gps.get().eph * _sub_gps.get().eph;
|
||||
}
|
||||
|
||||
if (_sub_gps.get().epv > GPS_EPV_MIN) {
|
||||
if (_sub_gps.get().epv > _gps_z_stddev.get()) {
|
||||
var_z = _sub_gps.get().epv * _sub_gps.get().epv;
|
||||
}
|
||||
|
||||
|
|
@ -170,13 +169,9 @@ void BlockLocalPositionEstimator::gpsCorrect()
|
|||
|
||||
if (beta > BETA_TABLE[n_y_gps]) {
|
||||
if (_gpsFault < FAULT_MINOR) {
|
||||
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps fault, beta: %5.2f", double(beta));
|
||||
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] r: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f",
|
||||
//double(r(0)), double(r(1)), double(r(2)),
|
||||
//double(r(3)), double(r(4)), double(r(5)));
|
||||
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] S_I: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f",
|
||||
//double(S_I(0, 0)), double(S_I(1, 1)), double(S_I(2, 2)),
|
||||
//double(S_I(3, 3)), double(S_I(4, 4)), double(S_I(5, 5)));
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps fault %3g %3g %3g %3g %3g %3g",
|
||||
double(r(0)*r(0) / S_I(0, 0)), double(r(1)*r(1) / S_I(1, 1)), double(r(2)*r(2) / S_I(2, 2)),
|
||||
double(r(3)*r(3) / S_I(3, 3)), double(r(4)*r(4) / S_I(4, 4)), double(r(5)*r(5) / S_I(5, 5)));
|
||||
_gpsFault = FAULT_MINOR;
|
||||
}
|
||||
|
||||
|
|
@ -188,7 +183,9 @@ void BlockLocalPositionEstimator::gpsCorrect()
|
|||
// kalman filter correction if no hard fault
|
||||
if (_gpsFault < fault_lvl_disable) {
|
||||
Matrix<float, n_x, n_y_gps> K = _P * C.transpose() * S_I;
|
||||
_x += K * r;
|
||||
Vector<float, n_x> dx = K * r;
|
||||
correctionLogic(dx);
|
||||
_x += dx;
|
||||
_P -= K * C * _P;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -109,14 +109,7 @@ void BlockLocalPositionEstimator::lidarCorrect()
|
|||
if (_lidarFault < fault_lvl_disable) {
|
||||
Matrix<float, n_x, n_y_lidar> K = _P * C.transpose() * S_I;
|
||||
Vector<float, n_x> dx = K * r;
|
||||
|
||||
if (!_canEstimateXY) {
|
||||
dx(X_x) = 0;
|
||||
dx(X_y) = 0;
|
||||
dx(X_vx) = 0;
|
||||
dx(X_vy) = 0;
|
||||
}
|
||||
|
||||
correctionLogic(dx);
|
||||
_x += dx;
|
||||
_P -= K * C * _P;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -98,7 +98,9 @@ void BlockLocalPositionEstimator::mocapCorrect()
|
|||
// kalman filter correction if no fault
|
||||
if (_mocapFault < fault_lvl_disable) {
|
||||
Matrix<float, n_x, n_y_mocap> K = _P * C.transpose() * S_I;
|
||||
_x += K * r;
|
||||
Vector<float, n_x> dx = K * r;
|
||||
correctionLogic(dx);
|
||||
_x += dx;
|
||||
_P -= K * C * _P;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -127,14 +127,7 @@ void BlockLocalPositionEstimator::sonarCorrect()
|
|||
Matrix<float, n_x, n_y_sonar> K =
|
||||
_P * C.transpose() * S_I;
|
||||
Vector<float, n_x> dx = K * r;
|
||||
|
||||
if (!_canEstimateXY) {
|
||||
dx(X_x) = 0;
|
||||
dx(X_y) = 0;
|
||||
dx(X_vx) = 0;
|
||||
dx(X_vy) = 0;
|
||||
}
|
||||
|
||||
correctionLogic(dx);
|
||||
_x += dx;
|
||||
_P -= K * C * _P;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -96,7 +96,9 @@ void BlockLocalPositionEstimator::visionCorrect()
|
|||
// kalman filter correction if no fault
|
||||
if (_visionFault < fault_lvl_disable) {
|
||||
Matrix<float, n_x, n_y_vision> K = _P * C.transpose() * S_I;
|
||||
_x += K * r;
|
||||
Vector<float, n_x> dx = K * r;
|
||||
correctionLogic(dx);
|
||||
_x += dx;
|
||||
_P -= K * C * _P;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -2138,6 +2138,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
memcpy(&(log_msg.body.log_EST2.cov), buf.estimator_status.covariances, maxcopy2);
|
||||
log_msg.body.log_EST2.gps_check_fail_flags = buf.estimator_status.gps_check_fail_flags;
|
||||
log_msg.body.log_EST2.control_mode_flags = buf.estimator_status.control_mode_flags;
|
||||
log_msg.body.log_EST2.health_flags = buf.estimator_status.health_flags;
|
||||
LOGBUFFER_WRITE_AND_COUNT(EST2);
|
||||
|
||||
log_msg.msg_type = LOG_EST3_MSG;
|
||||
|
|
|
|||
|
|
@ -410,9 +410,10 @@ struct log_EST1_s {
|
|||
/* --- EST2 - ESTIMATOR STATUS --- */
|
||||
#define LOG_EST2_MSG 34
|
||||
struct log_EST2_s {
|
||||
float cov[12];
|
||||
uint16_t gps_check_fail_flags;
|
||||
uint16_t control_mode_flags;
|
||||
float cov[12];
|
||||
uint16_t gps_check_fail_flags;
|
||||
uint16_t control_mode_flags;
|
||||
uint8_t health_flags;
|
||||
};
|
||||
|
||||
/* --- EST3 - ESTIMATOR STATUS --- */
|
||||
|
|
@ -687,7 +688,7 @@ static const struct log_format_s log_formats[] = {
|
|||
LOG_FORMAT_S(TEL3, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
|
||||
LOG_FORMAT(EST0, "ffffffffffffBBHB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fFault,fTOut"),
|
||||
LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"),
|
||||
LOG_FORMAT(EST2, "ffffffffffffHH", "P0,P1,P2,P3,P4,P5,P6,P7,P8,P9,P10,P11,GCHK,CTRL"),
|
||||
LOG_FORMAT(EST2, "ffffffffffffHHB", "P0,P1,P2,P3,P4,P5,P6,P7,P8,P9,P10,P11,GCHK,CTRL,fHealth"),
|
||||
LOG_FORMAT(EST3, "ffffffffffffffff", "P12,P13,P14,P15,P16,P17,P18,P19,P20,P21,P22,P23,P24,P25,P26,P27"),
|
||||
LOG_FORMAT(EST4, "ffffffffffff", "VxI,VyI,VzI,PxI,PyI,PzI,VxIV,VyIV,VzIV,PxIV,PyIV,PzIV"),
|
||||
LOG_FORMAT(EST5, "ffffffffff", "MAGxI,MAGyI,MAGzI,MAGxIV,MAGyIV,MAGzIV,HeadI,HeadIV,AirI,AirIV"),
|
||||
|
|
|
|||
Loading…
Reference in New Issue