Median Filter for Noise

2024-05-23 15:57 UTC gpt-4o Open in ChatGPT ↗

You

Instead of this simple filter, I want a median filter

// Apply noise filtering
if (abs(a - prev_a) > NOISE_THRESHOLD) {
    a = prev_a;  // Ignore the noisy value
} else {
    prev_a = a;  // Update previous value
}

if (abs(b - prev_b) > NOISE_THRESHOLD) {
    b = prev_b;  // Ignore the noisy value
} else {
    prev_b = b;  // Update previous value
}

ChatGPT

To replace the simple filter with a median filter, you’ll need to maintain a window of the last few values for a and b and calculate the median of those values. Here’s how you can modify your code to include a median filter:

1. Maintain a window (list) of previous values.
2. Calculate the median of the window.
3. Update the window with the new value.

Here’s a modified version of your code:

#include <deque>
#include <algorithm>
#include <iterator>

const int WINDOW_SIZE = 5;  // Adjust the window size as needed

std::deque<int> window_a;
std::deque<int> window_b;

int calculateMedian(std::deque<int>& window) {
    std::deque<int> temp = window;
    std::nth_element(temp.begin(), temp.begin() + temp.size() / 2, temp.end());
    return temp[temp.size() / 2];
}

void addValueToWindow(std::deque<int>& window, int value) {
    if (window.size() >= WINDOW_SIZE) {
        window.pop_front();
    }
    window.push_back(value);
}

// In your main code
void applyMedianFilter(int& a, int& b) {
    addValueToWindow(window_a, a);
    addValueToWindow(window_b, b);

    int median_a = calculateMedian(window_a);
    int median_b = calculateMedian(window_b);

    a = median_a;
    b = median_b;
}

// Example usage
int main() {
    int a = 10;  // Example value for 'a'
    int b = 20;  // Example value for 'b'

    applyMedianFilter(a, b);

    // Continue with your code...
    return 0;
}

Explanation

1. Window Size: WINDOW_SIZE determines how many past values you want to keep for calculating the median. Adjust this value based on your requirements.

2. Deque for Window: Using std::deque to maintain a window of the last WINDOW_SIZE values for a and b.

3. Calculate Median: calculateMedian function sorts the deque temporarily and returns the median value.

4. Add Value to Window: addValueToWindow manages the deque, ensuring it does not exceed WINDOW_SIZE.

5. Apply Median Filter: applyMedianFilter is called with the current values of a and b, updates the window, and replaces a and b with their respective medians.

This setup will filter out noise by using the median of the last few values, providing a more robust noise reduction method than the simple threshold-based filter.

You

I am on arduino. I dont have the luxury of fancy std library. Implement it raw, using concise and simple code

ChatGPT

Certainly! Below is a raw implementation of a median filter suitable for an Arduino environment. We’ll use simple arrays to maintain the window of previous values and manually calculate the median.

Here’s the code:

#define WINDOW_SIZE 5  // Adjust the window size as needed

int window_a[WINDOW_SIZE] = {0};
int window_b[WINDOW_SIZE] = {0};
int index_a = 0;
int index_b = 0;
bool filled_a = false;
bool filled_b = false;

void addValueToWindow(int window[], int value, int& index, bool& filled) {
    window[index] = value;
    index++;
    if (index >= WINDOW_SIZE) {
        index = 0;
        filled = true;
    }
}

int calculateMedian(int window[], bool filled) {
    int temp[WINDOW_SIZE];
    int size = filled ? WINDOW_SIZE : index_a; // Use current index as size if not filled yet

    for (int i = 0; i < size; i++) {
        temp[i] = window[i];
    }

    // Sort the array
    for (int i = 0; i < size - 1; i++) {
        for (int j = i + 1; j < size; j++) {
            if (temp[i] > temp[j]) {
                int tmp = temp[i];
                temp[i] = temp[j];
                temp[j] = tmp;
            }
        }
    }

    // Return the median
    if (size % 2 == 0) {
        return (temp[size / 2 - 1] + temp[size / 2]) / 2;
    } else {
        return temp[size / 2];
    }
}

void applyMedianFilter(int& a, int& b) {
    addValueToWindow(window_a, a, index_a, filled_a);
    addValueToWindow(window_b, b, index_b, filled_b);

    int median_a = calculateMedian(window_a, filled_a);
    int median_b = calculateMedian(window_b, filled_b);

    a = median_a;
    b = median_b;
}

void setup() {
    // Initialize serial communication for debugging
    Serial.begin(9600);
}

void loop() {
    int a = analogRead(A0);  // Example reading for 'a'
    int b = analogRead(A1);  // Example reading for 'b'

    applyMedianFilter(a, b);

    // Use filtered values 'a' and 'b'
    Serial.print("Filtered a: ");
    Serial.println(a);
    Serial.print("Filtered b: ");
    Serial.println(b);

    delay(100);  // Delay for readability in the serial monitor
}

Explanation

1. Window Size: WINDOW_SIZE determines how many past values you keep for calculating the median.

2. Arrays for Window: We use window_a and window_b to store the past values for a and b. We also have index_a and index_b to track the current index, and filled_a and filled_b to indicate if the window has been filled at least once.

3. Add Value to Window: addValueToWindow function adds a value to the window, updates the index, and sets the filled flag if the window is full.

4. Calculate Median: calculateMedian function creates a copy of the window, sorts it using a simple bubble sort, and returns the median value. If the window isn’t fully filled yet, it uses the current number of values.

5. Apply Median Filter: applyMedianFilter function is called with the current values of a and b, updates the window, calculates the median, and replaces a and b with their respective medians.

6. Setup and Loop: The setup and loop functions are typical for an Arduino sketch, initializing serial communication and continuously reading values, filtering them, and printing the results. Adjust analogRead inputs as necessary for your specific use case.

This implementation is designed to be simple and effective for an Arduino environment without relying on advanced C++ features or libraries.

You

create a simple test i can implement on an arduino simulator. No hardware inputs

ChatGPT

Sure! To test the median filter on an Arduino simulator without hardware inputs, you can simulate input values in the loop function and observe the filtered outputs through the serial monitor. Here’s a simple test:

#define WINDOW_SIZE 5  // Adjust the window size as needed

int window_a[WINDOW_SIZE] = {0};
int window_b[WINDOW_SIZE] = {0};
int index_a = 0;
int index_b = 0;
bool filled_a = false;
bool filled_b = false;

void addValueToWindow(int window[], int value, int& index, bool& filled) {
    window[index] = value;
    index++;
    if (index >= WINDOW_SIZE) {
        index = 0;
        filled = true;
    }
}

int calculateMedian(int window[], bool filled) {
    int temp[WINDOW_SIZE];
    int size = filled ? WINDOW_SIZE : index_a; // Use current index as size if not filled yet

    for (int i = 0; i < size; i++) {
        temp[i] = window[i];
    }

    // Sort the array
    for (int i = 0; i < size - 1; i++) {
        for (int j = i + 1; j < size; j++) {
            if (temp[i] > temp[j]) {
                int tmp = temp[i];
                temp[i] = temp[j];
                temp[j] = tmp;
            }
        }
    }

    // Return the median
    if (size % 2 == 0) {
        return (temp[size / 2 - 1] + temp[size / 2]) / 2;
    } else {
        return temp[size / 2];
    }
}

void applyMedianFilter(int& a, int& b) {
    addValueToWindow(window_a, a, index_a, filled_a);
    addValueToWindow(window_b, b, index_b, filled_b);

    int median_a = calculateMedian(window_a, filled_a);
    int median_b = calculateMedian(window_b, filled_b);

    a = median_a;
    b = median_b;
}

void setup() {
    // Initialize serial communication for debugging
    Serial.begin(9600);
}

void loop() {
    // Simulated input values
    static int test_values_a[] = {10, 20, 15, 40, 25, 5, 30};
    static int test_values_b[] = {100, 90, 110, 80, 95, 105, 85};
    static int index = 0;

    // Get the current test values
    int a = test_values_a[index];
    int b = test_values_b[index];

    // Apply the median filter
    applyMedianFilter(a, b);

    // Print the original and filtered values
    Serial.print("Original a: ");
    Serial.print(test_values_a[index]);
    Serial.print(" -> Filtered a: ");
    Serial.println(a);

    Serial.print("Original b: ");
    Serial.print(test_values_b[index]);
    Serial.print(" -> Filtered b: ");
    Serial.println(b);

    // Move to the next set of test values
    index++;
    if (index >= sizeof(test_values_a) / sizeof(test_values_a[0])) {
        index = 0;  // Restart the test values
    }

    delay(1000);  // Delay for readability in the serial monitor
}

Explanation

1. Test Values: Two arrays test_values_a and test_values_b hold simulated input values for a and b.

2. Index: A static variable index is used to iterate through the test values.

3. Loop Function: In the loop function, the current test values are retrieved, the median filter is applied, and the original and filtered values are printed to the serial monitor.

4. Serial Monitor: Use the serial monitor to observe the outputs. This will help you verify if the median filter is working correctly.

By running this code on an Arduino simulator, you can observe how the median filter processes the simulated input values and verify its functionality without requiring any hardware inputs.

You

why index_A

int size = filled ? WINDOW_SIZE : index_a; // Use current index as size if not filled yet

ChatGPT

You are correct; using index_a in the calculation of the median size is incorrect. Instead, we should use a parameter that represents the current index for the respective window (index in the context of the function). Here’s the corrected implementation:

#define WINDOW_SIZE 5  // Adjust the window size as needed

int window_a[WINDOW_SIZE] = {0};
int window_b[WINDOW_SIZE] = {0};
int index_a = 0;
int index_b = 0;
bool filled_a = false;
bool filled_b = false;

void addValueToWindow(int window[], int value, int& index, bool& filled) {
    window[index] = value;
    index++;
    if (index >= WINDOW_SIZE) {
        index = 0;
        filled = true;
    }
}

int calculateMedian(int window[], int index, bool filled) {
    int temp[WINDOW_SIZE];
    int size = filled ? WINDOW_SIZE : index; // Use current index as size if not filled yet

    for (int i = 0; i < size; i++) {
        temp[i] = window[i];
    }

    // Sort the array
    for (int i = 0; i < size - 1; i++) {
        for (int j = i + 1; j < size; j++) {
            if (temp[i] > temp[j]) {
                int tmp = temp[i];
                temp[i] = temp[j];
                temp[j] = tmp;
            }
        }
    }

    // Return the median
    if (size % 2 == 0) {
        return (temp[size / 2 - 1] + temp[size / 2]) / 2;
    } else {
        return temp[size / 2];
    }
}

void applyMedianFilter(int& a, int& b) {
    addValueToWindow(window_a, a, index_a, filled_a);
    addValueToWindow(window_b, b, index_b, filled_b);

    int median_a = calculateMedian(window_a, index_a, filled_a);
    int median_b = calculateMedian(window_b, index_b, filled_b);

    a = median_a;
    b = median_b;
}

void setup() {
    // Initialize serial communication for debugging
    Serial.begin(9600);
}

void loop() {
    // Simulated input values
    static int test_values_a[] = {10, 20, 15, 40, 25, 5, 30};
    static int test_values_b[] = {100, 90, 110, 80, 95, 105, 85};
    static int index = 0;

    // Get the current test values
    int a = test_values_a[index];
    int b = test_values_b[index];

    // Apply the median filter
    applyMedianFilter(a, b);

    // Print the original and filtered values
    Serial.print("Original a: ");
    Serial.print(test_values_a[index]);
    Serial.print(" -> Filtered a: ");
    Serial.println(a);

    Serial.print("Original b: ");
    Serial.print(test_values_b[index]);
    Serial.print(" -> Filtered b: ");
    Serial.println(b);

    // Move to the next set of test values
    index++;
    if (index >= sizeof(test_values_a) / sizeof(test_values_a[0])) {
        index = 0;  // Restart the test values
    }

    delay(1000);  // Delay for readability in the serial monitor
}

Explanation

• calculateMedian Function: The parameter index is used to determine the current size of the window if it is not yet filled. This makes the function more generic and reusable for different windows.
• Function Parameters: calculateMedian now takes index as a parameter to correctly determine the size of the array segment to be considered for median calculation.

With this change, the median filter should work correctly, even when the window is not yet completely filled.

You

// The first 4 bytes correspond to the first motor int32_t a = reconstructInt(data[0], data[1], data[2], data[3]); // The last 4 bytes correspond to the second motor int32_t b = reconstructInt(data[4], data[5], data[6], data[7]);

applyMedianFilter(a,b);

i get an error

Compilation error: cannot bind non-const lvalue reference of type ‘int&’ to an rvalue of type ‘int’

ChatGPT

The error you’re encountering is due to the fact that a and b are of type int32_t, but the applyMedianFilter function expects parameters of type int&. You can either change the applyMedianFilter function to accept int32_t& parameters or cast a and b to int before passing them to the function.

Here’s the updated code with applyMedianFilter accepting int32_t&:

#define WINDOW_SIZE 5  // Adjust the window size as needed

int32_t window_a[WINDOW_SIZE] = {0};
int32_t window_b[WINDOW_SIZE] = {0};
int index_a = 0;
int index_b = 0;
bool filled_a = false;
bool filled_b = false;

void addValueToWindow(int32_t window[], int32_t value, int& index, bool& filled) {
    window[index] = value;
    index++;
    if (index >= WINDOW_SIZE) {
        index = 0;
        filled = true;
    }
}

int32_t calculateMedian(int32_t window[], int index, bool filled) {
    int32_t temp[WINDOW_SIZE];
    int size = filled ? WINDOW_SIZE : index; // Use current index as size if not filled yet

    for (int i = 0; i < size; i++) {
        temp[i] = window[i];
    }

    // Sort the array
    for (int i = 0; i < size - 1; i++) {
        for (int j = i + 1; j < size; j++) {
            if (temp[i] > temp[j]) {
                int32_t tmp = temp[i];
                temp[i] = temp[j];
                temp[j] = tmp;
            }
        }
    }

    // Return the median
    if (size % 2 == 0) {
        return (temp[size / 2 - 1] + temp[size / 2]) / 2;
    } else {
        return temp[size / 2];
    }
}

void applyMedianFilter(int32_t& a, int32_t& b) {
    addValueToWindow(window_a, a, index_a, filled_a);
    addValueToWindow(window_b, b, index_b, filled_b);

    int32_t median_a = calculateMedian(window_a, index_a, filled_a);
    int32_t median_b = calculateMedian(window_b, index_b

You

that’s not just the problem. i still get the error

ChatGPT

I see. The issue stems from trying to pass the values of a and b as references to the applyMedianFilter function when they are not modifiable lvalues. Since they are results of expressions, you need to store them in variables first. Let’s fix this by ensuring that a and b are stored in variables before being passed to the applyMedianFilter function.

Here’s the revised version:

#define WINDOW_SIZE 5  // Adjust the window size as needed

int32_t window_a[WINDOW_SIZE] = {0};
int32_t window_b[WINDOW_SIZE] = {0};
int index_a = 0;
int index_b = 0;
bool filled_a = false;
bool filled_b = false;

void addValueToWindow(int32_t window[], int32_t value, int& index, bool& filled) {
    window[index] = value;
    index++;
    if (index >= WINDOW_SIZE) {
        index = 0;
        filled = true;
    }
}

int32_t calculateMedian(int32_t window[], int index, bool filled) {
    int32_t temp[WINDOW_SIZE];
    int size = filled ? WINDOW_SIZE : index; // Use current index as size if not filled yet

    for (int i = 0; i < size; i++) {
        temp[i] = window[i];
    }

    // Sort the array
    for (int i = 0; i < size - 1; i++) {
        for (int j = i + 1; j < size; j++) {
            if (temp[i] > temp[j]) {
                int32_t tmp = temp[i];
                temp[i] = temp[j];
                temp[j] = tmp;
            }
        }
    }

    // Return the median
    if (size % 2 == 0) {
        return (temp[size / 2 - 1] + temp[size / 2]) / 2;
    } else {
        return temp[size / 2];
    }
}

void applyMedianFilter(int32_t& a, int32_t& b) {
    addValueToWindow(window_a, a, index_a, filled_a);
    addValueToWindow(window_b, b, index_b, filled_b);

    int32_t median_a = calculateMedian(window_a, index_a, filled_a);
    int32_t median_b = calculateMedian(window_b, index_b, filled_b);

    a = median_a;
    b = median_b;
}

int32_t reconstructInt(uint8_t byte1, uint8_t byte2, uint8_t byte3, uint8_t byte4) {
    return (int32_t(byte1) << 24) | (int32_t(byte2) << 16) | (int32_t(byte3) << 8) | int32_t(byte4);
}

void setup() {
    // Initialize serial communication for debugging
    Serial.begin(9600);
}

void loop() {
    // Simulated input data
    uint8_t data[] = {0x00, 0x00, 0x00, 0x0A, 0x00, 0x00, 0x00, 0x14};  // Example data

    // The first 4 bytes correspond to the first motor
    int32_t a = reconstructInt(data[0], data[1], data[2], data[3]);
    // The last 4 bytes correspond to the second motor
    int32_t b = reconstructInt(data[4], data[5], data[6], data[7]);

    applyMedianFilter(a, b);

    // Print the filtered values
    Serial.print("Filtered a: ");
    Serial.println(a);
    Serial.print("Filtered b: ");
    Serial.println(b);

    delay(1000);  // Delay for readability in the serial monitor
}

Explanation

• Variable Declarations: Ensure that a and b are declared as variables before passing them to applyMedianFilter.
• Function Parameters: The applyMedianFilter function now correctly accepts int32_t& parameters.
• Reconstruction Function: The reconstructInt function converts four bytes into an int32_t value.
• Simulated Data: Example data is used to simulate the input, which you can modify as needed.
• Loop Function: The loop function reads simulated data, applies the median filter, and prints the results.

This should resolve the compilation error and allow you to test the median filter functionality in an Arduino simulator.

You

Isn’t there a problem with this filter? When a new value arrives, we replace it with the median of itself and the 4 previous values. So the “window” is never centered

ChatGPT

You’re right. The current implementation of the median filter doesn’t maintain a properly centered window, which means the filter doesn’t provide the most accurate median for the given data set. To fix this, we’ll need to ensure that our window of values is properly managed.

Here’s an improved version of the median filter that correctly maintains and centers the window:

#define WINDOW_SIZE 5  // Adjust the window size as needed

int32_t window_a[WINDOW_SIZE] = {0};
int32_t window_b[WINDOW_SIZE] = {0};
int count_a = 0;
int count_b = 0;

void addValueToWindow(int32_t window[], int32_t value, int& count) {
    if (count < WINDOW_SIZE) {
        window[count] = value;
        count++;
    } else {
        for (int i = 1; i < WINDOW_SIZE; i++) {
            window[i - 1] = window[i];
        }
        window[WINDOW_SIZE - 1] = value;
    }
}

int32_t calculateMedian(int32_t window[], int count) {
    int32_t temp[WINDOW_SIZE];
    int size = (count < WINDOW_SIZE) ? count : WINDOW_SIZE;

    for (int i = 0; i < size; i++) {
        temp[i] = window[i];
    }

    // Sort the array
    for (int i = 0; i < size - 1; i++) {
        for (int j = i + 1; j < size; j++) {
            if (temp[i] > temp[j]) {
                int32_t tmp = temp[i];
                temp[i] = temp[j];
                temp[j] = tmp;
            }
        }
    }

    // Return the median
    if (size % 2 == 0) {
        return (temp[size / 2 - 1] + temp[size / 2]) / 2;
    } else {
        return temp[size / 2];
    }
}

void applyMedianFilter(int32_t& a, int32_t& b) {
    addValueToWindow(window_a, a, count_a);
    addValueToWindow(window_b, b, count_b);

    int32_t median_a = calculateMedian(window_a, count_a);
    int32_t median_b = calculateMedian(window_b, count_b);

    a = median_a;
    b = median_b;
}

int32_t reconstructInt(uint8_t byte1, uint8_t byte2, uint8_t byte3, uint8_t byte4) {
    return (int32_t(byte1) << 24) | (int32_t(byte2) << 16) | (int32_t(byte3) << 8) | int32_t(byte4);
}

void setup() {
    // Initialize serial communication for debugging
    Serial.begin(9600);
}

void loop() {
    // Simulated input data
    uint8_t data[] = {0x00, 0x00, 0x00, 0x0A, 0x00, 0x00, 0x00, 0x14};  // Example data

    // The first 4 bytes correspond to the first motor
    int32_t a = reconstructInt(data[0], data[1], data[2], data[3]);
    // The last 4 bytes correspond to the second motor
    int32_t b = reconstructInt(data[4], data[5], data[6], data[7]);

    applyMedianFilter(a, b);

    // Print the filtered values
    Serial.print("Filtered a: ");
    Serial.println(a);
    Serial.print("Filtered b: ");
    Serial.println(b);

    delay(1000);  // Delay for readability in the serial monitor
}

Explanation

1. Window Management: The addValueToWindow function now properly manages the window by shifting values to the left when the window is full and adding the new value at the end. This ensures that the window is centered around the most recent values.

2. Median Calculation: The calculateMedian function calculates the median of the current window. It creates a temporary array, sorts it, and returns the median value. The size of the window is dynamically determined by the count of added values.

3. Counting Values: The count_a and count_b variables keep track of how many values have been added to each window. This allows the filter to properly handle the initial state when the window is not yet full.

This implementation ensures that the window of values is properly centered and that the median is calculated correctly, providing a more accurate filter.