pixhawk Lacal_position_estimator数据流

一、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;
}

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];