EKF2学习笔记之运行流程1

                                                                                                2017.8.3 by snow
src/moudule/ekf2/ekf2_main.cpp
void Ekf2::task_main()
1.do subscribe sensors update :订阅传感器数据
2.updateParams() initialise parameter cache // on ekf2_params.c
3.into main cycle while(!_task_should_exit)
4.update sensors data  //orb_copy
5.note:  push sensors data into estimator. It is the most important process
the first step:
push imu data into estimator:_ekf.setIMUData(now, sensors.gyro_integral_dt * 1.e6f, sensors.accelerometer_integral_dt * 1.e6f,
                gyro_integral, accel_integral);
now:now = hrt_absolute_time();//当前时间，这个非常关键，其他传感器的融合与这个时间息息相关，在一个延迟的fusion time horizon上运行，运行其他传感器在每次测量时相对imu存在不同时间的延迟
其他数据为传感器数据

// Accumulate imu data and store to buffer at desired rate
void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt,
                    float (&delta_ang)[3], float (&delta_vel)[3]) //这个函数相当关键，下面一步一步解析


if (!_initialised) { //在这里初始化time_usec  Ekf2每次循环应该都需要初始化一次
    init(time_usec);
    _initialised = true;
}
下面是函数原型
bool Ekf::init(uint64_t timestamp) 在ekf.cpp中，这里将所有的传感器数据清零，在这个里面有个重要的步骤：bool ret = initialise_interface(timestamp);

initialise_interface() 函数原型如下
bool EstimatorInterface::initialise_interface(uint64_t timestamp) 在estimator_interface.cpp中，这里面的炒作都很重要，直接影响ekf估计器的运行

// find the maximum time delay required to compensate for
    uint16_t max_time_delay_ms = math::max(_params.mag_delay_ms,
                     math::max(_params.range_delay_ms,
                         math::max(_params.gps_delay_ms,
                         math::max(_params.flow_delay_ms,
                             math::max(_params.ev_delay_ms,
                             math::max(_params.airspeed_delay_ms, _params.baro_delay_ms))))));

// calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter
    _imu_buffer_length = (max_time_delay_ms / FILTER_UPDATE_PERIOD_MS) + 1

static const unsigned FILTER_UPDATE_PERIOD_MS = 12;    // ekf prediction period in milliseconds - this should ideally be an integer multiple of the IMU time delta
以毫秒为单位的EKF预测周期——这应该是IMU时间增量的整数倍。


计算观测fuffer长度并作限幅
// set the observaton buffer length to handle the minimum time of arrival between observations in combination
    // with the worst case delay from current time to ekf fusion time
    // allow for worst case 50% extension of the ekf fusion time horizon delay due to timing jitter
    uint16_t ekf_delay_ms = max_time_delay_ms + (int)(ceilf((float)max_time_delay_ms * 0.5f));
    _obs_buffer_length = (ekf_delay_ms / _params.sensor_interval_min_ms) + 1;
// limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate)
    _obs_buffer_length = math::min(_obs_buffer_length, _imu_buffer_length);

分配buffer空间
if (!(_imu_buffer.allocate(_imu_buffer_length) &&
          _gps_buffer.allocate(_obs_buffer_length) &&
          _mag_buffer.allocate(_obs_buffer_length) &&
          _baro_buffer.allocate(_obs_buffer_length) &&
          _range_buffer.allocate(_obs_buffer_length) &&
          _airspeed_buffer.allocate(_obs_buffer_length) &&
          _flow_buffer.allocate(_obs_buffer_length) &&
          _ext_vision_buffer.allocate(_obs_buffer_length) &&
          _drag_buffer.allocate(_obs_buffer_length) &&
          _output_buffer.allocate(_imu_buffer_length))) {
        ECL_ERR("EKF buffer allocation failed!");
        unallocate_buffers();
        return false;
    }
做一些清零处理，这里面重要的一点是 _initialised = false;这个意味着，每次setIMUData()时都会再次对时间进行更新
// zero the data in the observation buffers
// zero the data in the imu data and output observer state buffers

到此setIMUData()里面的初始化完成。以上内容至关重要，需要详细阅读代码和有一定卡尔曼滤波基础

然后将传感更新的数据转化成EKF里面计算所需的数据。用一下结构体进行过渡
// copy data
    imuSample imu_sample_new = {};
计算圆锥振动量的指标
//calculate a metric which indicates the amount of coning vibration

Vector3f temp = cross_product(imu_sample_new.delta_ang, _delta_ang_prev);
_vibe_metrics[0] = 0.99f * _vibe_metrics[0] + 0.01f * temp.norm();

// calculate a metric which indiates the amount of high frequency gyro vibration
temp = imu_sample_new.delta_ang - _delta_ang_prev;
_delta_ang_prev = imu_sample_new.delta_ang;
_vibe_metrics[1] = 0.99f * _vibe_metrics[1] + 0.01f * temp.norm();

// calculate a metric which indicates the amount of high fequency accelerometer vibration
temp = imu_sample_new.delta_vel - _delta_vel_prev;
_delta_vel_prev = imu_sample_new.delta_vel;
_vibe_metrics[2] = 0.99f * _vibe_metrics[2] + 0.01f * temp.norm();

积累下的样本IMU数据推到缓冲区，当新的采样数据变得可用
// accumulate and down-sample imu data and push to the buffer when new downsampled data becomes available
collect_imu(imu_sample_new) //这个函数原型如下，在ekf.cppz中。
bool Ekf::collect_imu(imuSample &imu)这里面对imu数据进行处理，得到了四元素
如果_imu_down_sampled.delta_ang_dt >= target_dt - _imu_collection_time_adj为真才算会进行EKF imu数据的更新
其中// accumulate the time deltas
    _imu_down_sampled.delta_ang_dt += imu.delta_ang_dt;

// if the target time delta between filter prediction steps has been exceeded
    // write the accumulated IMU data to the ring buffer
    float target_dt = (float)(FILTER_UPDATE_PERIOD_MS) / 1000;
        
_imu_collection_time_adj这个值初始化是0.0f,在imudata更新中会被更新
// accumulate the amount of time to advance the IMU collection time so that we meet the
        
// average EKF update rate requirement
        
_imu_collection_time_adj += 0.01f * (_imu_down_sampled.delta_ang_dt - target_dt);
        
_imu_collection_time_adj = math::constrain(_imu_collection_time_adj, -0.5f * target_dt, 0.5f * target_dt);

这个函数返回为真，_imu_buffer.push(imu_sample_new);到此算是imu_data才算真正的更新，这个操作会在后面
// Sum the IMU delta angle measurements
imuSample imu_init = _imu_buffer.get_newest();将buffer里面的数据取出进行下一步融合，在ekf.cpp中的bool Ekf::initialiseFilter()函数中。

然后_imu_updated = true;并判断融合模式if (_params.fusion_mode & MASK_USE_DRAG)，进行融合状态的更新

最后更新_imu_sample_delayed，并计算最小观测时间积分，到此setIMUdata结束
// get the oldest data from the buffer
_imu_sample_delayed = _imu_buffer.get_oldest();

// calculate the minimum interval between observations required to guarantee no loss of data
// this will occur if data is overwritten before its time stamp falls behind the fusion time horizon
_min_obs_interval_us = (_imu_sample_new.time_us - _imu_sample_delayed.time_us) / (_obs_buffer_length - 1);
只有IMU更新了，才会融合其他传感器数据。EKF在一个延迟的fusion time horizon上运行，以允许传感器在每次测量时相对于IMU存在不同的时间延迟。
每个传感器的数据都是FIFO缓存的并由EKF从缓存中检索，得以在正确的时候使用。