BlockLocalPositionEstimator.cpp

Go to the documentation of this file.

#include "BlockLocalPositionEstimator.hpp"
#include <systemlib/mavlink_log.h>
#include <fcntl.h>
#include <systemlib/err.h>
#include <matrix/math.hpp>
#include <cstdlib>

orb_advert_t mavlink_log_pub = nullptr;

// 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

static const float P_MAX = 1.0e6f;  // max allowed value in state covariance
static const float LAND_RATE = 10.0f;   // rate of land detector correction

static const char *msg_label = "[lpe] ";    // rate of land detector correction

BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
    ModuleParams(nullptr),
    WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),

    // this block has no parent, and has name LPE
    SuperBlock(nullptr, "LPE"),

    // map projection
    _map_ref(),

    // flow gyro
    _flow_gyro_x_high_pass(this, "FGYRO_HP"),
    _flow_gyro_y_high_pass(this, "FGYRO_HP"),

    // stats
    _baroStats(this, ""),
    _sonarStats(this, ""),
    _lidarStats(this, ""),
    _flowQStats(this, ""),
    _visionStats(this, ""),
    _mocapStats(this, ""),
    _gpsStats(this, ""),

    // low pass
    _xLowPass(this, "X_LP"),

    // use same lp constant for agl
    _aglLowPass(this, "X_LP"),

    // delay
    _xDelay(this, ""),
    _tDelay(this, ""),

    // misc
    _timeStamp(hrt_absolute_time()),
    _time_origin(0),
    _timeStampLastBaro(hrt_absolute_time()),
    _time_last_hist(0),
    _time_last_flow(0),
    _time_last_baro(0),
    _time_last_gps(0),
    _time_last_lidar(0),
    _time_last_sonar(0),
    _time_init_sonar(0),
    _time_last_vision_p(0),
    _time_last_mocap(0),
    _time_last_land(0),
    _time_last_target(0),

    // reference altitudes
    _altOrigin(0),
    _altOriginInitialized(false),
    _altOriginGlobal(false),
    _baroAltOrigin(0),
    _gpsAltOrigin(0),

    // status
    _receivedGps(false),
    _lastArmedState(false),

    // masks
    _sensorTimeout(UINT16_MAX),
    _sensorFault(0),
    _estimatorInitialized(0),

    // sensor update flags
    _flowUpdated(false),
    _gpsUpdated(false),
    _visionUpdated(false),
    _mocapUpdated(false),
    _lidarUpdated(false),
    _sonarUpdated(false),
    _landUpdated(false),
    _baroUpdated(false),

    // sensor validation flags
    _vision_xy_valid(false),
    _vision_z_valid(false),
    _mocap_xy_valid(false),
    _mocap_z_valid(false),

    // sensor std deviations
    _vision_eph(0.0),
    _vision_epv(0.0),
    _mocap_eph(0.0),
    _mocap_epv(0.0),

    // local to global coversion related variables
    _is_global_cov_init(false),
    _global_ref_timestamp(0.0),
    _ref_lat(0.0),
    _ref_lon(0.0),
    _ref_alt(0.0)
{
    _sensors_sub.set_interval_ms(10); // main prediction loop, 100 hz

    // assign distance subs to array
    _dist_subs[0] = &_sub_dist0;
    _dist_subs[1] = &_sub_dist1;
    _dist_subs[2] = &_sub_dist2;
    _dist_subs[3] = &_sub_dist3;

    // initialize A, B,  P, x, u
    _x.setZero();
    _u.setZero();
    initSS();

    // map
    _map_ref.init_done = false;

    // print fusion settings to console
    PX4_INFO("fuse gps: %d, flow: %d, vis_pos: %d, "
         "landing_target: %d, land: %d, pub_agl_z: %d, flow_gyro: %d, "
         "baro: %d\n",
         (_param_lpe_fusion.get() & FUSE_GPS) != 0,
         (_param_lpe_fusion.get() & FUSE_FLOW) != 0,
         (_param_lpe_fusion.get() & FUSE_VIS_POS) != 0,
         (_param_lpe_fusion.get() & FUSE_LAND_TARGET) != 0,
         (_param_lpe_fusion.get() & FUSE_LAND) != 0,
         (_param_lpe_fusion.get() & FUSE_PUB_AGL_Z) != 0,
         (_param_lpe_fusion.get() & FUSE_FLOW_GYRO_COMP) != 0,
         (_param_lpe_fusion.get() & FUSE_BARO) != 0);
}

bool
BlockLocalPositionEstimator::init()
{
    if (!_sensors_sub.registerCallback()) {
        PX4_ERR("sensor combined callback registration failed!");
        return false;
    }

    return true;
}


Vector<float, BlockLocalPositionEstimator::n_x> BlockLocalPositionEstimator::dynamics(
    float t,
    const Vector<float, BlockLocalPositionEstimator::n_x> &x,
    const Vector<float, BlockLocalPositionEstimator::n_u> &u)
{
    return m_A * x + m_B * u;
}

void BlockLocalPositionEstimator::Run()
{
    if (should_exit()) {
        _sensors_sub.unregisterCallback();
        exit_and_cleanup();
        return;
    }

    sensor_combined_s imu;

    if (!_sensors_sub.update(&imu)) {
        return;
    }

    uint64_t newTimeStamp = hrt_absolute_time();
    float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
    _timeStamp = newTimeStamp;

    // set dt for all child blocks
    setDt(dt);

    // auto-detect connected rangefinders while not armed
    _sub_armed.update();
    bool armedState = _sub_armed.get().armed;

    if (!armedState && (_sub_lidar == nullptr || _sub_sonar == nullptr)) {
        // detect distance sensors
        for (size_t i = 0; i < N_DIST_SUBS; i++) {
            auto *s = _dist_subs[i];

            if (s == _sub_lidar || s == _sub_sonar) { continue; }

            if (s->update()) {

                if (s->get().timestamp == 0) { continue; }

                if (s->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_LASER &&
                    s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING &&
                    _sub_lidar == nullptr) {
                    _sub_lidar = s;
                    mavlink_and_console_log_info(&mavlink_log_pub, "%sDownward-facing Lidar detected with ID %zu", msg_label, i);

                } else if (s->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND &&
                       s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING &&
                       _sub_sonar == nullptr) {
                    _sub_sonar = s;
                    mavlink_and_console_log_info(&mavlink_log_pub, "%sDownward-facing Sonar detected with ID %zu", msg_label, i);
                }
            }
        }
    }

    // reset pos, vel, and terrain on arming

    // XXX this will be re-enabled for indoor use cases using a
    // selection param, but is really not helping outdoors
    // right now.

    // if (!_lastArmedState && armedState) {

    //      // we just armed, we are at origin on the ground
    //      _x(X_x) = 0;
    //      _x(X_y) = 0;
    //      // reset Z or not? _x(X_z) = 0;

    //      // we aren't moving, all velocities are zero
    //      _x(X_vx) = 0;
    //      _x(X_vy) = 0;
    //      _x(X_vz) = 0;

    //      // 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);
    //      _aglLowPass.setState(0);
    // }

    _lastArmedState = armedState;

    // see which updates are available
    bool paramsUpdated = _sub_param_update.update();
    _baroUpdated = false;

    if ((_param_lpe_fusion.get() & FUSE_BARO) && _sub_airdata.update()) {
        if (_sub_airdata.get().timestamp != _timeStampLastBaro) {
            _baroUpdated = true;
            _timeStampLastBaro = _sub_airdata.get().timestamp;
        }
    }

    _flowUpdated = (_param_lpe_fusion.get() & FUSE_FLOW) && _sub_flow.update();
    _gpsUpdated = (_param_lpe_fusion.get() & FUSE_GPS) && _sub_gps.update();
    _visionUpdated = (_param_lpe_fusion.get() & FUSE_VIS_POS) && _sub_visual_odom.update();
    _mocapUpdated = _sub_mocap_odom.update();
    _lidarUpdated = (_sub_lidar != nullptr) && _sub_lidar->update();
    _sonarUpdated = (_sub_sonar != nullptr) && _sub_sonar->update();
    _landUpdated = landed() && ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE);// throttle rate
    bool targetPositionUpdated = _sub_landing_target_pose.update();

    // get new data
    _sub_att.update();
    _sub_angular_velocity.update();

    // update parameters
    if (paramsUpdated) {
        SuperBlock::updateParams();
        ModuleParams::updateParams();
        updateSSParams();
    }

    // is xy valid?
    bool vxy_stddev_ok = false;

    if (math::max(m_P(X_vx, X_vx), m_P(X_vy, X_vy)) < _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get()) {
        vxy_stddev_ok = true;
    }

    if (_estimatorInitialized & EST_XY) {
        // if valid and gps has timed out, set to not valid
        if (!vxy_stddev_ok && (_sensorTimeout & SENSOR_GPS)) {
            _estimatorInitialized &= ~EST_XY;
        }

    } else {
        if (vxy_stddev_ok) {
            if (!(_sensorTimeout & SENSOR_GPS)
                || !(_sensorTimeout & SENSOR_FLOW)
                || !(_sensorTimeout & SENSOR_VISION)
                || !(_sensorTimeout & SENSOR_MOCAP)
                || !(_sensorTimeout & SENSOR_LAND)
                || !(_sensorTimeout & SENSOR_LAND_TARGET)
               ) {
                _estimatorInitialized |= EST_XY;
            }
        }
    }

    // is z valid?
    bool z_stddev_ok = sqrtf(m_P(X_z, X_z)) < _param_lpe_z_pub.get();

    if (_estimatorInitialized & EST_Z) {
        // if valid and baro has timed out, set to not valid
        if (!z_stddev_ok && (_sensorTimeout & SENSOR_BARO)) {
            _estimatorInitialized &= ~EST_Z;
        }

    } else {
        if (z_stddev_ok) {
            _estimatorInitialized |= EST_Z;
        }
    }

    // is terrain valid?
    bool tz_stddev_ok = sqrtf(m_P(X_tz, X_tz)) < _param_lpe_z_pub.get();

    if (_estimatorInitialized & EST_TZ) {
        if (!tz_stddev_ok) {
            _estimatorInitialized &= ~EST_TZ;
        }

    } else {
        if (tz_stddev_ok) {
            _estimatorInitialized |= EST_TZ;
        }
    }

    // check timeouts
    checkTimeouts();

    // if we have no lat, lon initialize projection to LPE_LAT, LPE_LON parameters
    if (!_map_ref.init_done && (_estimatorInitialized & EST_XY) && _param_lpe_fake_origin.get()) {
        map_projection_init(&_map_ref,
                    (double)_param_lpe_lat.get(),
                    (double)_param_lpe_lon.get());

        // set timestamp when origin was set to current time
        _time_origin = _timeStamp;

        mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (parameter) : lat %6.2f lon %6.2f alt %5.1f m",
                         double(_param_lpe_lat.get()), double(_param_lpe_lon.get()), double(_altOrigin));
    }

    // reinitialize x if necessary
    bool reinit_x = false;

    for (size_t i = 0; i < n_x; i++) {
        // should we do a reinit
        // of sensors here?
        // don't want it to take too long
        if (!PX4_ISFINITE(_x(i))) {
            reinit_x = true;
            mavlink_and_console_log_info(&mavlink_log_pub, "%sreinit x, x(%zu) not finite", msg_label, i);
            break;
        }
    }

    if (reinit_x) {
        for (size_t i = 0; i < n_x; i++) {
            _x(i) = 0;
        }

        mavlink_and_console_log_info(&mavlink_log_pub, "%sreinit x", msg_label);
    }

    // force P symmetry and reinitialize P if necessary
    bool reinit_P = false;

    for (size_t i = 0; i < n_x; i++) {
        for (size_t j = 0; j <= i; j++) {
            if (!PX4_ISFINITE(m_P(i, j))) {
                mavlink_and_console_log_info(&mavlink_log_pub,
                                 "%sreinit P (%zu, %zu) not finite", msg_label, i, j);
                reinit_P = true;
            }

            if (i == j) {
                // make sure diagonal elements are positive
                if (m_P(i, i) <= 0) {
                    mavlink_and_console_log_info(&mavlink_log_pub,
                                     "%sreinit P (%zu, %zu) negative", msg_label, i, j);
                    reinit_P = true;
                }

            } else {
                // copy elememnt from upper triangle to force
                // symmetry
                m_P(j, i) = m_P(i, j);
            }

            if (reinit_P) { break; }
        }

        if (reinit_P) { break; }
    }

    if (reinit_P) {
        initP();
    }

    // do prediction
    predict(imu);

    // sensor corrections/ initializations
    if (_gpsUpdated) {
        if (_sensorTimeout & SENSOR_GPS) {
            gpsInit();

        } else {
            gpsCorrect();
        }
    }

    if (_baroUpdated) {
        if (_sensorTimeout & SENSOR_BARO) {
            baroInit();

        } else {
            baroCorrect();
        }
    }

    if (_lidarUpdated) {
        if (_sensorTimeout & SENSOR_LIDAR) {
            lidarInit();

        } else {
            lidarCorrect();
        }
    }

    if (_sonarUpdated) {
        if (_sensorTimeout & SENSOR_SONAR) {
            sonarInit();

        } else {
            sonarCorrect();
        }
    }

    if (_flowUpdated) {
        if (_sensorTimeout & SENSOR_FLOW) {
            flowInit();

        } else {
            flowCorrect();
        }
    }

    if (_visionUpdated) {
        if (_sensorTimeout & SENSOR_VISION) {
            visionInit();

        } else {
            visionCorrect();
        }
    }

    if (_mocapUpdated) {
        if (_sensorTimeout & SENSOR_MOCAP) {
            mocapInit();

        } else {
            mocapCorrect();
        }
    }

    if (_landUpdated) {
        if (_sensorTimeout & SENSOR_LAND) {
            landInit();

        } else {
            landCorrect();
        }
    }

    if (targetPositionUpdated) {
        if (_sensorTimeout & SENSOR_LAND_TARGET) {
            landingTargetInit();

        } else {
            landingTargetCorrect();
        }
    }

    if (_altOriginInitialized) {
        // update all publications if possible
        publishLocalPos();
        publishOdom();
        publishEstimatorStatus();
        _pub_innov.get().timestamp = _timeStamp;
        _pub_innov.update();

        if ((_estimatorInitialized & EST_XY) && (_map_ref.init_done || _param_lpe_fake_origin.get())) {
            publishGlobalPos();
        }
    }

    // propagate delayed state, no matter what
    // if state is frozen, delayed state still
    // needs to be propagated with frozen state
    float dt_hist = 1.0e-6f * (_timeStamp - _time_last_hist);

    if (_time_last_hist == 0 ||
        (dt_hist > HIST_STEP)) {
        _tDelay.update(Scalar<uint64_t>(_timeStamp));
        _xDelay.update(_x);
        _time_last_hist = _timeStamp;
    }
}

void BlockLocalPositionEstimator::checkTimeouts()
{
    baroCheckTimeout();
    gpsCheckTimeout();
    lidarCheckTimeout();
    flowCheckTimeout();
    sonarCheckTimeout();
    visionCheckTimeout();
    mocapCheckTimeout();
    landCheckTimeout();
    landingTargetCheckTimeout();
}

bool BlockLocalPositionEstimator::landed()
{
    if (!(_param_lpe_fusion.get() & FUSE_LAND)) {
        return false;
    }

    _sub_land.update();

    bool disarmed_not_falling = (!_sub_armed.get().armed) && (!_sub_land.get().freefall);

    return _sub_land.get().landed || disarmed_not_falling;
}

void BlockLocalPositionEstimator::publishLocalPos()
{
    const Vector<float, n_x> &xLP = _xLowPass.getState();

    // lie about eph/epv to allow visual odometry only navigation when velocity est. good
    float evh = sqrtf(m_P(X_vx, X_vx) + m_P(X_vy, X_vy));
    float evv = sqrtf(m_P(X_vz, X_vz));
    float eph = sqrtf(m_P(X_x, X_x) + m_P(X_y, X_y));
    float epv = sqrtf(m_P(X_z, X_z));

    float eph_thresh = 3.0f;
    float epv_thresh = 3.0f;

    if (evh < _param_lpe_vxy_pub.get()) {
        if (eph > eph_thresh) {
            eph = eph_thresh;
        }

        if (epv > epv_thresh) {
            epv = epv_thresh;
        }
    }

    // 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 = _estimatorInitialized & EST_XY;
        _pub_lpos.get().z_valid = _estimatorInitialized & EST_Z;
        _pub_lpos.get().v_xy_valid = _estimatorInitialized & EST_XY;
        _pub_lpos.get().v_z_valid = _estimatorInitialized & EST_Z;

        _pub_lpos.get().x = xLP(X_x);   // north
        _pub_lpos.get().y = xLP(X_y);   // east

        if (_param_lpe_fusion.get() & FUSE_PUB_AGL_Z) {
            _pub_lpos.get().z = -_aglLowPass.getState();    // agl

        } else {
            _pub_lpos.get().z = xLP(X_z);   // down
        }

        _pub_gpos.get().yaw = matrix::Eulerf(matrix::Quatf(_sub_att.get().q)).psi();

        _pub_lpos.get().vx = xLP(X_vx);     // north
        _pub_lpos.get().vy = xLP(X_vy);     // east
        _pub_lpos.get().vz = xLP(X_vz);     // down

        // this estimator does not provide a separate vertical position time derivative estimate, so use the vertical velocity
        _pub_lpos.get().z_deriv = xLP(X_vz);

        _pub_lpos.get().ax = _u(U_ax);      // north
        _pub_lpos.get().ay = _u(U_ay);      // east
        _pub_lpos.get().az = _u(U_az);      // down

        _pub_lpos.get().xy_global = _estimatorInitialized & EST_XY;
        _pub_lpos.get().z_global = !(_sensorTimeout & SENSOR_BARO) && _altOriginGlobal;
        _pub_lpos.get().ref_timestamp = _time_origin;
        _pub_lpos.get().ref_lat = _map_ref.lat_rad * 180 / M_PI;
        _pub_lpos.get().ref_lon = _map_ref.lon_rad * 180 / M_PI;
        _pub_lpos.get().ref_alt = _altOrigin;
        _pub_lpos.get().dist_bottom = _aglLowPass.getState();
        _pub_lpos.get().dist_bottom_rate = -xLP(X_vz);
        // we estimate agl even when we don't have terrain info
        // if you are in terrain following mode this is important
        // so that if terrain estimation fails there isn't a
        // sudden altitude jump
        _pub_lpos.get().dist_bottom_valid = _estimatorInitialized & EST_Z;
        _pub_lpos.get().eph = eph;
        _pub_lpos.get().epv = epv;
        _pub_lpos.get().evh = evh;
        _pub_lpos.get().evv = evv;
        _pub_lpos.get().vxy_max = INFINITY;
        _pub_lpos.get().vz_max = INFINITY;
        _pub_lpos.get().hagl_min = INFINITY;
        _pub_lpos.get().hagl_max = INFINITY;
        _pub_lpos.update();
    }
}

void BlockLocalPositionEstimator::publishOdom()
{
    const Vector<float, n_x> &xLP = _xLowPass.getState();

    // publish vehicle odometry
    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_odom.get().timestamp = _timeStamp;
        _pub_odom.get().local_frame = _pub_odom.get().LOCAL_FRAME_NED;

        // position
        _pub_odom.get().x = xLP(X_x);   // north
        _pub_odom.get().y = xLP(X_y);   // east

        if (_param_lpe_fusion.get() & FUSE_PUB_AGL_Z) {
            _pub_odom.get().z = -_aglLowPass.getState();    // agl

        } else {
            _pub_odom.get().z = xLP(X_z);   // down
        }

        // orientation
        matrix::Quatf q = matrix::Quatf(_sub_att.get().q);
        q.copyTo(_pub_odom.get().q);

        // linear velocity
        _pub_odom.get().vx = xLP(X_vx);     // vel north
        _pub_odom.get().vy = xLP(X_vy);     // vel east
        _pub_odom.get().vz = xLP(X_vz);     // vel down

        // angular velocity
        _pub_odom.get().rollspeed = _sub_angular_velocity.get().xyz[0]; // roll rate
        _pub_odom.get().pitchspeed = _sub_angular_velocity.get().xyz[1]; // pitch rate
        _pub_odom.get().yawspeed = _sub_angular_velocity.get().xyz[2]; // yaw rate

        // get the covariance matrix size
        const size_t POS_URT_SIZE = sizeof(_pub_odom.get().pose_covariance) / sizeof(_pub_odom.get().pose_covariance[0]);
        const size_t VEL_URT_SIZE = sizeof(_pub_odom.get().velocity_covariance) / sizeof(
                            _pub_odom.get().velocity_covariance[0]);

        // initially set pose covariances to 0
        for (size_t i = 0; i < POS_URT_SIZE; i++) {
            _pub_odom.get().pose_covariance[i] = 0.0;
        }

        // set the position variances
        _pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_X_VARIANCE] = m_P(X_vx, X_vx);
        _pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_Y_VARIANCE] = m_P(X_vy, X_vy);
        _pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_Z_VARIANCE] = m_P(X_vz, X_vz);

        // unknown orientation covariances
        // TODO: add orientation covariance to vehicle_attitude
        _pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_ROLL_VARIANCE] = NAN;
        _pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_PITCH_VARIANCE] = NAN;
        _pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_YAW_VARIANCE] = NAN;

        // initially set velocity covariances to 0
        for (size_t i = 0; i < VEL_URT_SIZE; i++) {
            _pub_odom.get().velocity_covariance[i] = 0.0;
        }

        // set the linear velocity variances
        _pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_VX_VARIANCE] = m_P(X_vx, X_vx);
        _pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_VY_VARIANCE] = m_P(X_vy, X_vy);
        _pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_VZ_VARIANCE] = m_P(X_vz, X_vz);

        // unknown angular velocity covariances
        _pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_ROLLRATE_VARIANCE] = NAN;
        _pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_PITCHRATE_VARIANCE] = NAN;
        _pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_YAWRATE_VARIANCE] = NAN;

        _pub_odom.update();
    }
}

void BlockLocalPositionEstimator::publishEstimatorStatus()
{
    _pub_est_status.get().timestamp = _timeStamp;

    for (size_t i = 0; i < n_x; i++) {
        _pub_est_status.get().states[i] = _x(i);
    }

    // matching EKF2 covariances indexing
    // quaternion - not determined, as it is a position estimator
    _pub_est_status.get().covariances[0] = NAN;
    _pub_est_status.get().covariances[1] = NAN;
    _pub_est_status.get().covariances[2] = NAN;
    _pub_est_status.get().covariances[3] = NAN;
    // linear velocity
    _pub_est_status.get().covariances[4] = m_P(X_vx, X_vx);
    _pub_est_status.get().covariances[5] = m_P(X_vy, X_vy);
    _pub_est_status.get().covariances[6] = m_P(X_vz, X_vz);
    // position
    _pub_est_status.get().covariances[7] = m_P(X_x, X_x);
    _pub_est_status.get().covariances[8] = m_P(X_y, X_y);
    _pub_est_status.get().covariances[9] = m_P(X_z, X_z);
    // gyro bias - not determined
    _pub_est_status.get().covariances[10] = NAN;
    _pub_est_status.get().covariances[11] = NAN;
    _pub_est_status.get().covariances[12] = NAN;
    // accel bias
    _pub_est_status.get().covariances[13] = m_P(X_bx, X_bx);
    _pub_est_status.get().covariances[14] = m_P(X_by, X_by);
    _pub_est_status.get().covariances[15] = m_P(X_bz, X_bz);

    // mag - not determined
    for (size_t i = 16; i <= 21; i++) {
        _pub_est_status.get().covariances[i] = NAN;
    }

    // replacing the hor wind cov with terrain altitude covariance
    _pub_est_status.get().covariances[22] = m_P(X_tz, X_tz);
    _pub_est_status.get().covariances[23] = NAN;

    _pub_est_status.get().n_states = n_x;
    _pub_est_status.get().health_flags = _sensorFault;
    _pub_est_status.get().timeout_flags = _sensorTimeout;
    _pub_est_status.get().pos_horiz_accuracy = _pub_gpos.get().eph;
    _pub_est_status.get().pos_vert_accuracy = _pub_gpos.get().epv;

    _pub_est_status.update();
}

void BlockLocalPositionEstimator::publishGlobalPos()
{
    // publish global position
    double lat = 0;
    double lon = 0;
    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) + _altOrigin;

    // lie about eph/epv to allow visual odometry only navigation when velocity est. good
    float evh = sqrtf(m_P(X_vx, X_vx) + m_P(X_vy, X_vy));
    float eph = sqrtf(m_P(X_x, X_x) + m_P(X_y, X_y));
    float epv = sqrtf(m_P(X_z, X_z));

    float eph_thresh = 3.0f;
    float epv_thresh = 3.0f;

    if (evh < _param_lpe_vxy_pub.get()) {
        if (eph > eph_thresh) {
            eph = eph_thresh;
        }

        if (epv > epv_thresh) {
            epv = epv_thresh;
        }
    }

    if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt) &&
        PX4_ISFINITE(xLP(X_vx)) && PX4_ISFINITE(xLP(X_vy)) &&
        PX4_ISFINITE(xLP(X_vz))) {
        _pub_gpos.get().timestamp = _timeStamp;
        _pub_gpos.get().lat = lat;
        _pub_gpos.get().lon = lon;
        _pub_gpos.get().alt = alt;
        _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 = matrix::Eulerf(matrix::Quatf(_sub_att.get().q)).psi();
        _pub_gpos.get().eph = eph;
        _pub_gpos.get().epv = epv;
        _pub_gpos.get().terrain_alt = _altOrigin - xLP(X_tz);
        _pub_gpos.get().terrain_alt_valid = _estimatorInitialized & EST_TZ;
        _pub_gpos.get().dead_reckoning = !(_estimatorInitialized & EST_XY);
        _pub_gpos.update();
    }
}

void BlockLocalPositionEstimator::initP()
{
    m_P.setZero();
    // initialize to twice valid condition
    m_P(X_x, X_x) = 2 * EST_STDDEV_XY_VALID * EST_STDDEV_XY_VALID;
    m_P(X_y, X_y) = 2 * EST_STDDEV_XY_VALID * EST_STDDEV_XY_VALID;
    m_P(X_z, X_z) = 2 * EST_STDDEV_Z_VALID * EST_STDDEV_Z_VALID;
    m_P(X_vx, X_vx) = 2 * _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get();
    m_P(X_vy, X_vy) = 2 * _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get();
    // use vxy thresh for vz init as well
    m_P(X_vz, X_vz) = 2 * _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get();
    // initialize bias uncertainty to small values to keep them stable
    m_P(X_bx, X_bx) = 1e-6;
    m_P(X_by, X_by) = 1e-6;
    m_P(X_bz, X_bz) = 1e-6;
    m_P(X_tz, X_tz) = 2 * EST_STDDEV_TZ_VALID * EST_STDDEV_TZ_VALID;
}

void BlockLocalPositionEstimator::initSS()
{
    initP();

    // dynamics matrix
    m_A.setZero();
    // derivative of position is velocity
    m_A(X_x, X_vx) = 1;
    m_A(X_y, X_vy) = 1;
    m_A(X_z, X_vz) = 1;

    // input matrix
    m_B.setZero();
    m_B(X_vx, U_ax) = 1;
    m_B(X_vy, U_ay) = 1;
    m_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)
    m_A(X_vx, X_bx) = -_R_att(0, 0);
    m_A(X_vx, X_by) = -_R_att(0, 1);
    m_A(X_vx, X_bz) = -_R_att(0, 2);

    m_A(X_vy, X_bx) = -_R_att(1, 0);
    m_A(X_vy, X_by) = -_R_att(1, 1);
    m_A(X_vy, X_bz) = -_R_att(1, 2);

    m_A(X_vz, X_bx) = -_R_att(2, 0);
    m_A(X_vz, X_by) = -_R_att(2, 1);
    m_A(X_vz, X_bz) = -_R_att(2, 2);
}

void BlockLocalPositionEstimator::updateSSParams()
{
    // input noise covariance matrix
    m_R.setZero();
    m_R(U_ax, U_ax) = _param_lpe_acc_xy.get() * _param_lpe_acc_xy.get();
    m_R(U_ay, U_ay) = _param_lpe_acc_xy.get() * _param_lpe_acc_xy.get();
    m_R(U_az, U_az) = _param_lpe_acc_z.get() * _param_lpe_acc_z.get();

    // process noise power matrix
    m_Q.setZero();
    float pn_p_sq = _param_lpe_pn_p.get() * _param_lpe_pn_p.get();
    float pn_v_sq = _param_lpe_pn_v.get() * _param_lpe_pn_v.get();
    m_Q(X_x, X_x) = pn_p_sq;
    m_Q(X_y, X_y) = pn_p_sq;
    m_Q(X_z, X_z) = pn_p_sq;
    m_Q(X_vx, X_vx) = pn_v_sq;
    m_Q(X_vy, X_vy) = pn_v_sq;
    m_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 = _param_lpe_pn_b.get() * _param_lpe_pn_b.get();
    m_Q(X_bx, X_bx) = pn_b_sq;
    m_Q(X_by, X_by) = pn_b_sq;
    m_Q(X_bz, X_bz) = pn_b_sq;

    // terrain random walk noise ((m/s)/sqrt(hz)), scales with velocity
    float pn_t_noise_density =
        _param_lpe_pn_t.get() +
        (_param_lpe_t_max_grade.get() / 100.0f) * sqrtf(_x(X_vx) * _x(X_vx) + _x(X_vy) * _x(X_vy));
    m_Q(X_tz, X_tz) = pn_t_noise_density * pn_t_noise_density;
}

void BlockLocalPositionEstimator::predict(const sensor_combined_s &imu)
{
    // get acceleration
    _R_att = matrix::Dcm<float>(matrix::Quatf(_sub_att.get().q));
    Vector3f a(imu.accelerometer_m_s2);
    // note, bias is removed in dynamics function
    _u = _R_att * a;
    _u(U_az) += CONSTANTS_ONE_G;    // add g

    // update state space based on new states
    updateSSStates();

    // continuous time kalman filter prediction
    // 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);

    // don't integrate position if no valid xy data
    if (!(_estimatorInitialized & EST_XY))  {
        dx(X_x) = 0;
        dx(X_vx) = 0;
        dx(X_y) = 0;
        dx(X_vy) = 0;
    }

    // don't integrate z if no valid z data
    if (!(_estimatorInitialized & EST_Z))  {
        dx(X_z) = 0;
    }

    // don't integrate tz if no valid tz data
    if (!(_estimatorInitialized & EST_TZ))  {
        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);
        dx(X_bx) = bx - _x(X_bx);
    }

    if (std::abs(by) > BIAS_MAX) {
        by = BIAS_MAX * by / std::abs(by);
        dx(X_by) = by - _x(X_by);
    }

    if (std::abs(bz) > BIAS_MAX) {
        bz = BIAS_MAX * bz / std::abs(bz);
        dx(X_bz) = bz - _x(X_bz);
    }

    // propagate
    _x += dx;
    Matrix<float, n_x, n_x> dP = (m_A * m_P + m_P * m_A.transpose() +
                      m_B * m_R * m_B.transpose() + m_Q) * getDt();

    // covariance propagation logic
    for (size_t i = 0; i < n_x; i++) {
        if (m_P(i, i) > P_MAX) {
            // if diagonal element greater than max, stop propagating
            dP(i, i) = 0;

            for (size_t j = 0; j < n_x; j++) {
                dP(i, j) = 0;
                dP(j, i) = 0;
            }
        }
    }

    m_P += dP;
    _xLowPass.update(_x);
    _aglLowPass.update(agl());
}

int BlockLocalPositionEstimator::getDelayPeriods(float delay, uint8_t *periods)
{
    float t_delay = 0;
    uint8_t i_hist = 0;

    for (i_hist = 1; i_hist < HIST_LEN; i_hist++) {
        t_delay = 1.0e-6f * (_timeStamp - _tDelay.get(i_hist)(0, 0));

        if (t_delay > delay) {
            break;
        }
    }

    *periods = i_hist;

    if (t_delay > DELAY_MAX) {
        mavlink_and_console_log_info(&mavlink_log_pub, "%sdelayed data old: %8.4f", msg_label, double(t_delay));
        return -1;
    }

    return OK;
}

int
BlockLocalPositionEstimator::custom_command(int argc, char *argv[])
{
    return print_usage("unknown command");
}

int
BlockLocalPositionEstimator::task_spawn(int argc, char *argv[])
{
    BlockLocalPositionEstimator *instance = new BlockLocalPositionEstimator();

    if (instance) {
        _object.store(instance);
        _task_id = task_id_is_work_queue;

        if (instance->init()) {
            return PX4_OK;
        }

    } else {
        PX4_ERR("alloc failed");
    }

    delete instance;
    _object.store(nullptr);
    _task_id = -1;

    return PX4_ERROR;
}

int
BlockLocalPositionEstimator::print_usage(const char *reason)
{
    if (reason) {
        PX4_WARN("%s\n", reason);
    }

    PRINT_MODULE_DESCRIPTION(
        R"DESCR_STR(
### Description
Attitude and position estimator using an Extended Kalman Filter.

)DESCR_STR");

    PRINT_MODULE_USAGE_NAME("local_position_estimator", "estimator");
    PRINT_MODULE_USAGE_COMMAND("start");
    PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

    return 0;
}

extern "C" __EXPORT int local_position_estimator_main(int argc, char *argv[])
{
    return BlockLocalPositionEstimator::main(argc, argv);
}