c语言速度平滑滤波函数

1.非实时滤波

这个函数接受三个参数：速度数组、数组大小和窗口大小。函数中首先创建一个临时数组来存储滤波后的速度。对于每个位置，计算它前后窗口大小范围内的速度值之和，并求平均值，存储到临时数组中。最后将临时数组中的值复制回原速度数组中，完成滤波操作。注意，这里的速度数组是一个double类型的数组。

void smooth_speed(double *speed, int size, int window_size) {
    double* temp = (double*)malloc(size * sizeof(double)); // 用于存储滤波后的速度
    int half_window_size = window_size / 2;
    for (int i = 0; i < size; i++) {
        double sum = 0;
        int count = 0;
        for (int j = -half_window_size; j <= half_window_size; j++) {
            int idx = i + j;
            if (idx >= 0 && idx < size) {
                sum += speed[idx];
                count++;
            }
        }
        temp[i] = sum / count;
    }
    // 将滤波后的速度复制回原速度
    for (int i = 0; i < size; i++) {
        speed[i] = temp[i];
    }
    free(temp);
}

2.实时滤波

下面是一个实时平滑滤波函数，它接受两个参数：当前速度和历史速度数组。函数中通过给定的历史速度数组来计算平均速度，然后将当前速度加入数组最后一位，把历史速度数组中第一位去掉，再重新计算平均速度，从而实现实时的滤波输出：

double smooth_speed_realtime(double current_speed, double *history_speed, int size) {
    double sum = 0;
    for (int i = 0; i < size; i++) {
        sum += history_speed[i];
    }
    double average_speed = sum / size;
    for (int i = 0; i < size - 1; i++) {
        history_speed[i] = history_speed[i + 1];
    }
    history_speed[size - 1] = current_speed;
    sum = 0;
    for (int i = 0; i < size; i++) {
        sum += history_speed[i];
    }
    return sum / size;
}

这个函数首先计算历史速度数组的平均速度，然后将当前速度加入数组最后一位，把历史速度数组中第一位去掉，再重新计算平均速度，最后返回平均速度作为滤波输出。