Bộ lọc Kalman – giải pháp chống nhiễu tuyệt vời cho mọi dự án sử dụng cảm biến

Trong đo lường, do các yếu tố cả chủ quan lẫn khách quan mà kết quả đo đạc luôn chỉ được coi là tương đối so với giá trị thực cần đo. Độ chênh lệch giữa giá trị đo được với giá trị thực được gọi là Sai số.

Sai số gây ra bởi nhiễu, chúng được phân làm 2 loại chính: sai số hệ thống hoặc sai số ngẫu nhiên.

Đọc thêm tại:  https://vi.wikipedia.org/wiki/Sai_s%E1%BB%91

Gỉa sử ta tiến hành đo giá trị điện áp của một cục pin năng lượng mặt trời, kết quả thu được trên đồ thị như sau :

\

Gỉa sử giá trị điện áp thực của pin mặt trời là u₀=1.9545 vol.

Do ảnh hưởng bởi nhiều yếu tố trong quá trình đo đạc, sai số xuất hiện là điều rất khó tránh khỏi, kết quả là ta chỉ có thể đo giá trị điện áp bằng cách lấy tương đối kết quả trung bình các phép đo .

Tức u₀=u ± ∆ e =1.95 ± 1%,  (với ∆ e là sai số)

Đây là thành quả nghiên cứu có từ 56 năm trước, chủ nhân của thuật toán là ông Rudolf (Rudy) E. Kálmán  

Bộ lọc này được sử dụng rộng rãi trong các máy tính kỹ thuật số của các hệ thống điều khiển, Hệ thống định vị, Hệ thống điện tử, RADA, vệ tinh dẫn đường để lọc nhiễu.

Bộ lọc hoạt động ổn định đến mức, nó đã được sử dụng trong chương trình Apollo, tàu con thoi của NASA, tàu ngầm Hải quân và xe tự hành không gian không người lái và vũ khí, tên lửa hành trình.

Với thành công đó, ông cũng đã nhận được rất nhiều giải thưởng lớn khác nhau.

https://vi.wikipedia.org/wiki/Rudolf_E._K%C3%A1lm%C3%A1n

Của tác giả denyssene :  https://github.com/denyssene/SimpleKalmanFilter

Link dự phòng : https://drive.google.com/file/d/0BzMEcyRK_uUFZy1sSjE4c0E3aUk/view?usp=sharing (mirror)

Test thư viện với la bàn số

#include <Wire.h>
#include <HMC5883L.h>

#include <SimpleKalmanFilter.h>
SimpleKalmanFilter pressureKalmanFilter(1, 1, 0.001);
HMC5883L compass;

float x_kalman;
void checkSettings()
{
}
void setup()
{
    Serial.begin(9600);

    // Initialize HMC5883L
    Serial.println("Initialize HMC5883L");
    while (!compass.begin()) {
        Serial.println("Could not find a valid HMC5883L sensor, check wiring!");
        delay(500);
    }

    compass.setRange(HMC5883L_RANGE_1_3GA);
    compass.setMeasurementMode(HMC5883L_CONTINOUS);
    compass.setDataRate(HMC5883L_DATARATE_15HZ);
    compass.setSamples(HMC5883L_SAMPLES_1);

    // Check settings
    checkSettings();
}

void loop()
{
    Vector raw = compass.readRaw();
    Vector norm = compass.readNormalize();
    x_kalman = pressureKalmanFilter.updateEstimate(raw.XAxis);
    Serial.print(raw.XAxis);
    Serial.print(",");

    Serial.print(x_kalman, 3);
    Serial.println();
    delay(100);
}

Test thực tế với cảm biến áp suất

#include <SimpleKalmanFilter.h>
#include <SFE_BMP180.h>

/*
 This sample code demonstrates how the SimpleKalmanFilter object can be used with a
 pressure sensor to smooth a set of altitude measurements. 
 This example needs a BMP180 barometric sensor as a data source.
 https://www.sparkfun.com/products/11824

 SimpleKalmanFilter(e_mea, e_est, q);
 e_mea: Measurement Uncertainty 
 e_est: Estimation Uncertainty 
 q: Process Noise
 */
SimpleKalmanFilter pressureKalmanFilter(1, 1, 0.01);

SFE_BMP180 pressure;

// Serial output refresh time
const long SERIAL_REFRESH_TIME = 100;
long refresh_time;

float baseline; // baseline pressure

double getPressure()
{
    char status;
    double T, P;
    status = pressure.startTemperature();
    if (status != 0) {
        delay(status);
        status = pressure.getTemperature(T);
        if (status != 0) {
            status = pressure.startPressure(3);
            if (status != 0) {
                delay(status);
                status = pressure.getPressure(P, T);
                if (status != 0) {
                    return (P);
                }
            }
        }
    }
}

void setup()
{

    Serial.begin(115200);

    // BMP180 Pressure sensor start
    if (!pressure.begin()) {
        Serial.println("BMP180 Pressure Sensor Error!");
        while (1)
            ; // Pause forever.
    }
    baseline = getPressure();
}

void loop()
{

    float p = getPressure();
    float altitude = pressure.altitude(p, baseline);
    float estimated_altitude = pressureKalmanFilter.updateEstimate(altitude);

    if (millis() > refresh_time) {
        Serial.print(altitude, 6);
        Serial.print(",");
        Serial.print(estimated_altitude, 6);
        Serial.println();
        refresh_time = millis() + SERIAL_REFRESH_TIME;
    }
}

Test thư viện với cảm biến gia tốc

#include <SimpleKalmanFilter.h>
#include <Wire.h>
#include "I2C.h"
#define RESTRICT_PITCH
SimpleKalmanFilter simpleKalmanFilter(1, 1, 0.001);

uint8_t i2cData[14]; // Buffer for I2C data

int16_t accX;
float accX_kalman;

void setup()
{
    Serial.begin(9600);
    Wire.begin();
#if ARDUINO >= 157
    Wire.setClock(400000UL); // Set I2C frequency to 400kHz
#else
    TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
#endif

    i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
    i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
    i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
    i2cData[3] = 0x03; // Set Accelerometer Full Scale Range to ±16g
    while (i2cWrite(0x19, i2cData, 4, false))
        ; // Write to all four registers at once
    while (i2cWrite2(0x6B, 0x01, true))
        ; // PLL with X axis gyroscope reference and disable sleep mode
}
void loop()
{

    while (i2cRead(0x3B, i2cData, 14)) {
        ;
    }
    accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
    accX_kalman = simpleKalmanFilter.updateEstimate((float)accX);
    Serial.print(accX);
    Serial.print(",");
    Serial.print(accX_kalman, 3);
    Serial.println();
}