写在前面
源码版本: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;
}
}
if (_imu_updated) {
predictState();
predictCovariance();
controlFusionModes();
runTerrainEstimator();
}
calculateOutputStates();
if (!ISFINITE(_state.quat_nominal(0)) || !ISFINITE(_output_new.quat_nominal(0))) {
return false;
}
return _control_status.flags.tilt_align && _control_status.flags.yaw_align;
}
这个就是整个EKF代码的核心函数了,分别由几个函数组成,各自完成不同的功能。
在首次进入update()这个函数的时候,系统会对变量进行初始化initialiseFilter();这个函数只执行一次,跟进:
bool Ekf::initialiseFilter()
{
imuSample imu_init = _imu_buffer.get_newest();
_delVel_sum += imu_init.delta_vel;
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)) {
_mag_counter = 1;
} else if ((_mag_counter != 0) && (_mag_sample_delayed.time_us != 0)) {
_mag_counter ++;
if (_mag_counter == (uint8_t)(_obs_buffer_length + 1)) {
_mag_filt_state = _mag_sample_delayed.mag;
} else if (_mag_counter > (uint8_t)(_obs_buffer_length + 1)) {
_mag_filt_state = _mag_filt_state * 0.9f + _mag_sample_delayed.mag * 0.1f;
}
}
}
磁力计数据出栈
if (_hgt_counter == 0) {
_primary_hgt_source = _params.vdist_sensor_type;
}
上面代码,选择高度源,可以来自于气压计,GPG,视觉、(毫米波)超声波等。下面开始选择高度源
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)) {
_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)) {
_hgt_counter ++;
if (_hgt_counter == (uint8_t)(_obs_buffer_length + 1)) {
_rng_filt_state = _range_sample_delayed.rng;
} else if (_hgt_counter > (uint8_t)(_obs_buffer_length + 1)) {
_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) {
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)) {
_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)) {
_hgt_counter ++;
if (_hgt_counter == (uint8_t)(_obs_buffer_length + 1)) {
_baro_hgt_offset = _baro_sample_delayed.hgt;
} else if (_hgt_counter > (uint8_t)(_obs_buffer_length + 1)) {
_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;
}
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 {
_gps_drift_velD = 0.0f;
_gps_alt_ref = 0.0f;
_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,复位状态
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;
}
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;
_control_status.flags.yaw_align = resetMagHeading(mag_init);
磁力计数值初始化,进行偏航校准,后面的超声波作为高度源的代码没有粘贴直接跳过。
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;
}
}
float dt = 0.001f * (float)FILTER_UPDATE_PERIOD_MS;
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);
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];
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) {
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));
}
P[10][10] = sq(_params.switch_on_gyro_bias * dt);
P[11][11] = P[10][10];
P[12][12] = P[10][10];
_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];
for (uint8_t index=16; index <= 21; index ++) {
P[index][index] = sq(_params.mag_noise);
}
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;
alignOutputFilter();
return true;
}
}
void Ekf::alignOutputFilter()
{
Quaternion quat_inv = _state.quat_nominal.inversed();
Quaternion q_delta = _output_sample_delayed.quat_nominal * quat_inv;
q_delta.normalize();
Vector3f vel_delta = _state.vel - _output_sample_delayed.vel;
Vector3f pos_delta = _state.pos - _output_sample_delayed.pos;
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
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)