EKF2学习笔记之运行流程2

                                                                                                    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();
这两步为卡尔曼滤波的预测