PX4_ECL_EKF代码分析2
写在前面
源码版本：1.6.0rc1
源码位置1：Firmware-1.6.0rc1\src\modules\ekf2_main.cpp
源码位置2：Firmware-1.6.0rc1\src\lib\ecl\EKF\
整体框架：
上图PX4的EKF代码框架，PX4的代码由两部分组成，一部分是在modules下的ekf2，另一部分是ecl代码库中EKF部分。
第一部分实现数据的订阅（subscribe）、整理、储存、经过处理的数据发布（publish）。
第二部分实现数据的处理。
第二部分：
bool Ekf::update()
{
if (!_filter_initialised) {
_filter_initialised = initialiseFilter();
if (!_filter_initialised) {
return false;
}
}
// Only run the filter if IMU data in the buffer has been updated
if (_imu_updated) {
// perform state and covariance prediction for the main filter
predictState();
predictCovariance();
// control fusion of observation data
controlFusionModes();
// run a separate filter for terrain estimation
runTerrainEstimator();
}
// the output observer always runs
calculateOutputStates();
// check for NaN or inf on attitude states
if (!ISFINITE(_state.quat_nominal(0)) || !ISFINITE(_output_new.quat_nominal(0))) {
return false;
}
// We don't have valid data to output until tilt and yaw alignment is complete
return _control_status.flags.tilt_align && _control_status.flags.yaw_align;
}
这个就是整个EKF代码的核心函数了，分别由几个函数组成，各自完成不同的功能。
在首次进入update()这个函数的时候，系统会对变量进行初始化initialiseFilter();这个函数只执行一次，跟进：
bool Ekf::initialiseFilter()
{
// Keep accumulating measurements until we have a minimum of 10 samples for the required sensors
//继续累积测量，直到有至少10个样品为所需的传感器
// Sum the IMU delta angle measurements
imuSample imu_init = _imu_buffer.get_newest();
_delVel_sum += imu_init.delta_vel;
// Sum the magnetometer measurements
//对磁强计的测量值求和
if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
if ((_mag_counter == 0) && (_mag_sample_delayed.time_us != 0)) {
// initialise the counter when we start getting data from the buffer
_mag_counter = 1;
} else if ((_mag_counter != 0) && (_mag_sample_delayed.time_us != 0)) {
// increment the sample count and apply a LPF to the measurement
_mag_counter ++;
// don't start using data until we can be certain all bad initial data has been flushed
//在确定所有坏的初始数据都已被清除之前，不要开始使用数据
if (_mag_counter == (uint8_t)(_obs_buffer_length + 1)) {
// initialise filter states
_mag_filt_state = _mag_sample_delayed.mag;
} else if (_mag_counter > (uint8_t)(_obs_buffer_length + 1)) {
// noise filter the data
_mag_filt_state = _mag_filt_state * 0.9f + _mag_sample_delayed.mag * 0.1f;
}
}
}
磁力计数据出栈
// set the default height source from the adjustable parameter
if (_hgt_counter == 0) {
_primary_hgt_source = _params.vdist_sensor_type;
}
上面代码，选择高度源，可以来自于气压计，GPG，视觉、（毫米波）超声波等。下面开始选择高度源
// accumulate enough height measurements to be confident in the qulaity of the data
//积累足够的高度测量值以对保证数据的质量
if (_primary_hgt_source == VDIST_SENSOR_RANGE) {//使用超声波
if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) {
if ((_hgt_counter == 0) && (_range_sample_delayed.time_us != 0)) {
// initialise the counter height fusion method when we start getting data from the buffer
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = true;//使能超声波标志位
_control_status.flags.ev_hgt = false;
_hgt_counter = 1;
} else if ((_hgt_counter != 0) && (_range_sample_delayed.time_us != 0)) {
// increment the sample count and apply a LPF to the measurement
_hgt_counter ++;
// don't start using data until we can be certain all bad initial data has been flushed
if (_hgt_counter == (uint8_t)(_obs_buffer_length + 1)) {
// initialise filter states
_rng_filt_state = _range_sample_delayed.rng;
} else if (_hgt_counter > (uint8_t)(_obs_buffer_length + 1)) {
// noise filter the data
_rng_filt_state = 0.9f * _rng_filt_state + 0.1f * _range_sample_delayed.rng;
}
}
}//超声波数据出栈
} else if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS) {//使用气压计或者GPS
// if the user parameter specifies use of GPS for height we use baro height initially and switch to GPS
//如果user参数指定高度使用GPS，我们首先使用气压高度，然后切换到GPS
// later when it passes checks.
if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
if ((_hgt_counter == 0) && (_baro_sample_delayed.time_us != 0)) {
// initialise the counter and height fusion method when we start getting data from the buffer
_control_status.flags.baro_hgt = true;//使用气压计
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_hgt_counter = 1;
} else if ((_hgt_counter != 0) && (_baro_sample_delayed.time_us != 0)) {
// increment the sample count and apply a LPF to the measurement
_hgt_counter ++;
// don't start using data until we can be certain all bad initial data has been flushed
if (_hgt_counter == (uint8_t)(_obs_buffer_length + 1)) {
// initialise filter states
_baro_hgt_offset = _baro_sample_delayed.hgt;
} else if (_hgt_counter > (uint8_t)(_obs_buffer_length + 1)) {
// noise filter the data
_baro_hgt_offset = 0.9f * _baro_hgt_offset + 0.1f * _baro_sample_delayed.hgt;
}
}
}
} else if (_primary_hgt_source == VDIST_SENSOR_EV) {//使用视觉传感器
_hgt_counter = _ev_counter;
} else {
return false;
}
// check to see if we have enough measurements and return false if not
bool hgt_count_fail = _hgt_counter <= 2 * _obs_buffer_length;
bool mag_count_fail = _mag_counter <= 2 * _obs_buffer_length;
bool ev_count_fail = ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVYAW)) && (_ev_counter <= 2 * _obs_buffer_length);
if (hgt_count_fail || mag_count_fail || ev_count_fail) {
return false;
} else {
// reset variables that are shared with post alignment GPS checks重置与后校准GPS检查共享的变量
_gps_drift_velD = 0.0f;
_gps_alt_ref = 0.0f;
// Zero all of the states
_state.vel.setZero();
_state.pos.setZero();
_state.gyro_bias.setZero();
_state.accel_bias.setZero();
_state.mag_I.setZero();
_state.mag_B.setZero();
_state.wind_vel.setZero();
检测是否有足够的测量数据，如果没有则返回 false，复位状态
// get initial roll and pitch estimate from delta velocity vector, assuming vehicle is static
//假设飞行器是静态的，从速度矢量得到初始横滚和俯仰估计
float pitch = 0.0f;
float roll = 0.0f;
if (_delVel_sum.norm() > 0.001f) {
_delVel_sum.normalize();
pitch = asinf(_delVel_sum(0));
roll = atan2f(-_delVel_sum(1), -_delVel_sum(2));
} else {
return false;
}
// calculate initial tilt alignment
matrix::Euler<float> euler_init(roll, pitch, 0.0f);
_state.quat_nominal = Quaternion(euler_init);
_output_new.quat_nominal = _state.quat_nominal;
_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
根据上面得到的初始pitch 、roll计算初始倾斜角，并更新机体系到导航系的旋转矩阵。
Vector3f mag_init = _mag_filt_state;
// calculate the initial magnetic field and yaw alignment
_control_status.flags.yaw_align = resetMagHeading(mag_init);
磁力计数值初始化，进行偏航校准，后面的超声波作为高度源的代码没有粘贴直接跳过。
// initialise the state covariance matrix
initialiseCovariance();
协方差初始化函数，进去看一下：
void Ekf::initialiseCovariance()
{
for (unsigned i = 0; i < _k_num_states; i++) {//首先是协方差矩阵内容清零，以便赋值
for (unsigned j = 0; j < _k_num_states; j++) {
P[i][j] = 0.0f;
}
}
// calculate average prediction time step in sec//计算平均预测时间 单位 秒
float dt = 0.001f * (float)FILTER_UPDATE_PERIOD_MS;
// define the initial angle uncertainty as variances for a rotation vector
//定义初始角度不确定度为旋转矢量的方差
Vector3f rot_vec_var;
rot_vec_var(2) = rot_vec_var(1) = rot_vec_var(0) = sq(_params.initial_tilt_err);
//使用旋转向量方差初始化四元数协方差
initialiseQuatCovariances(rot_vec_var);
// velocity
P[4][4] = sq(fmaxf(_params.gps_vel_noise, 0.01f));
P[5][5] = P[4][4];
P[6][6] = sq(1.5f) * P[4][4];
// position
P[7][7] = sq(fmaxf(_params.gps_pos_noise, 0.01f));
P[8][8] = P[7][7];
if (_control_status.flags.rng_hgt) {//使用超声波 （这里不使用）
P[9][9] = sq(fmaxf(_params.range_noise, 0.01f));
} else if (_control_status.flags.gps_hgt) {//用GPS
float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
P[9][9] = sq(1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit));
} else {
P[9][9] = sq(fmaxf(_params.baro_noise, 0.01f));
}
// gyro bias
P[10][10] = sq(_params.switch_on_gyro_bias * dt);
P[11][11] = P[10][10];
P[12][12] = P[10][10];
// accel bias
_prev_dvel_bias_var(0) = P[13][13] = sq(_params.switch_on_accel_bias * dt);
_prev_dvel_bias_var(1) = P[14][14] = P[13][13];
_prev_dvel_bias_var(2) = P[15][15] = P[13][13];
// earth frame and body frame magnetic field
// set to observation variance
for (uint8_t index=16; index <= 21; index ++) {
P[index][index] = sq(_params.mag_noise);
}
// wind
P[22][22] = 1.0f;
P[23][23] = 1.0f;
}
initialiseQuatCovariances(rot_vec_var);这个函数可以到matlab模型里面可以找到 ，编译后会生成此函数，matlab路径：
Firmware-1.6.0rc1\src\lib\ecl\EKF\matlab\Inertial Nav EKF\QuatErrTransferEquations.m
生成的.c文件也在同路径下。
// 重置基本的融合超时计数器
_time_last_hgt_fuse = _time_last_imu;
_time_last_pos_fuse = _time_last_imu;
_time_last_vel_fuse = _time_last_imu;
_time_last_hagl_fuse = _time_last_imu;
_time_last_of_fuse = _time_last_imu;
// reset the output predictor state history to match the EKF initial values
//重置输出预测器状态历史以匹配EKF初始值
alignOutputFilter();
return true;
}
}//end bool Ekf::initialiseFilter();
void Ekf::alignOutputFilter()
{
// calculate the quaternion delta between the output and EKF quaternions at the EKF fusion time horizon
//在EKF融合时间层计算输出和EKF四元数之间的delta四元数
Quaternion quat_inv = _state.quat_nominal.inversed();
Quaternion q_delta = _output_sample_delayed.quat_nominal * quat_inv;
q_delta.normalize();
// calculate the velocity and posiiton deltas between the output and EKF at the EKF fusion time horizon
//在EKF融合时间层计算输出和EKF之间的速度和位置增量
Vector3f vel_delta = _state.vel - _output_sample_delayed.vel;
Vector3f pos_delta = _state.pos - _output_sample_delayed.pos;
// loop through the output filter state history and add the deltas
//循环遍历输出过滤器状态历史记录并添加增量
outputSample output_states = {};
unsigned output_length = _output_buffer.get_length();
for (unsigned i = 0; i < output_length; i++) {
output_states = _output_buffer.get_from_index(i);
output_states.quat_nominal *= q_delta;
output_states.quat_nominal.normalize();
output_states.vel += vel_delta;
output_states.pos += pos_delta;
_output_buffer.push_to_index(i, output_states);
}
}
以上就是我对ECL库中EKF代码初始化函数的理解，有很多写在了注释里面，也有很多不理解的地方还在学习中，如有理解错误的地方请批评指正，欢迎一起学习填坑PX4，QQ：1103706199