2017.8.4 by snow
2cd
read mag data and set it
这里有个细节,对于多个magnetometer同时工作时,在处理时需要
// Check if the magnetometer ID has changed and reset learned bias parameters if it has
//Do not do this when armed to prevent potential time slips casued by parameter set and notification events
if (vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) { //飞机处于未绑定状态才会做传感器选择
orb_copy(ORB_ID(sensor_selection), sensor_selection_sub, &sensor_selection);//在src/moudule/sensors 中发布
_ekf.setMagData(1000 * (uint64_t)mag_time_ms, mag_data_avg_ga);//进行EKF数据输入
函数原型为:
void EstimatorInterface::setMagData(uint64_t time_usec, float (&data)[3])//在lib/ecl/estimator_interface中
// limit data rate to prevent data being lost
if (time_usec - _time_last_mag > _min_obs_interval_us) //必须满足这个条件,才会更新ekf2中的mag数据
_mag_buffer.push(mag_sample_new);
在这个函数里对时间做了如下处理
mag_sample_new.time_us = time_usec - _params.mag_delay_ms * 1000; //这个延时参数是可以调节的
mag_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2; //这个时间在ekf更新里面会和imu更新时间比较计算出更新延时
3rd
read baro data and set it
超时检测,数据正常,进行EKF设置_ekf.set_air_density(rho);
// set air density used by the multi-rotor specific drag force fusion
函数原型:
void set_air_density(float air_density) {_air_density = air_density;}
其中:
const float pressure_to_density = 100.0f / (CONSTANTS_AIR_GAS_CONST * (20.0f - CONSTANTS_ABSOLUTE_NULL_CELSIUS));// estimate air density assuming typical 20degC ambient temperature
float rho = pressure_to_density * sensor_baro.pressure; //得到空气密度
4th
// calculate static pressure error = Pmeas - Ptruth
// model position error sensitivity as a body fixed ellipse with different scale in the positive and negtive X direction
const float max_airspeed_sq = MAX_AIRSPEED * MAX_AIRSPEED;
float K_pstatic_coef_x;
if (_vel_body_wind(0) >= 0.0f) {
K_pstatic_coef_x = _K_pstatic_coef_xp.get();
} else {
K_pstatic_coef_x = _K_pstatic_coef_xn.get();
}
float pstatic_err = 0.5f * rho * (K_pstatic_coef_x * fminf(_vel_body_wind(0) * _vel_body_wind(0), max_airspeed_sq)
+ _K_pstatic_coef_y.get() * fminf(_vel_body_wind(1) * _vel_body_wind(1), max_airspeed_sq)
+ _K_pstatic_coef_z.get() * fminf(_vel_body_wind(2) * _vel_body_wind(2), max_airspeed_sq));
// correct baro measurement using pressure error estimate and assuming sea level gravity
balt_data_avg += pstatic_err / (rho * 9.80665f);
这里做了气压高度模型,在机体在不同角度时,产生的气压不同,得到模型输出高度
// push to estimator
_ekf.setBaroData(1000 * (uint64_t)balt_time_ms, balt_data_avg);
函数原型:
void EstimatorInterface::setBaroData(uint64_t time_usec, float data)//data为高度
// limit data rate to prevent data being lost
if (time_usec - _time_last_baro > _min_obs_interval_us)//对更新时间做判断
_baro_buffer.push(baro_sample_new);
这里同样会对时间做处理
baro_sample_new.time_us = time_usec - _params.baro_delay_ms * 1000;//这个延时参数可调节
baro_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
5th
//如果gps数据更新,将其推送到估计器
_ekf.setGpsData(gps.timestamp, &gps_msg);
函数原型:
void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps)
在此需要判断是否融合gps
// limit data rate to prevent data being lost
bool need_gps = (_params.fusion_mode & MASK_USE_GPS) || (_params.vdist_sensor_type == VDIST_SENSOR_GPS);
与其他传感器一样,也会判断更新时间,并计算更新时间
在此还要对GPS数据进行处理,通过函数collect_gps(time_usec, gps)
函数原型:
bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
其中,先对控制模式进行判断
// Run GPS checks whenever the WGS-84 origin is not set or the vehicle is not using GPS
// Also run checks if the vehicle is on-ground as the check data can be used by vehicle pre-flight checks
if (!_control_status.flags.in_air || !_NED_origin_initialised || !_control_status.flags.gps)//_NED_origin_initialised这flag会在init中被复位
并检查GPS是否可用gps_is_good(gps);每次都会更新
然后对GPS数据进行处理,之后更新buffer
_gps_buffer.push(gps_sample_new);
6th
光流更新
_ekf.setOpticalFlowData(optical_flow.timestamp, &flow);
距离传感器更新
_ekf.setRangeData(range_finder.timestamp, range_finder.current_distance);
如果有视觉传感器
_ekf.setExtVisionData(ev.timestamp, &ev_data);
6.进行ekf更新
_ekf.update()在这里完成整个ekf算法
1st
滤波初始化initialiseFilter()
在这里面读取imu_buffer.get_newest();
读取地磁_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)
读取视觉_ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed)//如果有的话
读取距离_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)
更新四元数
// 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;
坐标系转换(统一到NED坐标系)
// update transformation matrix from body to world frame
_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
对齐yaw
// calculate the averaged magnetometer reading
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();
如果条件满足可以进行地形估计,需要距离传感器
// try to initialise the terrain estimator
_terrain_initialised = initHagl();
// reset the output predictor state history to match the EKF initial values
alignOutputFilter();
2rd
// perform state and covariance prediction for the main filter
predictState();
predictCovariance();
这两步为卡尔曼滤波的预测
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)