一、Lacal_position_estimator与position_estimator_inav是并列关系，用于位置估计
具体选择过程应该是这样的，还没测验，仅供参考
1.cmake将Lacal_position_estimator编译进.px4
2.rcS中启动Lacal_position_estimator，令SYS_MC_EST_GROUP=1，或者将判断注释掉
二、处理的数据流，以光流为例
因为Lacal_position_estimator是用卡尔曼算法，笔者这方面还比较弱，只能先把数据流程整理下，具体的物理含义还不知
int local_position_estimator_thread_main(int argc, char *argv[])
{
warnx("starting");
using namespace control;
BlockLocalPositionEstimator est;
thread_running = true;
while (!thread_should_exit) {
est.update();
}
warnx("exiting.");
thread_running = false;
return 0;
}
跳转至est.update();
即void BlockLocalPositionEstimator::update()
以下皆工作于此循环中
while (!thread_should_exit) {
est.update();
}
1.数据来源：
// see which updates are available //订阅数据 bool flowUpdated = _sub_flow.updated(); bool paramsUpdated = _sub_param_update.updated(); bool baroUpdated = _sub_sensor.updated(); bool gpsUpdated = _gps_on.get() && _sub_gps.updated(); bool homeUpdated = _sub_home.updated(); bool visionUpdated = _vision_on.get() && _sub_vision_pos.updated(); bool mocapUpdated = _sub_mocap.updated(); bool lidarUpdated = (_sub_lidar != NULL) && _sub_lidar->updated(); bool sonarUpdated = (_sub_sonar != NULL) && _sub_sonar->updated();
// get new data //获取新的数据 updateSubscriptions();
其原函数为
virtual void updateSubscriptions()
{
Block::updateSubscriptions();
if (getChildren().getHead() != NULL) { updateChildSubscriptions(); }
}
跳转至Block::updateSubscriptions();
void Block::updateSubscriptions()
{
uORB::SubscriptionNode *sub = getSubscriptions().getHead();
int count = 0;
while (sub != NULL) {
if (count++ > maxSubscriptionsPerBlock) {
char name[blockNameLengthMax];
getName(name, blockNameLengthMax);
printf("exceeded max subscriptions for block: %s\n", name);
break;
}
sub->update();
sub = sub->getSibling();
}
}
跳转至sub->update();
void SubscriptionBase::update(void *data)
{
if (updated()) {
int ret = orb_copy(_meta, _handle, data);
if (ret != PX4_OK) { warnx("orb copy failed"); }
}
}
于是通过while (sub != NULL) {}循环，将订阅的主题都copy下来了
2.状态预计
卡尔曼矩阵
_x(),状态向量
_u(),输入向量
_P(),状态协方差矩阵
2.1初始值(只运行一次)
initP();
_x.setZero();
_u.setZero();
_flowX = 0;
_flowY = 0;
Matrix<float, n_x, n_x> _P; // state covariance matrix
void BlockLocalPositionEstimator::initP()
{
_P.setZero();
_P(X_x, X_x) = 1;
_P(X_y, X_y) = 1;
_P(X_z, X_z) = 1;
_P(X_vx, X_vx) = 1;
_P(X_vy, X_vy) = 1;
_P(X_vz, X_vz) = 1;
_P(X_bx, X_bx) = 1e-6;
_P(X_by, X_by) = 1e-6;
_P(X_bz, X_bz) = 1e-6;
_P(X_tz, X_tz) = 1;
}
<img src="http://img.blog.csdn.net/20160703115049323?watermark/2/text/aHR0cDovL2Jsb2cuY3Nkbi5uZXQv/font/5a6L5L2T/fontsize/400/fill/I0JBQkFCMA==/dissolve/70/gravity/Center" alt="" />
2.2准备运算所需的矩阵或向量
输入向量_u
<pre name="code" class="cpp">if (_integrate.get() && _sub_att.get().R_valid) {
Matrix3f R_att(_sub_att.get().R);
Vector3f a(_sub_sensor.get().accelerometer_m_s2);
_u = R_att * a;
_u(U_az) += 9.81f; // add g
}
动态矩阵A
// dynamics matrix Matrix<float, n_x, n_x> A; // state dynamics matrix A.setZero(); // derivative of position is velocity A(X_x, X_vx) = 1; A(X_y, X_vy) = 1; A(X_z, X_vz) = 1; // derivative of velocity is accelerometer acceleration // (in input matrix) - bias (in body frame) Matrix3f R_att(_sub_att.get().R); A(X_vx, X_bx) = -R_att(0, 0); A(X_vx, X_by) = -R_att(0, 1); A(X_vx, X_bz) = -R_att(0, 2); A(X_vy, X_bx) = -R_att(1, 0); A(X_vy, X_by) = -R_att(1, 1); A(X_vy, X_bz) = -R_att(1, 2); A(X_vz, X_bx) = -R_att(2, 0); A(X_vz, X_by) = -R_att(2, 1); A(X_vz, X_bz) = -R_att(2, 2);
输入矩阵B
// input matrix <span style="white-space:pre"> </span>Matrix<float, n_x, n_u> B; // input matrix B.setZero(); B(X_vx, U_ax) = 1; B(X_vy, U_ay) = 1; B(X_vz, U_az) = 1;
输入噪声协方差矩阵R
// input noise covariance matrix Matrix<float, n_u, n_u> R; R.setZero(); R(U_ax, U_ax) = _accel_xy_stddev.get() * _accel_xy_stddev.get(); R(U_ay, U_ay) = _accel_xy_stddev.get() * _accel_xy_stddev.get(); R(U_az, U_az) = _accel_z_stddev.get() * _accel_z_stddev.get();
系统过程噪声矩阵Q
// process noise power matrix Matrix<float, n_x, n_x> Q; Q.setZero(); float pn_p_sq = _pn_p_noise_density.get() * _pn_p_noise_density.get(); float pn_v_sq = _pn_v_noise_density.get() * _pn_v_noise_density.get(); Q(X_x, X_x) = pn_p_sq; Q(X_y, X_y) = pn_p_sq; Q(X_z, X_z) = pn_p_sq; Q(X_vx, X_vx) = pn_v_sq; Q(X_vy, X_vy) = pn_v_sq; Q(X_vz, X_vz) = pn_v_sq; // technically, the noise is in the body frame, // but the components are all the same, so // ignoring for now float pn_b_sq = _pn_b_noise_density.get() * _pn_b_noise_density.get(); Q(X_bx, X_bx) = pn_b_sq; Q(X_by, X_by) = pn_b_sq; Q(X_bz, X_bz) = pn_b_sq; // terrain random walk noise float pn_t_sq = _pn_t_noise_density.get() * _pn_t_noise_density.get(); Q(X_tz, X_tz) = pn_t_sq;
连续时间的卡尔曼滤波器预测值
// continuous time kalman filter prediction Vector<float, n_x> dx = (A * _x + B * _u) * getDt();
跟新-x，-P
// propagate _x += dx; //状态向量 _P += (A * _P + _P * A.transpose() +B * R * B.transpose() + Q) * getDt();//状态协方差矩阵
输入是自定义的噪声矩阵、旋转矩阵和加速度向量，输出是-x(状态向量)，-P(状态协方差矩阵)
3．矫正Correct
flowCorrect();
测量矩阵C
// flow measurement matrix and noise matrix Matrix<float, n_y_flow, n_x> C; C.setZero(); C(Y_flow_x, X_x) = 1; C(Y_flow_y, X_y) = 1;
噪声矩阵R
<span style="white-space:pre"> </span>R.setZero(); R(Y_flow_x, Y_flow_x) = _flow_xy_stddev.get() * _flow_xy_stddev.get(); R(Y_flow_y, Y_flow_y) = _flow_xy_stddev.get() * _flow_xy_stddev.get();
剩余向量r
Vector<float, 2> r = y - C * _x;(_x来自上面的sonarCorrect()等)
剩余协方差(逆)
// residual covariance, (inverse) Matrix<float, n_y_flow, n_y_flow> S_I = inv<float, n_y_flow>(C * _P * C.transpose() + R);( _P来自上面的sonarCorrect()等)
故障检测
// fault detection
float beta = (r.transpose() * (S_I * r))(0, 0);
if (beta > BETA_TABLE[n_y_flow]) {
if (_flowFault < FAULT_MINOR) {
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow fault, beta %5.2f", double(beta));
_flowFault = FAULT_MINOR;
}
} else if (_flowFault) {
_flowFault = FAULT_NONE;
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow OK");
}
光流矫正
if (_flowFault < fault_lvl_disable) {
Matrix<float, n_x, n_y_flow> K =
_P * C.transpose() * S_I;
_x += K * r; //跟新_x, 状态向量
_P -= K * C * _P; //跟新_P, 状态协方差矩阵
} else {
// reset flow integral to current estimate of position
// if a fault occurred
_flowX = _x(X_x);
_flowY = _x(X_y);
}
4.发布状态
if (_altHomeInitialized) {
// update all publications if possible
publishLocalPos();
publishEstimatorStatus();
if (_canEstimateXY) {
publishGlobalPos();
}
}
publishLocalPos()发布这么多
_pub_lpos.get().timestamp = _timeStamp; _pub_lpos.get().xy_valid = _canEstimateXY; _pub_lpos.get().z_valid = _canEstimateZ; _pub_lpos.get().v_xy_valid = _canEstimateXY; _pub_lpos.get().v_z_valid = _canEstimateZ; _pub_lpos.get().x = _x(X_x); // north _pub_lpos.get().y = _x(X_y); // east _pub_lpos.get().z = _x(X_z); // down _pub_lpos.get().vx = _x(X_vx); // north _pub_lpos.get().vy = _x(X_vy); // east _pub_lpos.get().vz = _x(X_vz); // down _pub_lpos.get().yaw = _sub_att.get().yaw; _pub_lpos.get().xy_global = _sub_home.get().timestamp != 0; // need home for reference _pub_lpos.get().z_global = _baroInitialized; _pub_lpos.get().ref_timestamp = _sub_home.get().timestamp; _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 = _sub_home.get().alt; _pub_lpos.get().dist_bottom = agl(); _pub_lpos.get().dist_bottom_rate = -_x(X_vz); _pub_lpos.get().surface_bottom_timestamp = _timeStamp; _pub_lpos.get().dist_bottom_valid = _canEstimateZ; _pub_lpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y)); _pub_lpos.get().epv = sqrtf(_P(X_z, X_z));
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 =
((_baroFault > fault_lvl_disable) << SENSOR_BARO)
+ ((_gpsFault > fault_lvl_disable) << SENSOR_GPS)
+ ((_lidarFault > fault_lvl_disable) << SENSOR_LIDAR)
+ ((_flowFault > fault_lvl_disable) << SENSOR_FLOW)
+ ((_sonarFault > fault_lvl_disable) << SENSOR_SONAR)
+ ((_visionFault > fault_lvl_disable) << SENSOR_VISION)
+ ((_mocapFault > fault_lvl_disable) << SENSOR_MOCAP);
_pub_est_status.get().timeout_flags =
(_baroInitialized << SENSOR_BARO)
+ (_gpsInitialized << SENSOR_GPS)
+ (_flowInitialized << SENSOR_FLOW)
+ (_lidarInitialized << SENSOR_LIDAR)
+ (_sonarInitialized << SENSOR_SONAR)
+ (_visionInitialized << SENSOR_VISION)
+ (_mocapInitialized << SENSOR_MOCAP);
publishGlobalPos()发布这么多
_pub_gpos.get().timestamp = _timeStamp; _pub_gpos.get().time_utc_usec = _sub_gps.get().time_utc_usec; _pub_gpos.get().lat = lat; _pub_gpos.get().lon = lon; _pub_gpos.get().alt = alt; _pub_gpos.get().vel_n = _x(X_vx); _pub_gpos.get().vel_e = _x(X_vy); _pub_gpos.get().vel_d = _x(X_vz); _pub_gpos.get().yaw = _sub_att.get().yaw; _pub_gpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y)); _pub_gpos.get().epv = sqrtf(_P(X_z, X_z)); _pub_gpos.get().terrain_alt = _altHome - _x(X_tz); _pub_gpos.get().terrain_alt_valid = _canEstimateT; _pub_gpos.get().dead_reckoning = !_canEstimateXY && !_xyTimeout; _pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter[0];
时间： 2024-11-06 13:59:37