ComplementaryFilter.cs

// ==============================
// 互补滤波器
// ==============================

public class ComplementaryFilter : IFilter
{
    public double Alpha { get; set; } = 0.79;
    private const double DegToRad = Math.PI / 180.0;
    private const double RadToDeg = 180.0 / Math.PI;

    private double roll = 0, pitch = 0;
    private long lastTimestamp = 0;

    // 重力验证器
    private GravityValidator? validator;

    public ComplementaryFilter(double alpha = 0.79)
    {
        Alpha = alpha;
    }

    public void SetGravityValidator(bool enable, double baseTolerance = 0.4, int windowSize = 6, double stdThreshold = 0.15)
    {
        if (enable)
        {
            // 可根据需求调整容忍度和窗口大小
            validator = new GravityValidator(baseTolerance, windowSize, stdThreshold);
        }
        else
        {
            validator = null;
        }
    }

    public (double roll, double pitch, bool useAccel) Update(
        long timestampMs,
        double ax, double ay, double az,          // m/s²
        double gx, double gy, double gz)          // deg/s
    {
        double dt = (timestampMs - lastTimestamp) / 1000.0; // 转为秒
        if (lastTimestamp == 0 || dt <= 0)
        {
            dt = 0.1; // 默认 100ms
        }

        // 1. 陀螺仪积分（单位转换：deg/s → rad/s → 弧度增量）
        double gyroRollRate = gx * DegToRad;
        double gyroPitchRate = gy * DegToRad;

        double rollGyro = roll + gyroRollRate * dt;
        double pitchGyro = pitch + gyroPitchRate * dt;

        // 2. 判断加速度是否可信
        bool useAccel = validator?.IsGravityConsistent(ax, ay, az, gx, gy, gz) ?? true;

        // 3. 计算 Roll/Pitch
        double finalRoll, finalPitch;
        if (useAccel) // 加速度可信（设备近似静止或低动态）
        {
            // 加速度计计算静态 Roll/Pitch
            double accelRoll = Math.Atan2(ay, Math.Sqrt(ax * ax + az * az)) * RadToDeg;
            double accelPitch = Math.Atan2(-ax, Math.Sqrt(ay * ay + az * az)) * RadToDeg;

            // 互补滤波融合
            finalRoll = Alpha * rollGyro + (1 - Alpha) * accelRoll;
            finalPitch = Alpha * pitchGyro + (1 - Alpha) * accelPitch;
        }
        else // 加速度不可信（设备运动中）
        {
            // 仅用陀螺仪（不融合）
            finalRoll = rollGyro;
            finalPitch = pitchGyro;
        }

        // 可选：归一化到 [-180, 180]
        roll = NormalizeAngle(finalRoll);
        pitch = NormalizeAngle(finalPitch);

        lastTimestamp = timestampMs;
        return (roll, pitch, useAccel);
    }

    private double NormalizeAngle(double angle)
    {
        while (angle > 180) angle -= 360;
        while (angle < -180) angle += 360;
        return angle;
    }

    public void Reset()
    {
        roll = 0;
        pitch = 0;
        lastTimestamp = 0;
        validator?.Reset();
    }
}