TECS——ArduPilot——高度与爬升速度的估计（3阶互补滤波）

版权声明：本文为博主原创博文，未经允许不得转载，若要转载，请说明出处并给出博文链接 

本文详细讲解了ArduPilot中AP_TECS库中的用于估计高度和垂直方向上的爬升速率的三阶互补滤波的实现！

      首先，需要强调的是，本文介绍的3阶互补滤波（Complementary Filter）是在飞控无法获取EKF（Extend Kalman Filter）提供的北东地三个方向上的速度的情况下才使用的，意思是首选扩展卡尔曼滤波，扩展卡尔曼滤波不OK，使用3阶互补滤波。

      其次，下面是阅读代码后的个人理解与心得，如有不对，还望指正!!!!!

      3阶互补滤波:(融合气压计的高度信息与加速度计的垂直方向数据）

       omega2 = omega * omega，（omega-互补滤波的频率 - 由全参数给定，默认为3 radius / s）

       error_h = baro_h - filter_h，（h-高度，baro-气压计，filter-互补滤波器）

       Integ1 = error_h * omega2 * omega，（第一阶段，可以理解为高度方向上的加加速度 - 加速度的微分）

       dd_h = Integ1 * dt + dd_h，（通过积分，获得下一时刻的dd_h-滤波器估计出的高度的加速度）

       Integ2 = dd_h + acc_h + error_h * omega2 * 3，（第二阶，可以理解为高度方向上校正后的加速度）

       climb_rate = Integ2 * dt + climb_rate，（积分后得到下一时刻的爬升速率）

       Integ3 = error_h * omega * 3 + climb_rate，（第三阶段，可以理解为高度方向上校正后的速度）

       filter_h = Integ3 * dt + filter_h，（积分，估计出下一刻的高度）

其中，acc_h = - （acc_z + g）。acc_z =加速度计获取的z方向（垂直向下）数据，g - 重力加速度

          dt =采样间隔

代码如下：

/*
          use a complimentary filter to calculate climb_rate. This is
          designed to minimise lag
         */
        float baro_alt = _ahrs.get_baro().get_altitude();
        // Get height acceleration
        float hgt_ddot_mea = -(_ahrs.get_accel_ef().z + GRAVITY_MSS);
        // Perform filter calculation using backwards Euler integration
        // Coefficients selected to place all three filter poles at omega
        float omega2 = _hgtCompFiltOmega*_hgtCompFiltOmega;
        float hgt_err = baro_alt - _height_filter.height;
        float integ1_input = hgt_err * omega2 * _hgtCompFiltOmega;

        _height_filter.dd_height += integ1_input * DT;

        float integ2_input = _height_filter.dd_height + hgt_ddot_mea + hgt_err * omega2 * 3.0f;

        _climb_rate += integ2_input * DT;

        float integ3_input = _climb_rate + hgt_err * _hgtCompFiltOmega * 3.0f;
        // If more than 1 second has elapsed since last update then reset the integrator state
        // to the measured height
        if (DT > 1.0f) {
            _height_filter.height = _height;
        } else {
            _height_filter.height += integ3_input*DT;
        }

之所以不采用2阶互补滤波，而采用3阶互补滤波，是不想忽略加速度偏差或者重力计算偏差导致的误差，进一步精确估计结果。

 

      本文提出的3阶互补滤波参考的是一篇来自AIAA的论文“优化巴洛惯性垂直通道的收益”。

      想进一步学习的链接给出：

      https://download.csdn.net/download/u012814946/10437947