EKF2学习之控制融合模式

                                                                                By snowpang  2017.8.10

1.存储控制状态值，并开启状态变化检测

_control_status.value
// bitmask containing filter control status
union filter_control_status_u {
    struct {
        uint16_t tilt_align  : 1; // 0 - true if the filter tilt alignment is complete
        uint16_t yaw_align   : 1; // 1 - true if the filter yaw alignment is complete
        uint16_t gps         : 1; // 2 - true if GPS measurements are being fused
        uint16_t opt_flow    : 1; // 3 - true if optical flow measurements are being fused
        uint16_t mag_hdg     : 1; // 4 - true if a simple magnetic yaw heading is being fused
        uint16_t mag_3D      : 1; // 5 - true if 3-axis magnetometer measurement are being fused
        uint16_t mag_dec     : 1; // 6 - true if synthetic magnetic declination measurements are being fused
        uint16_t in_air      : 1; // 7 - true when the vehicle is airborne
        uint16_t wind        : 1; // 8 - true when wind velocity is being estimated
        uint16_t baro_hgt    : 1; // 9 - true when baro height is being fused as a primary height reference
        uint16_t rng_hgt     : 1; // 10 - true when range finder height is being fused as a primary height reference
        uint16_t gps_hgt     : 1; // 11 - true when GPS height is being fused as a primary height reference
        uint16_t ev_pos      : 1; // 12 - true when local position data from external vision is being fused
        uint16_t ev_yaw      : 1; // 13 - true when yaw data from external vision measurements is being fused
        uint16_t ev_hgt      : 1; // 14 - true when height data from external vision measurements is being fused
        uint16_t fuse_beta   : 1; // 15 - true when synthetic sideslip measurements are being fused
    } flags;
    uint16_t value;
};
eg.control_state.value = 0x0001 表示tilt_align filter compelet
   control_state.value = 0x0400 表示距离传感器将被作为基础高度数据源融合

2.获取磁偏角，用来对齐yaw和融合
// Get the magnetic declination
calcMagDeclination();返回_mag_declination

3.监测切斜对齐，如果对齐标志位没有置位，在调整倾斜的同时，监控方差，一旦方差减小到只有3度的不确定性，对齐yaw和更新磁场状态,，并宣布切斜对齐完成，标志位置位。
(angle_err_var_vec(0) + angle_err_var_vec(1)) < sq(0.05235f) //0.05235 /pi * 180 = 3

resetMagHeading(_mag_sample_delayed.mag);
在里面比较了321和312欧拉序列的好坏，采用好的序列进行计算，更新了_state.quat_normial.参考链接http://www.atacolorado.com/eulersequences.doc.接着初始化四元数协方差initialiseQuatCovariances(angle_err_var_vec);
然后更新_state.mag_I，并更新输出状态的四元数_output_new.quat_nominal

4.检查在融合时间内的传感器数据更新，并计算从距离传感器和光流传感器坐标系到地球坐标系的的旋转矩阵，_R_rng_to_earth_2_2 > 0.7071f，z轴倾斜要小于45度

5.检查高度传感器延时时间并如果有必要会重设并改变传感器
controlHeightSensorTimeouts();
这个函数主要是处理没有融合最近高度测量值和超出最大高度测量范围的情况。然后重置，使用能够获得最好的高度测量数据源，如果数据源改变将使用改变后的数据源，并宣布当前数据不可用。

依靠垂直更新是否实时去检查IMU加速度计振动导致数据缺失，数据缺失将导致平均加速度指向零移动，这将使得INS认为它真在下降并产生一个正的垂直更新。判断错误的垂直加速度bad_vert_accel，再检查高度是否连续下降，高度惯性航位预测太久

判断是否重设垂直位置和速度状态，if ((P[9][9] > sq(_params.hgt_reset_lim)) && (hgt_fusion_timeout || continuous_bad_accel_hgt))，根据不同的高度检测方式进行
GPS高度/气压高度/距离传感器高度进行互补，都会用到气压传感器，最后resetHeight(),更新_state.pos(2) _state.vel(2),output_states

6.开始融合地磁数据
controlMagFusion();先判断航向角是否来自视觉，否则融合地磁，在地面上时，时刻更新地面高度，根据设置不同的融合模式开始计算，一般选择自动融合模式。
if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO)
检查是否有个足够的高度去避免地面磁场异常，检查是否有足够的水平速度变化使得航向角可观测，判断_yaw_angle_observable/_mag_bias_observable

判断是否3轴地磁融合被使用use_3D_fusion，如果使用，进行一次地磁对齐，之后进行，更新地磁协方差矩阵控制状态mag_3D置位，否则如果之前进行了use_3D_fuse,那么将之前的协方差矩阵保存，否则控制状态mag_hdg置位

如果使用的是3D地磁融合，但是没有外部辅助传感器，就必须融合磁偏角来防止长时间的航向偏移。在使用hdg融合时，不必融合磁偏角。
在fuseMag()中，获得_mag_innov，更新输出23个状态
在fuseDeclination()中，得到innovation，更新输出状态
在fuseHeading()中，得到_heading_innov，更新输出状态

7.气压融合
controlBaroFusion();
判断高度数据源，如果使用气压高度，更新控制状态值，_fuse_height置位，否则计算高度滤波偏差。

8.GPS融合
controlGpsFusion();如果GPS数据更新
判断是否应该使用GPS辅助计算速度和水平位置，在使用GPS时需要对齐角度。yaw对齐之后，开始GPS辅助，如果没用光流就重设位置和速度，否者只重设位置。
开始处理有GPS但是一段时间没有使用它，如果依靠GPS帮助限制姿态漂移7秒后没有帮助或者位置测量没有融合超过14s，然后重置位置和速度。

之后，_fuse_pos _fuse_vert_vel _fuse_hor_vel置位，并根据IMU的偏差矫正速度 位置 高度。然后检查GPS高度是否作为高度数据源，如果是即更新控制状态标志，并将    _fuse_height = true;

如果GPS数据没有更新，处理没有GPS或者没有使用它一段时间，仍然依赖它的情况，gps 10s未更新，清除速度状态，清除gps控制状态

9.融合空气阻力模型
controlDragFusion();
飞机必须在空中，才会融合。判断控制状态中的风速估计，如果第一次融合，则重置风速状态和风速协方差。否则融合阻力fuseDrag()，得到_drag_innov,更新风速状态。

10.融合光流融合
controlOpticalFlowFusion();
必须倾斜对齐和yaw对齐，然后判断是否使用GPS，如果不，则需要设定速度位置状态和协方差。需要计算对地高度，在高度对光流计算可靠范围之内，然后根据光流数据计算_state.vel否则速度清零。
如果飞机在空中更新位置到上一次知道的位置，然后观测量到卡尔曼滤波状态对齐输出alignOutputFilter()，在这个函数中，先计算输出的四元数和EKF状态四元数的quaternion的差，右乘EKF状态的共轭四元数，让后对齐归一化，计算速度和位置的差，让后将其加入输出状态来更新输出状态。
接下来处理依赖光流定位但是失去光流更新数据，速度状态清零。
最后进行光流数据融合，进行状态更新。

11.融合距离传感器
controlRangeFinderFusion(）；
判断高度数据源是否设定为距离传感器，如果是，就置位_control_status.flags.rng_hgt，然后根据欧拉角校正高度。如果_control_status.flags.rng_hgt被置位，就将fuse_height置位，告诉估计器要进行高度融合。如果要使用距离传感器，但是飞机在地面上，且高度数据源不可用，则人工合成一个高度给估计器，然后在融合高度。

12.外部视觉信息融合，和GPS差不多
controlExternalVisionFusion();

13.融合空速计数据，在非drag模式下才会使用。
controlAirDataFusion();

14.合成侧滑，在非drag模式下才会使用。主要用于固定翼.
controlBetaFusion();

15.最重要的部分来了，这里会做详细的代码分析。融合速度位置从以上多个数据源。controlVelPosFusion();
在这里先判断有没有辅助传感器，如果没有，则判断有没有imu数据更新是否大于200ms或者有高度融合信息，则将位置融合置位。
然后判断可以融合的数据，_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel，然后进入fuseVelPosHeight()，之后_fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false。
下面进入fuseVelPosHeight()函数，在里面先定义了6个变量为融合做准备，
bool fuse_map[6] = {}; // 映射关系[VN,VE,VD,PN,PE,PD] 为真表示观测量可用
bool innov_check_pass_map[6] = {}; // 观测量的更新平滑度检查
float R[6] = {}; // 观测协方差
float gate_size[6] = {}; // 观测量的更新平滑度检查门槛值
float Kfusion[24] = {}; // 用来存放卡尔曼单个观测增益值
先判断_fuse_hor_vel，如果为真，fuse_map[0] = fuse_map[1] = true，更新_vel_pos_innov[0]，_vel_pos_innov[1]，R[0],R[1]，gate_size[0]，gate_size[1]
在判断_fuse_vert_vel，如果为真，fuse_map[2] = true，更新_vel_pos_innov[2]，R[2]，gate_size[2]
判断_fuse_pos，为真，fuse_map[3] = fuse_map[4] = true，再判断位置数据源来自哪里（GPS/EV/fake GPS），然后更新R[3]，R[4]，_vel_pos_innov[3]，_vel_pos_innov[4]，gate_size[3]，gate_size[4]
判断_fuse_height，为真，fuse_map[5] = true，在判断高度数据来源（气压计/gps高度/距离传感器/EV高度），更新_vel_pos_innov[5]，R[5]，gate_size[5]

接下来计算更新测试比率，计算更新协防差（SK = HPH + R），_vel_pos_innov_var[],计算更新比率—_vel_pos_test_ratio[]。

然后检查位置 速度 高度更新innov_check_pass_map[],中位置高度 位置在切斜对齐后，就让其融合。开始计算卡尔曼增益，再更新协方差矩阵。检查协方差是否正常，如果不正常则矫正，并发布哪个状态不正常，否者，应用更新的协方差矩阵做校正，得到矫正后的P矩阵，然后做协防差矩阵错误修复fixCovarianceErrors()，之后进入融合，再次更新状态State.

最后报告航迹推算_is_dead_reckoning,如果我们不再融合测量限制速度漂移。
融合的状态索引如下：
查看estimator_status中的states[32]。状态的索引如下：
    [0 ... 3] 四元数
    [4 ... 6] 速度 NED (m/s)
    [7 ... 9] 位置 NED (m)
    [10 ... 12] IMU 角度增量偏差 XYZ (rad)
    [13 ... 15] IMU 速度增量偏差 XYZ (m/s)
    [16 ... 18] 地球磁场 NED (gauss)
    [19 ... 21] 机体磁场 XYZ (gauss)
    [22 ... 23] 风速 NE (m/s)
    [24 ... 32] 未使用