칼만 필터 (Kalman Filter) 적용하기

아두이노에 센서를 부착해서 동작시킬때 값이 자음이 포함되어 흔들리는 경우를 흔히 볼 수 있습니다. 이를 최대한 잡아내는 방법으로 여러가지 필터들이 있는데요. 오늘 가속도 센서에 칼만필터를 적용하는 방법에 대해서 알아보도록 하겠습니다.

칼만 필터

• Disturbance를 가지는 관측치를 바탕으로 선형 역학계의 상태를 추정하는 재귀 필터로, 시스템의 상태를 추정하여 제어하기 위한 필터
• 칼만 필터는 과거에 수행한 측정값을 바탕으로 현재의 상태 변수의 결합분포를 추정한다.
• 알고리즘은 예측과 업데이트의 두 단계로 이루어진다.
  • 예측 단계에서는 현재 상태 변수의 값과 정확도를 예측한다. 현재 상태 변수의 값이 실제로 측정된 이후, 업데이트 단계에서는 이전에 추정한 상태 변수를 기반으로 예측한 측정치와 실제 측정치의 차이를 반영하여 현재의 상태 변수를 업데이트 한다.
• 확장 칼만 필터는 비선형 시스템을 기반으로 동작한다.

C++ & Header file source code

// gyroKalmanFilter.cpp
// C++ Source Code

// Arudino Gyroscope + Kalman Filter (MPU6050)

#include <math.h>
#include <Wire.h>
#include <Arduino.h>
#include "gyroKalmanFilter.h"

static const float R_angle = 0.3;
static const float Q_angle = 0.01;
static const float Q_gyro = 0.04;

// Value limitation of accelerometer (may vary)
const int lowX = -2150;
const int highX = 2210;
const int lowY = -2150;
const int highY = 2210;
const int lowZ = -2150;
const int highZ = 2550;

// Time
unsigned long prevSensoredTime = 0;
unsigned long curSensoredTime = 0;

int xInit[5] = { 0, 0, 0, 0, 0 };
int yInit[5] = { 0, 0, 0, 0, 0 };
int zInit[5] = { 0, 0, 0, 0, 0 };
int initIndex = 0;
int initSize = 5;
int xCal = 0, yCal = 0, zCal = 1800;

// For return
int _x = 0, _y = 0, _z = 0;

struct GyroKalman angX;
struct GyroKalman angY;
struct GyroKalman angZ;

void gyroInit() {
    Serial.begin(SERIAL_SPEED);
    Wire.begin();
    
    int error;
    uint8_t c;

    /************************************
     * Default at power up:
     * Gyro at 250 degrees second
     * Acceleration at 2g
     * Clock source at internal 8MHz
     ***********************************/

    initGyroKalman(&angX, Q_angle, Q_gyro, R_angle);
	initGyroKalman(&angY, Q_angle, Q_gyro, R_angle);
	initGyroKalman(&angZ, Q_angle, Q_gyro, R_angle);

    error = MPU6050_read(MPU6050_WHO_AM_I, &c, 1);

    // Uses Serial.print(F()) F() function to use flash memory, not SRAM memory
    Serial.print(F("WHO_AM_I: "));
    Serial.print(c, HEX);
    Serial.print(F(", error = "));
    Serial.println(error, DEC);

    // Clear Sleep Bit to start the MPU6050 sensor
    MPU6050_write_reg(MPU6050_PWR_MGMT_1, 0);
}

void gyroExecute() {
    int error;
    double dT;
    uint8_t tmp;
    accel_t_gyro_union accel_t_gyro;

    curSensoredTime = millis();

    // Read raw values (14 bit) of acceleration/gyro/temperature (Not Stable)
    error = MPU6050_read(MPU6050_ACCEL_XOUT_H, (uint8_t*)&accel_t_gyro, sizeof(accel_t_gyro));
    if(error != 0) {
        Serial.print(F("Read acceleration, temperature, gyro... error = "));
        Serial.println(error, DEC);
    }

    #define SWAP(x, y) tmp = x; x = y; y = tmp;
    SWAP(accel_t_gyro.reg.x_accel_h, accel_t_gyro.reg.x_accel_l);
    SWAP(accel_t_gyro.reg.y_accel_h, accel_t_gyro.reg.y_accel_l);
    SWAP(accel_t_gyro.reg.z_accel_h, accel_t_gyro.reg.z_accel_l);
    SWAP(accel_t_gyro.reg.t_h, accel_t_gyro.reg.t_l);
    SWAP(accel_t_gyro.reg.x_gyro_h, accel_t_gyro.reg.x_gyro_l);
    SWAP(accel_t_gyro.reg.y_gyro_h, accel_t_gyro.reg.y_gyro_l);
    SWAP(accel_t_gyro.reg.z_gyro_h, accel_t_gyro.reg.z_gyro_l);

    if(prevSensoredTime > 0) {
        int gx1 = 0, gy1 = 0, gz1 = 0;
        float gx2 = 0, gy2 = 0, gz2 = 0;

        int loopTime = curSensoredTime - prevSensoredTime;

        gx2 = angleInDegrees(lowX, highX, accel_t_gyro.value.x_gyro);
        gy2 = angleInDegrees(lowY, highY, accel_t_gyro.value.y_gyro);
        gz2 = angleInDegrees(lowZ, highZ, accel_t_gyro.value.z_gyro);

        predict(&angX, gx2, loopTime);
        predict(&angY, gy2, loopTime);
        predict(&angZ, gz2, loopTime);

        gx1 = update(&angX, accel_t_gyro.value.x_accel) / 10;
        gy1 = update(&angY, accel_t_gyro.value.y_accel) / 10;
        gz1 = update(&angZ, accel_t_gyro.value.z_accel) / 10;

        if(initIndex < initSize) {
            xInit[initIndex] = gx1;
            yInit[initIndex] = gy1;
            zInit[initIndex] = gz1;

            if(initIndex == initSize - 1) {
                int sumX = 0, sumY = 0, sumZ = 0;
                for(int k = 1; k <= initSize; k++) {
                    sumX += xInit[k];
                    sumY += yInit[k];
                    sumZ += zInit[k];
                }
                xCal -= sumX / (initSize - 1);
                yCal -= sumY / (initSize - 1);
                zCal = (sumZ / (initSize - 1) - zCal);
            }
            initIndex++;
        } else {
            gx1 += xCal;
            gy1 += yCal;
        }
        Serial.print(F("Angle x, y, z: "));
        Serial.print(gx1, DEC);
        _x = gx1;
        Serial.print(F(", "));
        Serial.print(gy1, DEC);
        _y = gy1;
        Serial.print(F(", "));
        Serial.print(gz1, DEC);
        _z = gz1;
        Serial.println(F(""));
    }
    prevSensoredTime = curSensoredTime;
    newDelay(200);
}

int getXcoord() {
    return _x;
}

int getYcoord() {
    return _y;
}

int getZcoord() {
    return _z;
}

/***********************
 * Sensor Read / Write
 ***********************/
int MPU6050_read(int start, uint8_t *buffer, int size) {
    int i, n, error;
    Wire.beginTransmission(MPU6050_I2C_ADDRESS);

    n = Wire.write(start);
    if(n != 1) {
        return -10;
    }
    n = Wire.endTransmission(false);
    if(n != 0) {
        return n;
    }
    Wire.requestFrom(MPU6050_I2C_ADDRESS, size, true);
    i = 0;
    while(Wire.available() && i < size) {
        buffer[i++] = Wire.read();
    }
    if(i != size) {
        return -11;
    }
    return 0;
}

int MPU6050_write(int start, const uint8_t *pData, int size) {
    int n, error;
    Wire.beginTransmission(MPU6050_I2C_ADDRESS);

    n = Wire.write(start);
    if(n != 1) {
        return -20;
    }
    n = Wire.write(pData, size);
    if(n != size) {
        return -21;
    }
    error = Wire.endTransmission(true);
    if(error != 0) {
        return error;
    }
    return 0;
}

int MPU6050_write_reg(int reg, uint8_t data) {
    int error;
    error = MPU6050_write(reg, &data, 1);
    return error;
}

/************************
 * Raw data processing
 ************************/
float angleInDegrees(int lo, int hi, int measured) {
    float x = (hi - lo) / 180.0;
    return (float)measured / x;
}

void initGyroKalman(struct GyroKalman *kalman, const float Q_angle, const float Q_gyro, const float R_angle) {
    kalman->Q_angle = Q_angle;
    kalman->Q_gyro = Q_gyro;
    kalman->R_angle = R_angle;

    kalman->P_00 = 0;
    kalman->P_01 = 0;
    kalman->P_10 = 0;
    kalman->P_11 = 0;
}

/********************************************************************************************
 * Kalman Predict Method
 * kalman: the kalman data structure
 * dotAngle: Derivitive Of The (D O T) Angle. This is the change in the angle from the gyro.
 *           This is the value from the Wii Motion Plus, scaled to fast/slow.
 * dt: the change in time, in sec.
 ********************************************************************************************/
void predict(struct GyroKalman *kalman, float dotAngle, float dt) {
    kalman->x_angle += dt * (dotAngle - kalman->x_bias);
    kalman->P_00 += (-1) * dt * (kalman->P_10 + kalman->P_01) + dt * dt * kalman->P_11 + kalman->Q_angle;
    kalman->P_01 += (-1) * dt * kalman->P_11;
    kalman->P_10 += (-1) * dt * kalman->P_11;
    kalman->P_11 += kalman->Q_gyro;
}

float update(struct GyroKalman *kalman, float angle_m) {
    const float y = angle_m - kalman->x_angle;
    const float S = kalman->P_00 + kalman->R_angle;
    const float K_0 = kalman->P_00 / S;
    const float K_1 = kalman->P_10 / S;
    kalman->x_angle += K_0 * y;
    kalman->x_bias += K_1 * y;
    kalman->P_00 -= K_0 * kalman->P_00;
    kalman->P_01 -= K_0 * kalman->P_01;
    kalman->P_10 -= K_1 * kalman->P_00;
    kalman->P_11 -= K_1 * kalman->P_01;

    return kalman->x_angle;
}

/**************
 * Utility
 **************/
void newDelay(int x) {
    // Delay function for x milliseconds which does not interfere with timer0
    for(int i = 0; i < x; i++) delayMicroseconds(1000);
}

// gyroKalmanFilter.h
// Header file

#ifndef _GYROKALMANFILTER_H_
#define _GYROKALMANFILTER_H_

#define SERIAL_SPEED 115200

// MPU-6050 Sensor
#define MPU6050_ACCEL_XOUT_H 0x3B   // R
#define MPU6050_PWR_MGMT_1   0x6B   // R/W
#define MPU6050_PWR_MGMT_2   0x6C   // R/W
#define MPU6050_WHO_AM_I     0x75   // R
#define MPU6050_I2C_ADDRESS  0x68   // I2C Address

// Kalman Filter
struct GyroKalman {
    // Variable to represent state matrix x
    float x_angle, x_bias;

    // Error covariance matrix
    float P_00, P_01, P_10, P_11;

    /******************************************************************
     * Q is 2x2 matriax of covariance
     * Gyro and accelerometer noise is independent of each other
     * x = F(x) + B(u) + w
     * w has a normal distribution with covariance Q
     * covariance = E[(X - E[X]) * (X - E[X])']
     * Covariance R: observation noise from accelerometer (1x1 Matrix)
     ******************************************************************/

    float Q_angle, Q_gyro;
    float R_angle;
};

typedef union accel_t_gyro_union {
    struct {
        uint8_t x_accel_h, x_accel_l;
        uint8_t y_accel_h, y_accel_l;
        uint8_t z_accel_h, z_accel_l;
        uint8_t t_h, t_l;
        uint8_t x_gyro_h, x_gyro_l;
        uint8_t y_gyro_h, y_gyro_l;
        uint8_t z_gyro_h, z_gyro_l;
    } reg;
    struct {
        int x_accel, y_accel, z_accel;
        int temperature;
        int x_gyro, y_gyro, z_gyro;
    } value;
};

// Function Prototype
void gyroInit();
void gyroExecute();
int MPU6050_read(int start, uint8_t *buffer, int size);
int MPU6050_write(int start, const uint8_t *pData, int size);
int MPU6050_write_reg(int reg, uint8_t data);
float angleInDegrees(int lo, int hi, int measured);
void initGyroKalman(struct GyroKalman *kalman, const float Q_angle, const float Q_gyro, const float R_angle);
void predict(struct GyroKalman *kalman, float dotAngle, float dt);
float update(struct GyroKalman *kalman, float angle_m);
int getXcoord();
int getYcoord();
int getZcoord();
void newDelay(int x);


#endif /*_GYROKALMANFILTER_H_*/

아두이노 샘플 코드

아래 샘플코드는 가속도 센서 값을 칼만 필터에 적용 후 값에 따라 8x8 도트 매트릭스에 문자를 표시하는 샘플입니다.

// MPU6050 with Kalman Filter applied

#include "gyroKalmanFilter.h"
#include <LedControl.h>

LedControl lc = LedControl(12, 11, 10, 1);

byte circle[8] = {B00011000,
                  B00100100,
                  B01000010,
                  B10000001,
                  B10000001,
                  B01000010,
                  B00100100,
                  B00011000};
byte leftarrow[8] = {B00011000,
                     B00100000,
                     B01000000,
                     B11111111,
                     B11111111,
                     B01000000,
                     B00100000,
                     B00011000};
byte strongleftarrow[8] = {B00011010,
                           B00100100,
                           B01001000,
                           B11111111,
                           B11111111,
                           B01001000,
                           B00100100,
                           B00011010};
byte rightarrow[8] = {B00011000,
                      B00000100,
                      B00000010,
                      B11111111,
                      B11111111,
                      B00000010,
                      B00000100,
                      B00011000};
byte strongrightarrow[8] = {B01011000,
                            B00100100,
                            B00010010,
                            B11111111,
                            B11111111,
                            B00010010,
                            B00100100,
                            B01011000};
byte uparrow[8] = {B00011000,
                   B00111100,
                   B01011010,
                   B10011001,
                   B00011000,
                   B00011000,
                   B00011000,
                   B00011000};
byte stronguparrow[8] = {B00011000,
                         B00111100,
                         B01011010,
                         B10111101,
                         B01011010,
                         B10011001,
                         B00011000,
                         B00011000};
byte downarrow[8] = {B00011000,
                     B00011000,
                     B00011000,
                     B00011000,
                     B10011001,
                     B01011010,
                     B00111100,
                     B00011000};
byte strongdownarrow[8] = {B00011000,
                           B00011000,
                           B10011001,
                           B01011010,
                           B10111101,
                           B01011010,
                           B00111100,
                           B00011000};
                


void setup() {
    Serial.begin(115200);
    gyroInit();
    lc.shutdown(0, false);
    lc.clearDisplay(0);
    for(int i = 0; i < 8; i++) {
        lc.setRow(0, i, circle[i]);
    }
    delay(3000);
}

void loop() {
    int x, y, z;
    int prevx = 0, prevy = 0, prevz = 0;
    int dx, dy, dz;
    gyroExecute();
    x = getXcoord();
    y = getYcoord();
    z = getZcoord();
    Serial.println(x);
    Serial.println(y);
    Serial.println(z);
    dx = x - prevx;
    dy = y - prevy;
    dz = z - prevz;
    if(dx > 1000) {
        for(int i = 0; i < 8; i++) {
            lc.setRow(0, i, strongleftarrow[i]);
        }
    } else if(dx < -1000) {
        for(int i = 0; i < 8; i++) {
            lc.setRow(0, i, strongrightarrow[i]);
        }
    } else if(dy > 1000) {
        for(int i = 0; i < 8; i++) {
            lc.setRow(0, i, strongdownarrow[i]);
        }
    } else if(dy < -1000) {
        for(int i = 0; i < 8; i++) {
            lc.setRow(0, i, stronguparrow[i]);
        }
    } else if(dx > 500) {
        for(int i = 0; i < 8; i++) {
            lc.setRow(0, i, leftarrow[i]);
        }
    } else if(dx < -500) {
        for(int i = 0; i < 8; i++) {
            lc.setRow(0, i, rightarrow[i]);
        }
    } else if(dy > 500) {
        for(int i = 0; i < 8; i++) {
            lc.setRow(0, i, downarrow[i]);
        }
    } else if(dy < -500) {
        for(int i = 0; i < 8; i++) {
            lc.setRow(0, i, uparrow[i]);
        }
    } else {
        for(int i = 0; i < 8; i++) {
            lc.setRow(0, i, circle[i]);
        }
    }

    prevx = x;
    prevy = y;
    prevz = z;
    newDelay(1000);
}

MPU 6050 기반의 가속도 센서를 사용했으며 칼만필터를 적용시  상당히 정확한 모습을 보여주네요.

MPU 6050 : https://invensense.tdk.com/wp-content/uploads/2015/02/MPU-6000-Datasheet1.pdf

가속도 센서를 사용해서 상당히 다양한 활용처가 있지만, 그 중에서 무선 이어폰의 경우 움직임을 감지해 배터리 최적화를 하는 등 우리 실생활에서 굉장히 많이 사용되고 있는데요, 저렇게 방향에 대한 움직임도 정확하다면 간단한 제스처 동작 역시 무리없이 상품화가 가능할 것으로 생각됩니다. 여러분의 아이디어 구현시 꼭 이용해보시기 바랍니다.!