环形滤波算法

#include <stdio.h>
#include <stdlib.h>

#define BUFFER_SIZE 10  // 缓冲区大小
#define THRESHOLD 180   // 跳变阈值，根据实际情况调整

// 环形缓冲区结构体
typedef struct {
    float values[BUFFER_SIZE];  // 存储角度值的数组
    int index;                  // 当前写入位置
    int size;                   // 缓冲区大小
} CircularBuffer;

// 初始化环形缓冲区
void CircularBuffer_Init(CircularBuffer *buffer) {
    for (int i = 0; i < BUFFER_SIZE; ++i) {
        buffer->values[i] = 0.0;  // 初始化为0
    }
    buffer->index = 0;
    buffer->size = BUFFER_SIZE;
}

// 添加新的角度值到环形缓冲区
void CircularBuffer_AddValue(CircularBuffer *buffer, float value) {
    buffer->values[buffer->index] = value;
    buffer->index = (buffer->index + 1) % buffer->size;  // 循环写入
}

// 计算环形平均值
float CircularBuffer_CalculateCircularMean(CircularBuffer *buffer) {
    float sum_sin = 0.0;
    float sum_cos = 0.0;

    // 使用正弦和余弦的和来计算平均值
    for (int i = 0; i < buffer->size; ++i) {
        float angle_rad = buffer->values[i] * (3.14159265359 / 180.0);  // 角度转弧度
        sum_sin += sin(angle_rad);
        sum_cos += cos(angle_rad);
    }

    // 计算平均值
    float mean_angle_rad = atan2(sum_sin, sum_cos);
    float mean_angle_deg = mean_angle_rad * (180.0 / 3.14159265359);  // 弧度转角度

    // 确保角度在0到360度之间
    if (mean_angle_deg < 0) {
        mean_angle_deg += 360.0;
    }

    return mean_angle_deg;
}

int main() {
    CircularBuffer buffer;
    CircularBuffer_Init(&buffer);

    // 模拟接收到的角度数据
    float angles[] = {10.0, 350.0, 5.0, 358.0, 3.0, 2.0, 1.0, 0.0, 359.0, 358.0};

    // 将角度数据添加到环形缓冲区并计算平均值
    for (int i = 0; i < sizeof(angles) / sizeof(float); ++i) {
        CircularBuffer_AddValue(&buffer, angles[i]);
        float filtered_angle = CircularBuffer_CalculateCircularMean(&buffer);
        printf("Filtered angle: %.2f\n", filtered_angle);
    }

    return 0;
}