加速度计和气压计、激光互补滤波融合算法

融合原理：

1. 加速度积分出来【速度】和【位移】
2. 由于积分会累计误差，倒置积分结果越偏越大
3. 用气压计来修正积分产生的误差
4. 修正分成位移修正和速度修正，两个都需要修正，否则偏离都会越来越大
5. 修正方法：第二传感器结果【减去】第一传感器结果，等到【误差】
6. 【误差】乘以一个【系数】得到【修正值】
7. 第一传感器加上【修正值】即得到【融合结果】
8. 【系数】一般需要乘以dt，不乘也行，你看这个系数是不是和阶低通的系数很像，它们效果也很像：越小滤波越强，变化越小，数据越平缓，反应也越慢
9. 网上大多算法只用了气压计的高度修正，但速度没有修正，如果用这个速度去定高，这个速度肯定会炸的
10. 用气压计高度的微分去修正速度，权重可比高度大一点，保证速度不会炸，这时可以把if(i==0)那部分去掉
11. 如果没有速度修正，也可心用修正后的高度微分+低通来算一个速度去定高，但反应会很慢
12. 融合后的位移也是没法完成避免气压计的缓慢飘动的，但这个飘动很小
13. 无人机的快速上下刹停还是要靠速度
14. 想要高度更稳就要加入激光，修正方法同理，权重可以超级加倍，只修正高度就可以了

 INV算法：

/*
 * 函数名：inertial_filter_predict
 * 功能：一次预测
 */
void inertial_filter_predict(float dt, float x[2], float acc)
{
    if (isfinite(dt))
    {
        if (!isfinite(acc))
        {
            acc = 0.0f;
        }
        x[0] += x[1] * dt + acc * dt * dt / 2.0f;
        x[1] += acc * dt;
    }
}

/*
 * 函数名：inertial_filter_correct
 * 功能：一次校正
 */
void inertial_filter_correct(float e, float dt, float x[2], int i, float w)
{
    if (isfinite(e) && isfinite(w) && isfinite(dt))
    {
        float ewdt = e * w * dt;
        x[i] += ewdt;
        if (i == 0)
        {
        x[1] += w * w * ewdt;
        }
    }
}

用法：

void estimator_update(float dt)
{
    if (dt < 0.01f)
        dt = 0.01f; // 防止dt过小
    if (!FC_STATE_CHECK(FC_ARMED))
    {
        // 未解锁时，加大baro权重, 防止acc积分误差过大
        baro_auto_gain = 10.f;
    }
    else
    {
        if (!FC_STATE_CHECK(FC_AIRBORN)) // 起飞时权重小，避免地面效应
        {
            baro_auto_gain = 0.1f;
        }
        else
        {
            if (baro_auto_gain < 1.0f)//恢复到正常值
            {
                baro_auto_gain += 0.01f;
            }
            else
            {
                baro_auto_gain = 1.0f;
            }
        }
    }

    if (FC_STATE_CHECK(FC_IMU_CALIBRATING) || (millis() < 500))
    {
        /*校准时重置高度、速度、气压起始高度 */
        fc_sensor_data.baro.offset = fc_sensor_data.baro.altitude;
        tofAlt = 0;
        accel_bias_corr[Z] = 0;
        z_est[ALT_POS] = 0;
        z_est[ALT_VEL] = 0;
    }

    baroAlt = fc_sensor_data.baro.altitude - fc_sensor_data.baro.offset; // 气压高度（减去起点） 单位：cm，
    baro_vel = pt1FilterApply(&baro_v_fiter, (baroAlt - baroAlt_pre) / dt); // 气压速度(低通滤波，其实互补滤波本身就有低通效果，不用也行) 单位：cm/s

    baro_vel = constrainf(baro_vel, -200.0f, 200.0f);//限制速度过大
    baroAlt_pre = baroAlt;
    acc[Z] = (zdrone_state.acc.z + accel_bias_corr[Z]) * GRAVITY_CMSS;//地理加速度转换成cm/s^2， 不是机体加速度
    acc_lpf[Z] = pt1FilterApply(&accZ_fiter, acc[Z]);//加速度低通滤波，其实互补滤波本身就有低通效果，不用也行

    if (acc_lpf[Z] > ACC_DEADBAND || acc_lpf[Z] < -ACC_DEADBAND)
    {
        // 加死区
    }

    inertial_filter_predict(dt, z_est, acc[Z]);//加速度积分，得到速度和位置

    corr_baro = baroAlt - z_est[ALT_POS];//气压计误差
    corr_baro_vel = baro_vel - z_est[ALT_VEL];//速度误差

    inertial_filter_correct(corr_baro, dt, z_est, ALT_POS, est_params_f.w_baro_p * baro_auto_gain);//高度修正
    inertial_filter_correct(corr_baro_vel, dt, z_est, ALT_VEL, est_params_f.w_baro_v * baro_auto_gain);//速度修正
    accel_bias_corr[Z] += corr_baro_vel * est_params_f.w_baro_a * dt * dt * dt;//加速度修正

    isTofAltValid = fc_sensor_data.tof.range < 6000 && fc_sensor_data.tof.quality > 0 && fc_sensor_data.tof.isOnline;//激光距离有效
    if (isTofAltValid)
    {
        //激光距离有效
        int16_t dtTof = fc_sensor_data.tof.range - tofRange_pre;
        tofRange_pre = fc_sensor_data.tof.range;
        if (ABS(dtTof) < 50)//丢弃激光距离变化过大的情况
        {
            /**激光变化量积分 */
            tofAlt += (dtTof * 0.1f); // cm
        }
        corr_tof = tofAlt - z_est[ALT_POS];//激光高度误差
        tof_vel = (tofAlt - tofAlt_pre) / dt;//激光速度
        tofAlt_pre = tofAlt;
        corr_tof_vel = tof_vel - z_est[ALT_VEL];
        inertial_filter_correct(corr_tof, dt, z_est, ALT_POS, est_params_f.w_tof_p * USED_TOF);//激光高度修正
        inertial_filter_correct(corr_tof_vel, dt, z_est, ALT_VEL, est_params_f.w_tof_v * USED_TOF);//激光速度修正
        accel_bias_corr[Z] += corr_tof_vel * est_params_f.w_tof_a * dt * dt * dt;

        obs_height = fc_sensor_data.tof.range * 0.1f;//光流对面的观测高度
    }
    else
    {
        tofAlt = z_est[ALT_POS];
        obs_height = z_est[ALT_POS];
    }
    obs_height = constrainf(obs_height, 1, 500);
    obs_height_lpf += (obs_height - obs_height_lpf) * 0.1f;

    estimator_xy_update(dt);

    z_est[ALT_VEL] = constrainf(z_est[ALT_VEL], -200.0f, 200.0f);//限制速度过大
    zdrone_state.pos.z = z_est[ALT_POS];//融合高度
    zdrone_state.vel.z = applyDeadbandFloat(z_est[ALT_VEL], 1);//融合速度
}