1. Non-real-time filtering
This function accepts three parameters: speed array, array size and window size. The function first creates a temporary array to store the filtered velocity. For each position, calculate the sum of the speed values within the window size range before and after it, average it, and store it in a temporary array. Finally, the values in the temporary array are copied back to the original speed array to complete the filtering operation. Note that the speed array here is an array of double type.
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. Real-time filtering
Below is a real-time smoothing filter function that accepts two parameters: current speed and historical speed array. The function calculates the average speed through the given historical speed array, then adds the current speed to the last digit of the array, removes the first digit from the historical speed array, and then recalculates the average speed to achieve real-time filtered output:
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;
}
This function first calculates the average speed of the historical speed array, then adds the current speed to the last digit of the array, removes the first digit of the historical speed array, recalculates the average speed, and finally returns the average speed as the filtered output.