前言：之前学习的是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;
// required standard deviation of estimate for estimator to publish data
// 发布的数据估计量所要求的的标准偏差
static const uint32_t EST_STDDEV_XY_VALID = 2.0; // x、y方向的标准位置偏差2.0 m
static const uint32_t EST_STDDEV_Z_VALID = 2.0; // z方向的标准位置偏差2.0 m
static const uint32_t EST_STDDEV_TZ_VALID = 2.0; // 地面的海拔高度的标准偏差2.0 m
static const float P_MAX = 1.0e6f; // 规定状态协方差的最大值
static const float LAND_RATE = 10.0f; // rate of land detector correction
static const char *msg_label = "[lpe] ";
//下面是构造函数，从24行到167行都是初始化列表
BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
// this block has no parent, and has name LPE
SuperBlock(nullptr, "LPE"), //用于制定父类的名字和该block的名字，因为该block没有父类所以为空指针
ModuleParams(nullptr), //c++里面的标准类，用去其他类使用配置参数
// 下面是用于订阅相关的信息，设置订阅速率，并添加到列表
_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()),
// set flow max update rate higher than expected to we don't lose packets
// 订阅光流传感器的信息
_sub_flow(ORB_ID(optical_flow), 1000 / 100, 0, &getSubscriptions()),
// 订阅sensor_combined信息
_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()),
// gps 10 hz
// 订阅gps位置信息
_sub_gps(ORB_ID(vehicle_gps_position), 1000 / 10, 0, &getSubscriptions()),
// vision 50 hz
// 订阅视觉位置信息
_sub_vision_pos(ORB_ID(vehicle_vision_position), 1000 / 50, 0, &getSubscriptions()),
// mocap 50 hz
//订阅mocap信息
_sub_mocap(ORB_ID(att_pos_mocap), 1000 / 50, 0, &getSubscriptions()),
// all distance sensors, 10 hz
// 订阅所有的距离传感器信息，比如雷达、声呐等等
_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()),
// publications
// 配置发布信息发布
_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 projection
// 地图规划，里面有很多东西，这里主要是初始经纬度
_map_ref(),
// flow gyro
// 设置 flow gyro filter
_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"),
// 配置针对agl的低通滤波
_aglLowPass(this, "X_LP"),
// delay
// 配置延迟
_xDelay(this, ""),
_tDelay(this, ""),
// misc
// 把这些时间该归零的归零，该设置成绝对时间的设置成绝对时间
_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),
// reference altitudes
// 配置参考点的海拔及初始化情况
_altOrigin(0),
_altOriginInitialized(false),
_altOriginGlobal(false),
_baroAltOrigin(0),
_gpsAltOrigin(0),
// status
// 配置gps、armed状态
_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)
/*******************下面是构造函数的具体内容*********************/
{
//把订阅的距离信息放进数组里面
_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;
//初始化 A, B, P, x, u
_x.setZero();
_u.setZero();
initSS(); //这里面包含了A、B、P的初始化
// map
_map_ref.init_done = false;
// print fusion settings to console
// 将融合的设置信息打印在控制台
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);
}
//下面这个dynamics函数就是动态方程，本质上是一个一阶微分方程
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;
}
//下面的update函数是重点
//update函数的结构流程是：
//1.先调用了预测函数predict结合加速度计和飞机此时的姿态位置进行预测
//2.调用了各个模块的修正函数，比如mocapCorrect
//当然在调用前有一些判断机制，看mocap消息是否有更新，是否启用mocap，是否超时等
void BlockLocalPositionEstimator::update()
{
// 这里的poll是用来轮询之前提过的flow、param_update、sensor_combined
// 轮询周期是100ms
int ret = px4_poll(_polls, 3, 100);
if (ret < 0) //poll<0说明poll机制的返回值函数调用失败，同时会自动设置全局变量erro
{
return;
}
uint64_t newTimeStamp = hrt_absolute_time();//获取绝对时间
float dt = (newTimeStamp - _timeStamp) / 1.0e6f;//计算出程序更新周期（us）
_timeStamp = newTimeStamp;
// 将计算的dt应用于所有子模块（child block）
setDt(dt);
// 检测飞机状态是否解锁，arm代表解锁，disarm代表上锁
bool armedState = _sub_armed.get().armed;
if (!armedState && (_sub_lidar == nullptr || _sub_sonar == nullptr))
//如果没有解锁就读取测量的距离值，这里主要是雷达数据和超声波数据
{
//检查距离传感器
//N_DIST_SUBS代表距离传感器的个数，所以for循环就是挨个挨个检查
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，即换下一个传感器
{
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)//if括号里的内容用于判断数据是否来自激光
{
_sub_lidar = s;
//向控制台打印mavlink信息
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)//else if括号里的内容用于判断数据是否来自超声波
{
_sub_sonar = s;
//向控制台打印mavlink信息
mavlink_and_console_log_info(&mavlink_log_pub, "%sDownward-facing Sonar detected with ID %i", 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.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;
}
}
}
//然后开始订阅各个数据，包括光流数据、gps数据、视觉、传感器数据等等，并获取他们的更新状态，是true还是false
//疑惑的地方：下面用到二进制码相与的运算符&，是用于什么？和获取更新状态有什么关系？根据前面的内容我猜测大概是指配置是否正确
//&左边是是得到的配置信息，&右边是规定的配置信息
_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);// throttle rate
bool targetPositionUpdated = _sub_landing_target_pose.updated();
// 这个才是订阅数据，上面的部分主要是获取更新状态以及订阅前的uorb_check
updateSubscriptions();
// 更新参数
if (paramsUpdated)
{
SuperBlock::updateParams();//更新block中的参数
ModuleParams::updateParams();//更新module参数，主要是更新一些模块或子模块的首地址
updateSSParams();//更新R矩阵和Q矩阵
}
// 接着判断X坐标和Y坐标数据的有效性
bool vxy_stddev_ok = false;
//判断p中关于x、y向的速度方差是否小于是否小于设定值，即反映了vx和vy是否有效
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;
}
//判断测量xy向的各个传感器是否都初始化
if (_estimatorInitialized & EST_XY)//如果都初始化了
{
// 但是如果vx、vy无效并且gps超时，还是将Initialized设为未完成
if (!vxy_stddev_ok && (_sensorTimeout & SENSOR_GPS))
{
_estimatorInitialized &= ~EST_XY;
}
}
else //else说明xy向的各个传感器并没有都初始化
{
if (vxy_stddev_ok)
{
//但是只要有一个模块初始化完成，并且vx和vy有效，都说明初始化完成
if (!(_sensorTimeout & SENSOR_GPS)
|| !(_sensorTimeout & SENSOR_FLOW)
|| !(_sensorTimeout & SENSOR_VISION)
|| !(_sensorTimeout & SENSOR_MOCAP)
|| !(_sensorTimeout & SENSOR_LAND)
|| !(_sensorTimeout & SENSOR_LAND_TARGET))
{
_estimatorInitialized |= EST_XY;
}
}
}
// 接着判断z坐标数据的有效性，与上面同理
bool z_stddev_ok = sqrtf(_P(X_z, X_z)) < _z_pub_thresh.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;
}
}
// 判断地形数据有效性，地面的海拔高度反应地形，与上面同理
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();
// 开始更新GPS数据
// 如果validxy==true并且未设置初始经纬度，则将类的初始化列表中的经纬度（即前面提到的_map_ref）
// 设为设为初始化经纬度
if (!_map_ref.init_done && (_estimatorInitialized & EST_XY) && _fake_origin.get())
{
//将类的初始化列表中的经纬度（即前面提到的_map_ref）设为初始化经纬度
map_projection_init(&_map_ref,
_init_origin_lat.get(),
_init_origin_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(_init_origin_lat.get()), double(_init_origin_lon.get()), double(_altOrigin));
}
// 重新初始化x数据
bool reinit_x = false; //重新初始化x数据的标识符为false
for (int i = 0; i < n_x; i++) //x数据总共有十个
{
// should we do a reinit
// of sensors here?
// don't want it to take too long
if (!PX4_ISFINITE(_x(i))) //只要有一个x数据发散，就需要将重新初始化x数据的标识符设为true
//然后跳出for循环执行重新初始化操作，其实也就是全设为零
//而如果x数据不发散，由于reinit_x = false，就不会执行下面的初始化操作
{
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;//将10个x数据初始化为0
}
mavlink_and_console_log_info(&mavlink_log_pub, "%sreinit x", msg_label);
}
//与上面同理，检查p里面数据的有效性
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)
{
// make sure diagonal elements are positive
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);//利用p的对称性
}
if (reinit_P)
{
break;
}
}
if (reinit_P)
{
break;
}
}
if (reinit_P)
{
initP();
}
// 预测，即状态预估计
predict();
// 下面就是结合传感器以及其他模块对预测值进行补偿校正
if (_gpsUpdated) //如果用的是gps
{
if (_sensorTimeout & SENSOR_GPS)//如果超时
{
gpsInit();//gps初始化
}
else
{
gpsCorrect();//没有超时就校正
}
}
if (_baroUpdated) //如果用的是baro
{
if (_sensorTimeout & SENSOR_BARO) //如果超时
{
baroInit();//baro初始化
}
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) //如果用的是mocap
{
if (_sensorTimeout & SENSOR_MOCAP) //如果超时
{
mocapInit();//视觉初始化
}
else
{
mocapCorrect();//没有超时就校正
}
}
if (_landUpdated) //如果用的是land
{
if (_sensorTimeout & SENSOR_LAND) //如果超时
{
landInit();//land初始化
}
else
{
landCorrect();//没有超时就校正
}
}
if (targetPositionUpdated) //如果用的是landing_target
{
if (_sensorTimeout & SENSOR_LAND_TARGET) //如果超时
{
landingTargetInit();//初始化
}
else
{
landingTargetCorrect();//没有超时就校正
}
}
/****************发布信息*****************/
if (_altOriginInitialized)
{
// update all publications if possible
publishLocalPos();//发布local position
publishEstimatorStatus();//发布十个状态量
_pub_innov.get().timestamp = _timeStamp;//发布时间
_pub_innov.update(); //更新公告、订阅、发布的句柄
if ((_estimatorInitialized & EST_XY) && (_map_ref.init_done || _fake_origin.get()))
{
publishGlobalPos();//发布global position
}
}
//更新延迟信息
//注意：下面的dt_hist是接收传感器信息的时间间隔，而前面在update函数
//最开始定义的dt是执行update函数的时间周期，注意区分两者
float dt_hist = 1.0e-6f * (_timeStamp - _time_last_hist);//本次的时间减去上一次记录的时间就是间隔（us）
if (_time_last_hist == 0 ||(dt_hist > HIST_STEP)) //前面定义过最大的间隔为HIST_STEP = 0.05s
{
_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();
}
/****判断是否着陆****/
//判断的方法：根据vehicle_land_detected这个topic
//这个topic中有bool landed、bool freefall、bool ground_contact、bool maybe_landed这四个布尔量
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();
// lie about eph/epv to allow visual odometry only navigation when velocity est. good
float vxy_stddev = sqrtf(_P(X_vx, X_vx) + _P(X_vy, X_vy));//求vx、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;//eph的threshold（阈值）
float epv_thresh = 3.0f;//epv的threshold（阈值）
//根据两个阈值限定eph和epv
if (vxy_stddev < _vxy_pub_thresh.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; //发布xy的距离数据是否有效
_pub_lpos.get().z_valid = _estimatorInitialized & EST_Z; //发布z的距离数据是否有效
_pub_lpos.get().v_xy_valid = _estimatorInitialized & EST_XY;//发布xy的速度数据是否有效
_pub_lpos.get().v_z_valid = _estimatorInitialized & EST_Z; //发布z的速度数据是否有效
_pub_lpos.get().x = xLP(X_x); //发布x（北）方向的距离信息
_pub_lpos.get().y = xLP(X_y); //发布y（东）方向的距离信息
if (_fusion.get() & FUSE_PUB_AGL_Z) //判断是要发布相对高度还是绝对高度
{
_pub_lpos.get().z = -_aglLowPass.getState()；//发布相对地面的高度（agl）
//因为坐标系为东北地，所以要加负号
}
else
{
_pub_lpos.get().z = xLP(X_z); // down 发布绝对高度
}
_pub_lpos.get().vx = xLP(X_vx); // 发布x（北）方向的速度
_pub_lpos.get().vy = xLP(X_vy); // 发布y（东）方向的速度
_pub_lpos.get().vz = xLP(X_vz); // 发布z（下）方向的速度
// 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); //发布z方向的位置变化率，用X_vz代替
_pub_lpos.get().yaw = _eul(2);//发布偏航角速度
_pub_lpos.get().xy_global = _estimatorInitialized & EST_XY;//发布xy方向的global position信息的有效性
_pub_lpos.get().z_global = !(_sensorTimeout & SENSOR_BARO) && _altOriginGlobal;//发布z方向的global position信息的有效性
_pub_lpos.get().ref_timestamp = _time_origin;//发布更新gps时的时间点，_time_origin在前面gps数据更新时出现过
_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.update();
//TODO provide calculated values for these
_pub_lpos.get().evh = 0.0f; //发布水平速度误差的标准差=0
_pub_lpos.get().evv = 0.0f; //发布垂直速度误差的标准差=0
_pub_lpos.get().vxy_max = 0.0f; //注意：vxy指水平这个合成速度，这里=0不是指水平速度=0，而是指没有用到这个量
//从vehicle_local_position.msg里得知的
_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);//发布误差协方差矩阵p
}
_pub_est_status.get().n_states = n_x; //发布状态量的个数，这里为10
_pub_est_status.get().nan_flags = 0; //用于检验状态数据是否为无穷量
_pub_est_status.get().health_flags = _sensorFault; //发布传感器的健康状况
_pub_est_status.get().timeout_flags = _sensorTimeout; //发布表明超时的flag
_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();
}
/**发布global position**/
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; //绝对高度=地面海拔+相对高度，因为xLP(X_z)是负值，所以加负号转为正
// lie about eph/epv to allow visual odometry only navigation when velocity est. good
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; //eph的阈值
float epv_thresh = 3.0f; //epv的阈值
//根据两个阈值限定eph和epv，跟前面的发布local position一样
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; //发布eph
_pub_gpos.get().epv = epv; //发布epv
_pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter; //发布气压高度
_pub_gpos.get().terrain_alt = _altOrigin - xLP(X_tz); //发布地面的海拔高度，注意：X_tz是地面到原点的距离
_pub_gpos.get().terrain_alt_valid = _estimatorInitialized & EST_TZ;//发布地面的海拔高度信息的有效性
_pub_gpos.get().dead_reckoning = !(_estimatorInitialized & EST_XY);//发布是不是用了dead_reckoninggps（惯性导航技术）
_pub_gpos.update();
}
}
/**初始化状态协方差矩阵P**/
void BlockLocalPositionEstimator::initP()
{
_P.setZero();//将P清零
// initialize to twice valid condition
// 疑惑：为什么要初始化成两倍的方差？根据协方差矩阵的定义不应该就是方差吗？
_P(X_x, X_x) = 2 * EST_STDDEV_XY_VALID * EST_STDDEV_XY_VALID;//=8
_P(X_y, X_y) = 2 * EST_STDDEV_XY_VALID * EST_STDDEV_XY_VALID;//=8
_P(X_z, X_z) = 2 * EST_STDDEV_Z_VALID * EST_STDDEV_Z_VALID;//=8
_P(X_vx, X_vx) = 2 * _vxy_pub_thresh.get() * _vxy_pub_thresh.get();//=8
_P(X_vy, X_vy) = 2 * _vxy_pub_thresh.get() * _vxy_pub_thresh.get();//=8
// use vxy thresh for vz init as well
_P(X_vz, X_vz) = 2 * _vxy_pub_thresh.get() * _vxy_pub_thresh.get();//=8
// initialize bias uncertainty to small values to keep them stable
_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;//=8
}
/**初始化A、B、Q、R**/
void BlockLocalPositionEstimator::initSS()
{
initP();
// dynamics matrix
_A.setZero();
// derivative of position is velocity
// 这里只设置了A的其中三个，其余的在下面的updateSSStates()函数中。
_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();
}
/**设置A**/
void BlockLocalPositionEstimator::updateSSStates()
{
// derivative of velocity is accelerometer acceleration
// (in input matrix) - bias (in body frame)
// 速度的微分就是加速度计的加速度信息减去偏差
_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);
}
/**设置R、Q**/
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 ((m/s)/sqrt(hz)), scales with velocity
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]);//括号里的q来自vehicle_attitude这个topic，也就是经过传感器数据融合后修正的q，这里为什么是q（0），其实这里的q(0)指的是首地址
_eul = matrix::Euler<float>(q); //由四元数得到飞机的姿态（欧拉角）
_R_att = matrix::Dcm<float>(q); //由四元数得到旋转矩阵
Vector3f a(_sub_sensor.get().accelerometer_m_s2); //a向量里面是加速度信息
// note, bias is removed in dynamics function
// 准备输入向量_u
_u = _R_att * a; //转换到大地系
_u(U_az) += 9.81f; // add g 地理坐标系下的z轴加速度是有重力加速度的，因此补偿上去。
// update state space based on new states
// 更新系统状态空间转移矩阵，即A矩阵
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(); //起初看到这个h我先猜是教材中ekf公式中的dt，还是pid中的dt？后面发现都不是，而是指rk中的步长，这里指时间间隔
Vector<float, n_x> k1, k2, k3, k4; //从这里可知用到了四阶的龙格库塔，k1到k4表示取的中间四个点的斜率。
k1 = dynamics(0, _x, _u);//dynamics就是现代控制理论中的动态方程，是一个一阶的微分方程
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);//dx是下一时刻的状态的变化值，也就是博客里的deltaY
// dx的限制性处理
if (!(_estimatorInitialized & EST_XY)) //相与后非表示没有初始化或者xy没有有效数据
{
dx(X_x) = 0; //x方向的位置变化为零
dx(X_vx) = 0; //x方向的速度变化为零
dx(X_y) = 0; //y方向的位置变化为零
dx(X_vy) = 0; //y方向的速度变化为零
}
// don't integrate z if no valid z data
if (!(_estimatorInitialized & EST_Z))
{
dx(X_z) = 0; //z方向的位置变化为零
}
// 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);
//下面对偏差bx、by、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;// _x就是下一时刻的预测值，接下来的任务就是对他进行校正
//下面是p的一阶微分方程，在解这个方程的时候，用的是最简单的欧拉法
Matrix<float, n_x, n_x> dP = (_A * _P + _P * _A.transpose() +
_B * _R * _B.transpose() + _Q) * getDt();
// dp的限制性处理
// 处理的内容是：如果本身的p已经大于了P_MAX，则不会再累加dp
for (int i = 0; i < n_x; i++)
{
if (_P(i, i) > P_MAX)
{
// if diagonal element greater than max, stop propagating
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); //将_x低通处理后存在_xLowPass里的state中
_aglLowPass.update(agl()); //将agl低通处理后存在_aglLowPass的state中
}
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> //c++里的base class,用于其他的参数使用配置参数
#include <controllib/blocks.hpp> //之前同学讲过的
#include <mathlib/mathlib.h> //数学库
#include <lib/ecl/geo/geo.h>
#include <matrix/Matrix.hpp> //矩阵
// 下面就是本程序的订阅和发布的信息流。
// uORB Subscriptions 一些订阅的topics
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_status.h> //飞机的状态
#include <uORB/topics/actuator_armed.h> //执行机构的armed情况
#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> //飞行器的gps位置
#include <uORB/topics/att_pos_mocap.h> //通过mocap获取的姿态位置信息
#include <uORB/topics/landing_target_pose.h> //着陆点的相对位置
// uORB Publications 发布的信息
#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; // 延迟的最大值=0.5s
static const float HIST_STEP = 0.05f; // 20 hz，历史消息间隔？
static const float BIAS_MAX = 1e-1f; //偏差最大值
static const size_t HIST_LEN = 10; // DELAY_MAX / HIST_STEP;历史消息的长度
static const size_t N_DIST_SUBS = 4; //待查？
// 统计学中的卡方检验：卡方检验就是统计样本的实际观测值与理论推断值之间的偏离程度，
// 实际观测值与理论推断值之间的偏离程度就决定卡方值的大小，卡方值越大，越不符合；卡方值越小，偏差越小，越趋于符合，
// 若两个值完全相等时，卡方值就为0，表明理论值完全符合。
// for fault detection
// chi squared distribution, false alarm probability 0.0001
// see fault_table.py
// note skip 0 index so we can use degree of freedom as index
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
{
// dynamics:
//
// x(+) = A * x(-) + B * u(+) ，系统状态方程
// y_i = C_i*x ， 观测方程
//
// kalman filter
//
// E[xx'] = P 估计量的误差的协方差
// E[uu'] = W 系统噪声
// E[y_iy_i'] = R_i 系统噪声协方差矩阵
//
// prediction
// x(+|-) = A*x(-|-) + B*u(+)
// P(+|-) = A*P(-|-)*A' + B*W*B' 对应书上190页公式2
//
// correction
// x(+|+) = x(+|-) + K_i * (y_i - H_i * x(+|-) )
//
//
// input:
// ax, ay, az (acceleration NED)
//
// states:
// px, py, pz , ( position NED, m)
// vx, vy, vz ( vel NED, m/s),
// bx, by, bz ( accel bias, m/s^2)
// tz (terrain altitude, ASL, m)
//
// measurements:
//
// sonar: pz (measured d*cos(phi)*cos(theta))
//
// baro: pz
//
// flow: vx, vy (flow is in body x, y frame)
//
// gps: px, py, pz, vx, vy, vz (flow is in body x, y frame)
//
// lidar: pz (actual measured d*cos(phi)*cos(theta))
//
// vision: px, py, pz, vx, vy, vz
//
// mocap: px, py, pz
//
// land (detects when landed)): pz (always measures agl = 0)
//
public:
// constants
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,
};
// public methods
BlockLocalPositionEstimator();//BlockLocalPositionEstimator类的构造函数
void update(); //作用：对飞机的状态先验估计值进行补偿校正
virtual ~BlockLocalPositionEstimator() = default; //析构函数
private:
BlockLocalPositionEstimator(const BlockLocalPositionEstimator &) = delete;
BlockLocalPositionEstimator operator=(const BlockLocalPositionEstimator &) = delete;
// methods
// ----------------------------
//
Vector<float, n_x> dynamics //动态方程，形式为：x‘ = _A * x + _B * u，这是一个一阶微分方程，也就是
//描述系统状态空间的状态方程，属于现代控制理论中的。
//区分kf中的 x_(k) = A*x(k-1) + B*u_(k-1)。
//本程序中的状态估计用的是这个一阶微分方程结合龙哥库塔，而不是用的kf中的第一个方程。因为
//这是一个连续系统
(
float t,
const Vector<float, n_x> &x,
const Vector<float, n_u> &u
);
void initP(); //初始化状态协方差矩阵P
void initSS(); //这个函数包括了下面的两个函数，执行这个函数的同时也就执行了下面的两个
void updateSSStates(); //设置A
void updateSSParams(); //设置R、Q
// 预测下一时刻的空间状态
void predict();
// lidar 雷达的相关函数
int lidarMeasure(Vector<float, n_y_lidar> &y); //数据测量
void lidarCorrect(); //将之前用predict()预测的状态结合雷达数据进行校正
void lidarInit(); //雷达的初始化
void lidarCheckTimeout();//检查超时吗
// sonar
int sonarMeasure(Vector<float, n_y_sonar> &y);//数据测量
void sonarCorrect(); //将之前用predict()预测的状态结合声呐数据进行校正
void sonarInit(); //声呐的初始化
void sonarCheckTimeout();//检查超时吗
// baro
int baroMeasure(Vector<float, n_y_baro> &y);//数据测量
void baroCorrect(); //将之前用predict()预测的状态结合气压计数据进行校正
void baroInit(); //气压计的初始化
void baroCheckTimeout(); //检查超时吗
// gps
int gpsMeasure(Vector<double, n_y_gps> &y);//数据测量
void gpsCorrect(); //将之前用predict()预测的状态结合gps数据进行校正
void gpsInit(); //gps的初始化
void gpsCheckTimeout(); //检查超时吗
// flow
int flowMeasure(Vector<float, n_y_flow> &y);//数据测量
void flowCorrect(); //将之前用predict()预测的状态结合光流数据进行校正
void flowInit(); //光流的初始化
void flowCheckTimeout(); //检查超时吗
// vision
int visionMeasure(Vector<float, n_y_vision> &y);//数据测量
void visionCorrect(); //将之前用predict()预测的状态结合视觉数据进行校正
void visionInit(); //视觉的初始化
void visionCheckTimeout(); //检查超时吗
// mocap
int mocapMeasure(Vector<float, n_y_mocap> &y);//数据测量
void mocapCorrect(); //将之前用predict()预测的状态结合mocap数据进行校正
void mocapInit(); //mocap的初始化
void mocapCheckTimeout(); //检查超时吗
// land
int landMeasure(Vector<float, n_y_land> &y);//数据测量
void landCorrect(); //将之前用predict()预测的状态结合land数据进行校正
void landInit(); //land的初始化
void landCheckTimeout(); //检查超时吗
// landing target
int landingTargetMeasure(Vector<float, n_y_target> &y);//数据测量
void landingTargetCorrect(); //将之前用predict()预测的状态结合landing target数据进行校正
void landingTargetInit(); //landing target的初始化
void landingTargetCheckTimeout();//检查超时吗
// timeouts
void checkTimeouts(); //检查超时
// misc
inline float agl() //用于检测是否着陆，着陆时agl=0.
{
return _x(X_tz) - _x(X_z);// _x(X_tz是地面到原点的z， _x(X_z)是飞行器到原点的z
}
bool landed();
int getDelayPeriods(float delay, uint8_t *periods); //获取延迟时间长度
// publications 用于发布信息的函数
void publishLocalPos();
void publishGlobalPos();
void publishEstimatorStatus();
// attributes
// ----------------------------
// subscriptions
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;
// publications
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;
// map projection
struct map_projection_reference_s _map_ref; //地图构建的参考。这个结构体在基于gps的位置控制中有用到，
//用于map_projection_project函数和map_projection_global_reproject
//函数，前者是将地理学坐标系(geographic coordinate system)中的点(球)投影到本地方位等距平面(XOY)中
//后者是将本地方位等距平面中的点投影到地理学坐标系，也就是球面坐标和平面坐标之间的转换
DEFINE_PARAMETERS(
(ParamInt<px4::params::SYS_AUTOSTART>) _sys_autostart, /**< example parameter */
// general parameters
(ParamInt<px4::params::LPE_FUSION>) _fusion,
(ParamFloat<px4::params::LPE_VXY_PUB>) _vxy_pub_thresh,
(ParamFloat<px4::params::LPE_Z_PUB>) _z_pub_thresh,
// sonar parameters
(ParamFloat<px4::params::LPE_SNR_Z>) _sonar_z_stddev,
(ParamFloat<px4::params::LPE_SNR_OFF_Z>) _sonar_z_offset,
// lidar parameters
(ParamFloat<px4::params::LPE_LDR_Z>) _lidar_z_stddev,
(ParamFloat<px4::params::LPE_LDR_OFF_Z>) _lidar_z_offset,
// accel parameters
(ParamFloat<px4::params::LPE_ACC_XY>) _accel_xy_stddev,
(ParamFloat<px4::params::LPE_ACC_Z>) _accel_z_stddev,
// baro parameters
(ParamFloat<px4::params::LPE_BAR_Z>) _baro_stddev,
// gps parameters
(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,
// vision parameters
(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,
// mocap parameters
(ParamFloat<px4::params::LPE_VIC_P>) _mocap_p_stddev,
// flow parameters
(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,
// land parameters
(ParamFloat<px4::params::LPE_LAND_Z>) _land_z_stddev,
(ParamFloat<px4::params::LPE_LAND_VXY>) _land_vxy_stddev,
// process noise
(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,
// init origin
(ParamInt<px4::params::LPE_FAKE_ORIGIN>) _fake_origin,
(ParamFloat<px4::params::LPE_LAT>) _init_origin_lat,
(ParamFloat<px4::params::LPE_LON>) _init_origin_lon
)
// target mode paramters from landing_target_estimator module
// 与地面目标有关的参数，比如着陆点是固定还是移动的
enum TargetMode {
Target_Moving = 0, //目标移动状态
Target_Stationary = 1 //目标为静止状态
};
// flow gyro filter 滤波器，这个在之前讲过的block类里面
BlockHighPass _flow_gyro_x_high_pass;
BlockHighPass _flow_gyro_y_high_pass;
// stats 传感器或者其他模块的统计量
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;
// low pass
BlockLowPassVector<float, n_x> _xLowPass;
BlockLowPass _aglLowPass;
// delay blocks
BlockDelay<float, n_x, 1, HIST_LEN> _xDelay;
BlockDelay<uint64_t, 1, 1, HIST_LEN> _tDelay;
// misc 一些时间变量
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;
// reference altitudes
float _altOrigin;//原点的海拔
bool _altOriginInitialized;//原点海拔初始化
bool _altOriginGlobal; // true when the altitude of the origin is defined wrt a global reference frame
float _baroAltOrigin;//原点的气压高度
float _gpsAltOrigin;//原点的gps高度
// status
bool _receivedGps;
bool _lastArmedState;
// masks
uint16_t _sensorTimeout; //传感器超时时间
uint16_t _sensorFault; //传感器故障
uint8_t _estimatorInitialized;
// sensor update flags
bool _flowUpdated;//光流传感器更新
bool _gpsUpdated; //gps更新
bool _visionUpdated;//视觉更新
bool _mocapUpdated;//motion capture更新
bool _lidarUpdated;//雷达更新
bool _sonarUpdated;//声呐更新
bool _landUpdated; //land detector更新
bool _baroUpdated;//气压计更新
// state space
Vector<float, n_x> _x; // 状态向量
Vector<float, n_u> _u; // 系统输入量
Matrix<float, n_x, n_x> _P; // 状态协方差矩阵
matrix::Dcm<float> _R_att;//旋转矩阵，为了与下面的输入协方差矩阵_R区别,于是加了_att
Vector3f _eul;
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 过程噪声的协方差矩阵
};