Chống nhiễu cho cảm biến bằng bộ lọc Kalman

Của tác giả denyssene : SimpleKalmanFilter

Link dự phòng : https://drive.google.com/file/d/0BzM…

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();
}