这里有一小段代码：

pidHeightRate.measured = (pidHeightRate.measured + acc_error * dt)*0.98f + 0.02f*(Control_high - last_high)/dt; 
 last_high =  pidHeightHigh.measured = Control_high; 

其中

typedef volatile struct
{
    float desired;     ///期望
    float offset;      //
    float prevError;    // 上次偏差
    float integ;        //误差积分累加值
    float kp;           //p参数
    float ki;           //i参数
    float kd;           //d参数
    float IntegLimitHigh;       //< integral limit
    float IntegLimitLow;
    float measured;     ////pid反馈量
    float out;
    float OutLimitHigh;
    float OutLimitLow;    
    float Control_OutPut;//控制器总输出
    float Last_Control_OutPut;//上次控制器总输出
    float Control_OutPut_Limit;//输出限幅
        /***************************************/
    float Last_FeedBack;//上次反馈值
    float Dis_Err;//微分量
    float Dis_Error_History[5];//历史微分量
    float Err_LPF;
    float Last_Err_LPF;
    float Dis_Err_LPF;

//    int8_t Err_Limit_Flag :1;//偏差限幅标志
//    int8_t Integrate_Limit_Flag :1;//积分限幅标志
//    int8_t Integrate_Separation_Flag :1;//积分分离标志        
  Butter_BufferData Control_Device_LPF_Buffer;//控制器低通输入输出缓冲    
}PidObject;

PidObject pidHeightRate;

一，概念解释

需要估计垂直速度，有两种原始方法：

1，加速度积分  

     速度 = 上一时刻速度 + 加速度×dt

特点是：

短时间很准，响应快，但积分久了会漂移（传感器零偏导致）

2，高度微分

     速度 = (当前高度−上次高度)/dt

特点是：

没有漂移，但噪声大（高度传感器有噪声，微分放大噪声）

既想要方法①的快速响应和平滑性，又想要方法②的长期稳定性，但两者单独用都不完美。互补滤波器把两者按权重加起来，让高频成分信任加速度积分，低频成分信任高度微分，从而得到一个“既不漂移也不嘈杂”的速度估计。

注：

• 高频成分：信号中变化很快的部分，比如你用手快速抖动传感器得到的波动。

• 低频成分：信号中变化很慢的部分，比如长时间慢慢漂移出去的值。

再对应到两种测量方法

• 加速度积分：它对快速变化很敏感（你一抖它立刻有反应），但长时间会慢慢飘走。
  → 所以适合用来反映高频（快变）部分。

• 高度微分：它对快速变化反应迟钝（因为有噪声，直接微分抖得厉害），但长时间不会飘。
  → 所以适合用来反映低频（慢变、漂移）部分。

3，一阶互补滤波器的典型公式

对速度估计，最常见的形式是：

    速度_估计 = α × (速度_估计_上一时刻 + 加速度×dt)  +  (1-α) × (高度差分速度)

• α 是一个接近 1 的数（例如 0.98 或 0.99）

• 高度差分速度 = (当前高度 − 上次高度) / dt

• α 越接近 1 → 越信任加速度，响应越快，但漂移修正越慢（可能需要几分钟才能消除漂移）

• α 越小 → 越信任高度微分，但噪声会更大

这个滤波器在连续域相当于一个一阶低通滤波器（对误差信号）。你可以把它理解为一个截止频率为 f_c = (1-α)/(2π·dt) 的低通滤波器。当 α 接近 1 时，截止频率很低，只有极低频的误差（漂移）才被高度微分项修正，而高频噪声被滤除。

这比“直接平均”或“无记忆的加权平均”更优秀，因为它利用了历史信息（上一时刻的速度估计值），所以是一阶动态系统。

举例如下：

假设：

• 真实速度 = 1.0 m/s

• 加速度积分给出 = 1.05 m/s（轻微正漂移）

• 高度微分给出 = 0.95 m/s（有噪声，但无偏）

使用 α=0.98：

估计速度 = 0.98×1.05 + 0.02×0.95 = 1.029 + 0.019 = 1.048 m/s

比纯加速度积分（1.05）更靠近真实值，同时平滑了高度微分的噪声。经过多个周期，漂移会被逐渐拉回。

如果没有高度微分项（α=1），估计值会慢慢漂到 1.1、1.2…；如果没有加速度积分项（α=0），速度会剧烈抖动。互补滤波让两者取长补短。

4，理解一下两部分速度的物理定义

一、速度 = 上一时刻速度 + 加速度 × dt 的原理

这是匀加速直线运动的速度公式在离散时间下的应用。

关键点：

• 高度 h 可以用气压计、超声波、激光测距等传感器获取

• 这个公式对噪声非常敏感，因为高度测量中如果有微小波动，除以很小的 dt 会放大成很大的速度波动

• 优点是没有积分漂移，长期稳定

代码正是结合了两者：大部分信任积分结果（平滑响应快），小部分信任差分结果（抑制漂移），形成了互补滤波。

• 速度 = 上一时刻速度 + 加速度×dt：来自“速度变化量 = 加速度 × 时间”的积分思想

• 速度 = (当前高度 − 上次高度)/dt：来自“速度 = 位置变化量 ÷ 时间”的微分思想

• 两者都是物理定律在离散程序中的最直接近似，也是互补滤波的基础公式

二，代码逻辑

这两行代码实现了高度-速度串级控制中的核心融合逻辑。

整体控制逻辑图（串级PID）

期望高度 ──→ [高度外环PID] ──→ 期望速度 ──→ [速度内环PID] ──→ 执行器（电机等）
                                       ↑                                               ↑
                                   高度反馈                                 速度反馈
                       (pidHeightHigh.measured)           (pidHeightRate.measured)

总结：第一行做互补滤波以获得高质量速度反馈，第二行更新高度反馈并保存高度历史值，两者共同为串级高度控制提供稳定可靠的反馈信号。