前言:之前学习的是px4源码中的attitude_estimator_q,可以说是学习的相当仔细和深入。于是借着这股劲继续学习位置估计中的local position estimator,另外需要说明的是,在px4源码中还有一个位置估计是position_estimator_inav,这个是基于导航的位置估计。
一、文件夹下有什么?
我们先看看文件夹下有什么,如图:
说明:
1.sensors文件夹下是进行位置估计时用到的模块:气压,光流,gps,mocap,超声波,视觉等的初始化和卡尔曼滤波的修正函数,这些函数在BlockLocalPositionEstimator.cpp中被调用。
2.BlockLocalPositionEstimator.cpp和BlockLocalPositionEstimator.h是BlockLocalPositionEstimator这个类的定义,.h里面是类的成员,cpp里面是成员的定义,这个类就是lpe的核心,后面专门用一节讲这个类。
3.local_position_estimator_main.cpp, 这个文件是nuttx操作系统的一个应用程序,它启动local_position_estimator_thread_main这个任务,这个任务函数的代码比较短。
二、sensor文件夹介绍
上面说过,sensors文件夹下是进行位置估计时用到的模块,用到的模块有气压,光流,gps,mocap,超声波,视觉,这些cpp文件的写法风格和函数名称都比较类似,这里选择其中一个mocap进行介绍,因为mocap中三维的消息都有,其他超声波只有在高度,光流只有平面位置,gps只有在室外才能用,视觉的门槛高,精度又不高。
在mocap中只有4个函数(其他模块也一样)。
mocapInit:
在第一次连续接受到20帧mocap消息就任务初始化完毕,这个函数在上电后第一次使用或者mocap消息超时以后会被调用。mocap消息超时指连续200ms没有接收到mocap消息,这时px4认为mocap消息断开了,会重新调用初始化函数重新开始。在实际测试过程中,由于通信的原因,偶尔的两帧消息之间超过200ms导致的超时不会产生很大的影响,一般在1s之内它能够重新初始化,或者把源码中的REQ_MOCAP_INIT_COUNT改小也可以使偶尔因为通信带来的超时造成的影响变小。
mocapMeasure:
这函数把订阅到的mocap消息赋值到卡尔曼的测量矩阵,并记录收到mocap消息的时间
mocapCorrect:
卡尔曼修正函数,里面是数学矩阵的运算,具体可以查阅卡尔曼滤波相关内容。
mocapCheckTimeout:
检查是否超时。
三、BlockLocalPositionEstimator类
这个类中以publish开头的函数是把估计出的位置通过uORB发送出去。
在讲下面两个函数前先简单说说卡尔曼的函数实现,其实就4个字,“预测”,“修正”,lpe用加速度计信息和飞机当前姿态进行位置的预测,用sensors文件夹的各个模块进行修正。
predict函数:这个函数利用当前的加速度计信息和当前飞机的姿态进行位置的预测。使用了runge kutta methods 的卡尔曼的公式。
update函数中调用了上一节中各个模块的修正函数,比如mocapCorrect,当然在调用前有一些判断机制,看mocap消息是否有更新,是否启用mocap,是否超时等。还调用了预测函数predict。
其他的一些函数是对卡尔曼滤波矩阵的赋值,有些相当于滤波参数,可以直接在地面站进行修改。这里用的卡尔曼滤波的A矩阵大小是10*10
四、具体的源码
#include "BlockLocalPositionEstimator.hpp"
#include <drivers/drv_hrt.h> //之前介绍过,和rtc有关
#include <systemlib/mavlink_log.h>
#include <fcntl.h> //与文件控制操作有关,fcntl=file control
#include <systemlib/err.h>
#include <matrix/math.hpp>
#include <cstdlib> //cstdlib是C++里面的一个常用函数库, 等价于C中的<stdlib.h>。
orb_advert_t mavlink_log_pub = nullptr;
static const uint32_t EST_STDDEV_XY_VALID = 2.0;
static const uint32_t EST_STDDEV_Z_VALID = 2.0;
static const uint32_t EST_STDDEV_TZ_VALID = 2.0;
static const float P_MAX = 1.0e6f;
static const float LAND_RATE = 10.0f;
static const char *msg_label = "[lpe] ";
BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
SuperBlock(nullptr, "LPE"),
ModuleParams(nullptr),
_sub_armed(ORB_ID(actuator_armed), 1000 / 2, 0, &getSubscriptions()),
_sub_land(ORB_ID(vehicle_land_detected), 1000 / 2, 0, &getSubscriptions()),
_sub_att(ORB_ID(vehicle_attitude), 1000 / 100, 0, &getSubscriptions()),
_sub_flow(ORB_ID(optical_flow), 1000 / 100, 0, &getSubscriptions()),
_sub_sensor(ORB_ID(sensor_combined), 1000 / 100, 0, &getSubscriptions()),
_sub_param_update(ORB_ID(parameter_update), 1000 / 2, 0, &getSubscriptions()),
_sub_gps(ORB_ID(vehicle_gps_position), 1000 / 10, 0, &getSubscriptions()),
_sub_vision_pos(ORB_ID(vehicle_vision_position), 1000 / 50, 0, &getSubscriptions()),
_sub_mocap(ORB_ID(att_pos_mocap), 1000 / 50, 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(nullptr),
_sub_sonar(nullptr),
_sub_landing_target_pose(ORB_ID(landing_target_pose), 1000 / 40, 0, &getSubscriptions()),
_pub_lpos(ORB_ID(vehicle_local_position), -1, &getPublications()),
_pub_gpos(ORB_ID(vehicle_global_position), -1, &getPublications()),
_pub_est_status(ORB_ID(estimator_status), -1, &getPublications()),
_pub_innov(ORB_ID(ekf2_innovations), -1, &getPublications()),
_map_ref(),
_flow_gyro_x_high_pass(this, "FGYRO_HP"),
_flow_gyro_y_high_pass(this, "FGYRO_HP"),
_baroStats(this, ""),
_sonarStats(this, ""),
_lidarStats(this, ""),
_flowQStats(this, ""),
_visionStats(this, ""),
_mocapStats(this, ""),
_gpsStats(this, ""),
_xLowPass(this, "X_LP"),
_aglLowPass(this, "X_LP"),
_xDelay(this, ""),
_tDelay(this, ""),
_polls(),
_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),
_altOrigin(0),
_altOriginInitialized(false),
_altOriginGlobal(false),
_baroAltOrigin(0),
_gpsAltOrigin(0),
_receivedGps(false),
_lastArmedState(false),
_sensorTimeout(UINT16_MAX),
_sensorFault(0),
_estimatorInitialized(0),
_flowUpdated(false),
_gpsUpdated(false),
_visionUpdated(false),
_mocapUpdated(false),
_lidarUpdated(false),
_sonarUpdated(false),
_landUpdated(false),
_baroUpdated(false)
{
_dist_subs[0] = &_sub_dist0;
_dist_subs[1] = &_sub_dist1;
_dist_subs[2] = &_sub_dist2;
_dist_subs[3] = &_sub_dist3;
_polls[POLL_FLOW].fd = _sub_flow.getHandle();
_polls[POLL_FLOW].events = POLLIN;
_polls[POLL_PARAM].fd = _sub_param_update.getHandle();
_polls[POLL_PARAM].events = POLLIN;
_polls[POLL_SENSORS].fd = _sub_sensor.getHandle();
_polls[POLL_SENSORS].events = POLLIN;
_x.setZero();
_u.setZero();
initSS();
_map_ref.init_done = false;
printf("[lpe] fuse gps: %d, flow: %d, vis_pos: %d, "
"landing_target: %d, land: %d, pub_agl_z: %d, flow_gyro: %d, "
"baro: %d\n",
(_fusion.get() & FUSE_GPS) != 0,
(_fusion.get() & FUSE_FLOW) != 0,
(_fusion.get() & FUSE_VIS_POS) != 0,
(_fusion.get() & FUSE_LAND_TARGET) != 0,
(_fusion.get() & FUSE_LAND) != 0,
(_fusion.get() & FUSE_PUB_AGL_Z) != 0,
(_fusion.get() & FUSE_FLOW_GYRO_COMP) != 0,
(_fusion.get() & FUSE_BARO) != 0);
}
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()
{
int ret = px4_poll(_polls, 3, 100);
if (ret < 0)
{
return;
}
uint64_t newTimeStamp = hrt_absolute_time();
float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
_timeStamp = newTimeStamp;
setDt(dt);
bool armedState = _sub_armed.get().armed;
if (!armedState && (_sub_lidar == nullptr || _sub_sonar == nullptr))
{
for (int i = 0; i < N_DIST_SUBS; i++)
{
uORB::Subscription<distance_sensor_s> *s = _dist_subs[i];
if (s == _sub_lidar || s == _sub_sonar)
{
continue;
}
if (s->updated())
{
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 %i", 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 %i", msg_label, i);
}
}
}
}
_lastArmedState = armedState;
bool paramsUpdated = _sub_param_update.updated();
_baroUpdated = false;
if ((_fusion.get() & FUSE_BARO) && _sub_sensor.updated())
{
int32_t baro_timestamp_relative = _sub_sensor.get().baro_timestamp_relative;
if (baro_timestamp_relative != _sub_sensor.get().RELATIVE_TIMESTAMP_INVALID)
{
uint64_t baro_timestamp = _sub_sensor.get().timestamp + \
_sub_sensor.get().baro_timestamp_relative;
if (baro_timestamp != _timeStampLastBaro)
{
_baroUpdated = true;
_timeStampLastBaro = baro_timestamp;
}
}
}
_flowUpdated = (_fusion.get() & FUSE_FLOW) && _sub_flow.updated();
_gpsUpdated = (_fusion.get() & FUSE_GPS) && _sub_gps.updated();
_visionUpdated = (_fusion.get() & FUSE_VIS_POS) && _sub_vision_pos.updated();
_mocapUpdated = _sub_mocap.updated();
_lidarUpdated = (_sub_lidar != nullptr) && _sub_lidar->updated();
_sonarUpdated = (_sub_sonar != nullptr) && _sub_sonar->updated();
_landUpdated = landed() && ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE);
bool targetPositionUpdated = _sub_landing_target_pose.updated();
updateSubscriptions();
if (paramsUpdated)
{
SuperBlock::updateParams();
ModuleParams::updateParams();
updateSSParams();
}
bool vxy_stddev_ok = false;
if (math::max(_P(X_vx, X_vx), _P(X_vy, X_vy)) < _vxy_pub_thresh.get() * _vxy_pub_thresh.get())
{
vxy_stddev_ok = true;
}
if (_estimatorInitialized & EST_XY)
{
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;
}
}
}
bool z_stddev_ok = sqrtf(_P(X_z, X_z)) < _z_pub_thresh.get();
if (_estimatorInitialized & EST_Z)
{
if (!z_stddev_ok && (_sensorTimeout & SENSOR_BARO))
{
_estimatorInitialized &= ~EST_Z;
}
}
else
{
if (z_stddev_ok)
{
_estimatorInitialized |= EST_Z;
}
}
bool tz_stddev_ok = sqrtf(_P(X_tz, X_tz)) < _z_pub_thresh.get();
if (_estimatorInitialized & EST_TZ) {
if (!tz_stddev_ok) {
_estimatorInitialized &= ~EST_TZ;
}
} else {
if (tz_stddev_ok) {
_estimatorInitialized |= EST_TZ;
}
}
checkTimeouts();
if (!_map_ref.init_done && (_estimatorInitialized & EST_XY) && _fake_origin.get())
{
map_projection_init(&_map_ref,
_init_origin_lat.get(),
_init_origin_lon.get());
_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(_init_origin_lat.get()), double(_init_origin_lon.get()), double(_altOrigin));
}
bool reinit_x = false;
for (int i = 0; i < n_x; i++)
{
if (!PX4_ISFINITE(_x(i)))
{
reinit_x = true;
mavlink_and_console_log_info(&mavlink_log_pub, "%sreinit x, x(%d) not finite", msg_label, i);
break;
}
}
if (reinit_x)
{
for (int i = 0; i < n_x; i++)
{
_x(i) = 0;
}
mavlink_and_console_log_info(&mavlink_log_pub, "%sreinit x", msg_label);
}
bool reinit_P = false;
for (int i = 0; i < n_x; i++)
{
for (int j = 0; j <= i; j++)
{
if (!PX4_ISFINITE(_P(i, j)))
{
mavlink_and_console_log_info(&mavlink_log_pub,
"%sreinit P (%d, %d) not finite", msg_label, i, j);
reinit_P = true;
}
if (i == j)
{
if (_P(i, i) <= 0)
{
mavlink_and_console_log_info(&mavlink_log_pub,
"%sreinit P (%d, %d) negative", msg_label, i, j);
reinit_P = true;
}
}
else
{
_P(j, i) = _P(i, j);
}
if (reinit_P)
{
break;
}
}
if (reinit_P)
{
break;
}
}
if (reinit_P)
{
initP();
}
predict();
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)
{
publishLocalPos();
publishEstimatorStatus();
_pub_innov.get().timestamp = _timeStamp;
_pub_innov.update();
if ((_estimatorInitialized & EST_XY) && (_map_ref.init_done || _fake_origin.get()))
{
publishGlobalPos();
}
}
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 (!(_fusion.get() & FUSE_LAND))
{
return false;
}
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();
float vxy_stddev = sqrtf(_P(X_vx, X_vx) + _P(X_vy, X_vy));
float epv = sqrtf(_P(X_z, X_z));
float eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
float eph_thresh = 3.0f;
float epv_thresh = 3.0f;
if (vxy_stddev < _vxy_pub_thresh.get())
{
if (eph > eph_thresh)
{
eph = eph_thresh;
}
if (epv > epv_thresh)
{
epv = epv_thresh;
}
}
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);
_pub_lpos.get().y = xLP(X_y);
if (_fusion.get() & FUSE_PUB_AGL_Z)
{
_pub_lpos.get().z = -_aglLowPass.getState();
}
else
{
_pub_lpos.get().z = xLP(X_z);
}
_pub_lpos.get().vx = xLP(X_vx);
_pub_lpos.get().vy = xLP(X_vy);
_pub_lpos.get().vz = xLP(X_vz);
_pub_lpos.get().z_deriv = xLP(X_vz);
_pub_lpos.get().yaw = _eul(2);
_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);
_pub_lpos.get().dist_bottom_valid = _estimatorInitialized & EST_Z;
_pub_lpos.get().eph = eph;
_pub_lpos.get().epv = epv;
_pub_lpos.update();
_pub_lpos.get().evh = 0.0f;
_pub_lpos.get().evv = 0.0f;
_pub_lpos.get().vxy_max = 0.0f;
_pub_lpos.get().limit_hagl = false;
}
}
void BlockLocalPositionEstimator::publishEstimatorStatus()
{
_pub_est_status.get().timestamp = _timeStamp;
for (int i = 0; i < n_x; i++)
{
_pub_est_status.get().states[i] = _x(i);
_pub_est_status.get().covariances[i] = _P(i, i);
}
_pub_est_status.get().n_states = n_x;
_pub_est_status.get().nan_flags = 0;
_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()
{
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;
float vxy_stddev = sqrtf(_P(X_vx, X_vx) + _P(X_vy, X_vy));
float epv = sqrtf(_P(X_z, X_z));
float eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
float eph_thresh = 3.0f;
float epv_thresh = 3.0f;
if (vxy_stddev < _vxy_pub_thresh.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 = _eul(2);
_pub_gpos.get().eph = eph;
_pub_gpos.get().epv = epv;
_pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter;
_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()
{
_P.setZero();
_P(X_x, X_x) = 2 * EST_STDDEV_XY_VALID * EST_STDDEV_XY_VALID;
_P(X_y, X_y) = 2 * EST_STDDEV_XY_VALID * EST_STDDEV_XY_VALID;
_P(X_z, X_z) = 2 * EST_STDDEV_Z_VALID * EST_STDDEV_Z_VALID;
_P(X_vx, X_vx) = 2 * _vxy_pub_thresh.get() * _vxy_pub_thresh.get();
_P(X_vy, X_vy) = 2 * _vxy_pub_thresh.get() * _vxy_pub_thresh.get();
_P(X_vz, X_vz) = 2 * _vxy_pub_thresh.get() * _vxy_pub_thresh.get();
_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) = 2 * EST_STDDEV_TZ_VALID * EST_STDDEV_TZ_VALID;
}
void BlockLocalPositionEstimator::initSS()
{
initP();
_A.setZero();
_A(X_x, X_vx) = 1;
_A(X_y, X_vy) = 1;
_A(X_z, X_vz) = 1;
_B.setZero();
_B(X_vx, U_ax) = 1;
_B(X_vy, U_ay) = 1;
_B(X_vz, U_az) = 1;
updateSSStates();
updateSSParams();
}
void BlockLocalPositionEstimator::updateSSStates()
{
_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()
{
_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();
_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;
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;
float pn_t_noise_density =
_pn_t_noise_density.get() +
(_t_max_grade.get() / 100.0f) * sqrtf(_x(X_vx) * _x(X_vx) + _x(X_vy) * _x(X_vy));
_Q(X_tz, X_tz) = pn_t_noise_density * pn_t_noise_density;
}
void BlockLocalPositionEstimator::predict()
{
matrix::Quatf q(&_sub_att.get().q[0]);
_eul = matrix::Euler<float>(q);
_R_att = matrix::Dcm<float>(q);
Vector3f a(_sub_sensor.get().accelerometer_m_s2);
_u = _R_att * a;
_u(U_az) += 9.81f;
updateSSStates();
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);
if (!(_estimatorInitialized & EST_XY))
{
dx(X_x) = 0;
dx(X_vx) = 0;
dx(X_y) = 0;
dx(X_vy) = 0;
}
if (!(_estimatorInitialized & EST_Z))
{
dx(X_z) = 0;
}
if (!(_estimatorInitialized & EST_TZ))
{
dx(X_tz) = 0;
}
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);
}
_x += dx;
Matrix<float, n_x, n_x> dP = (_A * _P + _P * _A.transpose() +
_B * _R * _B.transpose() + _Q) * getDt();
for (int i = 0; i < n_x; i++)
{
if (_P(i, i) > P_MAX)
{
dP(i, i) = 0;
for (int j = 0; j < n_x; j++)
{
dP(i, j) = 0;
dP(j, i) = 0;
}
}
}
_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;
}
#pragma once
#include <px4_posix.h>
#include <px4_module_params.h>
#include <controllib/blocks.hpp>
#include <mathlib/mathlib.h>
#include <lib/ecl/geo/geo.h>
#include <matrix/Matrix.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/att_pos_mocap.h>
#include <uORB/topics/landing_target_pose.h>
#include <uORB/Publication.hpp>
#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>
using namespace matrix;
using namespace control;
static const float DELAY_MAX = 0.5f;
static const float HIST_STEP = 0.05f;
static const float BIAS_MAX = 1e-1f;
static const size_t HIST_LEN = 10;
static const size_t N_DIST_SUBS = 4;
static const float BETA_TABLE[7] = {0,
8.82050518214,
12.094592431,
13.9876612368,
16.0875642296,
17.8797700658,
19.6465647819,
};
class BlockLocalPositionEstimator : public control::SuperBlock, public ModuleParams
{
public:
enum {X_x = 0, X_y, X_z, X_vx, X_vy, X_vz, X_bx, X_by, X_bz, X_tz, n_x};
enum {U_ax = 0, U_ay, U_az, n_u};
enum {Y_baro_z = 0, n_y_baro};
enum {Y_lidar_z = 0, n_y_lidar};
enum {Y_flow_vx = 0, Y_flow_vy, n_y_flow};
enum {Y_sonar_z = 0, n_y_sonar};
enum {Y_gps_x = 0, Y_gps_y, Y_gps_z, Y_gps_vx, Y_gps_vy, Y_gps_vz, n_y_gps};
enum {Y_vision_x = 0, Y_vision_y, Y_vision_z, n_y_vision};
enum {Y_mocap_x = 0, Y_mocap_y, Y_mocap_z, n_y_mocap};
enum {Y_land_vx = 0, Y_land_vy, Y_land_agl, n_y_land};
enum {Y_target_x = 0, Y_target_y, n_y_target};
enum {POLL_FLOW = 0, POLL_SENSORS, POLL_PARAM, n_poll};
enum {
FUSE_GPS = 1 << 0,
FUSE_FLOW = 1 << 1,
FUSE_VIS_POS = 1 << 2,
FUSE_LAND_TARGET = 1 << 3,
FUSE_LAND = 1 << 4,
FUSE_PUB_AGL_Z = 1 << 5,
FUSE_FLOW_GYRO_COMP = 1 << 6,
FUSE_BARO = 1 << 7
};
enum sensor_t {
SENSOR_BARO = 1 << 0,
SENSOR_GPS = 1 << 1,
SENSOR_LIDAR = 1 << 2,
SENSOR_FLOW = 1 << 3,
SENSOR_SONAR = 1 << 4,
SENSOR_VISION = 1 << 5,
SENSOR_MOCAP = 1 << 6,
SENSOR_LAND = 1 << 7,
SENSOR_LAND_TARGET = 1 << 8,
};
enum estimate_t {
EST_XY = 1 << 0,
EST_Z = 1 << 1,
EST_TZ = 1 << 2,
};
BlockLocalPositionEstimator();
void update();
virtual ~BlockLocalPositionEstimator() = default;
private:
BlockLocalPositionEstimator(const BlockLocalPositionEstimator &) = delete;
BlockLocalPositionEstimator operator=(const BlockLocalPositionEstimator &) = delete;
Vector<float, n_x> dynamics
(
float t,
const Vector<float, n_x> &x,
const Vector<float, n_u> &u
);
void initP();
void initSS();
void updateSSStates();
void updateSSParams();
void predict();
int lidarMeasure(Vector<float, n_y_lidar> &y);
void lidarCorrect();
void lidarInit();
void lidarCheckTimeout();
int sonarMeasure(Vector<float, n_y_sonar> &y);
void sonarCorrect();
void sonarInit();
void sonarCheckTimeout();
int baroMeasure(Vector<float, n_y_baro> &y);
void baroCorrect();
void baroInit();
void baroCheckTimeout();
int gpsMeasure(Vector<double, n_y_gps> &y);
void gpsCorrect();
void gpsInit();
void gpsCheckTimeout();
int flowMeasure(Vector<float, n_y_flow> &y);
void flowCorrect();
void flowInit();
void flowCheckTimeout();
int visionMeasure(Vector<float, n_y_vision> &y);
void visionCorrect();
void visionInit();
void visionCheckTimeout();
int mocapMeasure(Vector<float, n_y_mocap> &y);
void mocapCorrect();
void mocapInit();
void mocapCheckTimeout();
int landMeasure(Vector<float, n_y_land> &y);
void landCorrect();
void landInit();
void landCheckTimeout();
int landingTargetMeasure(Vector<float, n_y_target> &y);
void landingTargetCorrect();
void landingTargetInit();
void landingTargetCheckTimeout();
void checkTimeouts();
inline float agl()
{
return _x(X_tz) - _x(X_z);
}
bool landed();
int getDelayPeriods(float delay, uint8_t *periods);
void publishLocalPos();
void publishGlobalPos();
void publishEstimatorStatus();
uORB::Subscription<actuator_armed_s> _sub_armed;
uORB::Subscription<vehicle_land_detected_s> _sub_land;
uORB::Subscription<vehicle_attitude_s> _sub_att;
uORB::Subscription<optical_flow_s> _sub_flow;
uORB::Subscription<sensor_combined_s> _sub_sensor;
uORB::Subscription<parameter_update_s> _sub_param_update;
uORB::Subscription<vehicle_gps_position_s> _sub_gps;
uORB::Subscription<vehicle_local_position_s> _sub_vision_pos;
uORB::Subscription<att_pos_mocap_s> _sub_mocap;
uORB::Subscription<distance_sensor_s> _sub_dist0;
uORB::Subscription<distance_sensor_s> _sub_dist1;
uORB::Subscription<distance_sensor_s> _sub_dist2;
uORB::Subscription<distance_sensor_s> _sub_dist3;
uORB::Subscription<distance_sensor_s> *_dist_subs[N_DIST_SUBS];
uORB::Subscription<distance_sensor_s> *_sub_lidar;
uORB::Subscription<distance_sensor_s> *_sub_sonar;
uORB::Subscription<landing_target_pose_s> _sub_landing_target_pose;
uORB::Publication<vehicle_local_position_s> _pub_lpos;
uORB::Publication<vehicle_global_position_s> _pub_gpos;
uORB::Publication<estimator_status_s> _pub_est_status;
uORB::Publication<ekf2_innovations_s> _pub_innov;
struct map_projection_reference_s _map_ref;
DEFINE_PARAMETERS(
(ParamInt<px4::params::SYS_AUTOSTART>) _sys_autostart,
(ParamInt<px4::params::LPE_FUSION>) _fusion,
(ParamFloat<px4::params::LPE_VXY_PUB>) _vxy_pub_thresh,
(ParamFloat<px4::params::LPE_Z_PUB>) _z_pub_thresh,
(ParamFloat<px4::params::LPE_SNR_Z>) _sonar_z_stddev,
(ParamFloat<px4::params::LPE_SNR_OFF_Z>) _sonar_z_offset,
(ParamFloat<px4::params::LPE_LDR_Z>) _lidar_z_stddev,
(ParamFloat<px4::params::LPE_LDR_OFF_Z>) _lidar_z_offset,
(ParamFloat<px4::params::LPE_ACC_XY>) _accel_xy_stddev,
(ParamFloat<px4::params::LPE_ACC_Z>) _accel_z_stddev,
(ParamFloat<px4::params::LPE_BAR_Z>) _baro_stddev,
(ParamFloat<px4::params::LPE_GPS_DELAY>) _gps_delay,
(ParamFloat<px4::params::LPE_GPS_XY>) _gps_xy_stddev,
(ParamFloat<px4::params::LPE_GPS_Z>) _gps_z_stddev,
(ParamFloat<px4::params::LPE_GPS_VXY>) _gps_vxy_stddev,
(ParamFloat<px4::params::LPE_GPS_VZ>) _gps_vz_stddev,
(ParamFloat<px4::params::LPE_EPH_MAX>) _gps_eph_max,
(ParamFloat<px4::params::LPE_EPV_MAX>) _gps_epv_max,
(ParamFloat<px4::params::LPE_VIS_XY>) _vision_xy_stddev,
(ParamFloat<px4::params::LPE_VIS_Z>) _vision_z_stddev,
(ParamFloat<px4::params::LPE_VIS_DELAY>) _vision_delay,
(ParamFloat<px4::params::LPE_VIC_P>) _mocap_p_stddev,
(ParamFloat<px4::params::LPE_FLW_OFF_Z>) _flow_z_offset,
(ParamFloat<px4::params::LPE_FLW_SCALE>) _flow_scale,
(ParamInt<px4::params::LPE_FLW_QMIN>) _flow_min_q,
(ParamFloat<px4::params::LPE_FLW_R>) _flow_r,
(ParamFloat<px4::params::LPE_FLW_RR>) _flow_rr,
(ParamFloat<px4::params::LPE_LAND_Z>) _land_z_stddev,
(ParamFloat<px4::params::LPE_LAND_VXY>) _land_vxy_stddev,
(ParamFloat<px4::params::LPE_PN_P>) _pn_p_noise_density,
(ParamFloat<px4::params::LPE_PN_V>) _pn_v_noise_density,
(ParamFloat<px4::params::LPE_PN_B>) _pn_b_noise_density,
(ParamFloat<px4::params::LPE_PN_T>) _pn_t_noise_density,
(ParamFloat<px4::params::LPE_T_MAX_GRADE>) _t_max_grade,
(ParamFloat<px4::params::LPE_LT_COV>) _target_min_cov,
(ParamInt<px4::params::LTEST_MODE>) _target_mode,
(ParamInt<px4::params::LPE_FAKE_ORIGIN>) _fake_origin,
(ParamFloat<px4::params::LPE_LAT>) _init_origin_lat,
(ParamFloat<px4::params::LPE_LON>) _init_origin_lon
)
enum TargetMode {
Target_Moving = 0,
Target_Stationary = 1
};
BlockHighPass _flow_gyro_x_high_pass;
BlockHighPass _flow_gyro_y_high_pass;
BlockStats<float, n_y_baro> _baroStats;
BlockStats<float, n_y_sonar> _sonarStats;
BlockStats<float, n_y_lidar> _lidarStats;
BlockStats<float, 1> _flowQStats;
BlockStats<float, n_y_vision> _visionStats;
BlockStats<float, n_y_mocap> _mocapStats;
BlockStats<double, n_y_gps> _gpsStats;
uint16_t _landCount;
BlockLowPassVector<float, n_x> _xLowPass;
BlockLowPass _aglLowPass;
BlockDelay<float, n_x, 1, HIST_LEN> _xDelay;
BlockDelay<uint64_t, 1, 1, HIST_LEN> _tDelay;
px4_pollfd_struct_t _polls[3];
uint64_t _timeStamp;
uint64_t _time_origin;
uint64_t _timeStampLastBaro;
uint64_t _time_last_hist;
uint64_t _time_last_flow;
uint64_t _time_last_baro;
uint64_t _time_last_gps;
uint64_t _time_last_lidar;
uint64_t _time_last_sonar;
uint64_t _time_init_sonar;
uint64_t _time_last_vision_p;
uint64_t _time_last_mocap;
uint64_t _time_last_land;
uint64_t _time_last_target;
float _altOrigin;
bool _altOriginInitialized;
bool _altOriginGlobal;
float _baroAltOrigin;
float _gpsAltOrigin;
bool _receivedGps;
bool _lastArmedState;
uint16_t _sensorTimeout;
uint16_t _sensorFault;
uint8_t _estimatorInitialized;
bool _flowUpdated;
bool _gpsUpdated;
bool _visionUpdated;
bool _mocapUpdated;
bool _lidarUpdated;
bool _sonarUpdated;
bool _landUpdated;
bool _baroUpdated;
Vector<float, n_x> _x;
Vector<float, n_u> _u;
Matrix<float, n_x, n_x> _P;
matrix::Dcm<float> _R_att;
Vector3f _eul;
Matrix<float, n_x, n_x> _A;
Matrix<float, n_x, n_u> _B;
Matrix<float, n_u, n_u> _R;
Matrix<float, n_x, n_x> _Q;
};
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)