11
2016-Feb
칼만필터(Kalman Filter)
작성자: 
Blonix
IP ADRESS: *.148.87.98 조회 수: 4375
칼만필터 소개
칼만필터는 Predict -> Update 의 두 과정으로 분리 가능
한개의 센서값 이용도 가능하고 두 개 이상의 센서값을 사용할 수도 있다.
예를 들어 가속도 + 자이로 센서로 기울기를 측정하려 할 때, Predict 는 자이로값을 이용하고 Update는 가속도값을 이용하는 것.
이전값을 이용한 한 순간의 Predict 에서는 자이로의 드리프트가 없다고 볼 수 있기에 예측에 유용하다.
====================================================================
이론은 복잡해서 제대로 설명하기는 좀 많이 어려우니 아래 참고자료를 참고하기 바란다.
그리고 그냥 간단히 내 코드를 소개한다.
아래 코드중 KalmanPredict2D 함수는 가속도 + 자이로계의 칼만 필터 예측에만 적용되는 함수이다. 시스템이 다르면 달라진다.
struct SKalman1D{
double X; // 상태 행렬
double Q; // 센서 노이즈 공분산 상수
double P; // 상태 공분산 행렬
double R; // 측정 공분산 행렬
};
struct SKalman2D{
double X_0, X_1; // 상태 행렬
double Q_0, Q_1; // 센서 노이즈 공분산 상수
double P_00, P_01, P_10, P_11; // 상태 공분산 행렬
double R; // 측정 공분산 행렬
};
void KalmanPredictUpdate1D(SKalman1D *kalman, double NewData);
virtual void KalmanPredict2D(SKalman2D *kalman, double NewX_1, double Dt); // 시스템마다 달라질 수 있음. 그럴 경우 오버라이딩
void KalmanUpdate2D(SKalman2D*, double NewX_0);
이렇게 함수는 세개가 있고..
그 정의는 다음과 같다.
void CSensorBase::KalmanPredictUpdate1D(SKalman1D* Kalman, double NewData)
{
double K; // Kalman gain
// Predict
// Kalman->X 는 1차원에서 그냥 예전값과 동일할거라 예측됨(?)
Kalman->P = Kalman->P + Kalman->Q;
// Update
K = Kalman->P / (Kalman->P + Kalman->R);
Kalman->X = Kalman->X + K * (NewData - Kalman->X);
Kalman->P = (1 - K) * Kalman->P;
}
void CSensorBase::KalmanPredict2D(SKalman2D* Kalman, double NewX_1, double DeltaTime)
{
// 자이로 + 가속도계 기준
Kalman->X_0 += DeltaTime * (NewX_1 - Kalman->X_1);
Kalman->P_00 += DeltaTime * (DeltaTime*Kalman->P_11 - Kalman->P_01 - Kalman->P_10) + Kalman->Q_0;
Kalman->P_01 -= DeltaTime * Kalman->P_11;
Kalman->P_10 -= DeltaTime * Kalman->P_11;
Kalman->P_11 += Kalman->Q_1; // ?
}
void CSensorBase::KalmanUpdate2D(SKalman2D* Kalman, double NewX_0)
{
double y = NewX_0 - Kalman->X_0;
double S = Kalman->P_00 + Kalman->R;
double K_0 = Kalman->P_00 / S;
double K_1 = Kalman->P_10 / S;
Kalman->X_0 += K_0 * y;
Kalman->X_1 += 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;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////
초기화는 이렇게 해준다.
X 는 현재 상태를 의미한다. 자이로 + 가속도계의 경우 X_0은 각도, X_1은 자이로 편차를 의미한다.
Q는 노이즈 공분산 상수(물리적 시스템에 따라 결정)이다. 적절한 값으로 초기화해주면 된다. (보통 물리적 시스템 분석을 통해 구한다)
R은 측정 공분산 행렬로, 측정결과에 얼마나 노이즈가 섞이는지에 대한 상수이다. 적절한 값으로 초기화.
P는 상태 공분산 행렬로, 초기화시 정지상태면 0으로 전부 주면 되고, 불확실할수록 대각성분에 큰 값을 주면 된다.
void CMPU6050::InitKalmanRot(double Roll, double Pitch, double Yaw, double P)
{
KalmanAngX.X_0 = Roll;
KalmanAngX.X_1 = 0;
KalmanAngX.Q_0 = 0.01;
KalmanAngX.Q_1 = 0.04;
KalmanAngX.R = 0.3;
KalmanAngX.P_00 = P;
KalmanAngX.P_01 = 0;
KalmanAngX.P_10 = 0;
KalmanAngX.P_11 = P;
KalmanAngY.X_0 = Pitch;
KalmanAngY.X_1 = 0;
KalmanAngY.Q_0 = 0.01;
KalmanAngY.Q_1 = 0.04;
KalmanAngY.R = 0.3;
KalmanAngY.P_00 = P;
KalmanAngY.P_01 = 0;
KalmanAngY.P_10 = 0;
KalmanAngY.P_11 = P;
YawAngle = Yaw;
YawHF.tau = 0.01; // 얘는 yaw 데이터의 하이패스필터에 쓰이는 상수
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////
이제 구현부분을 잠깐 살펴보면..
#if MPU6050_CALIBRATEDACCGYRO == 1
#define MPU6050_AXOFFSET 0
#define MPU6050_AYOFFSET 0
#define MPU6050_AZOFFSET 0
#define MPU6050_AXGAIN 16384.0
#define MPU6050_AYGAIN 16384.0
#define MPU6050_AZGAIN 16384.0
#define MPU6050_GXOFFSET -21
#define MPU6050_GYOFFSET -10
#define MPU6050_GZOFFSET -24
#define MPU6050_GXGAIN 131
#define MPU6050_GYGAIN 131
#define MPU6050_GZGAIN 16.4
#endif
Vector CMPU6050::getConvRotateData(double DeltaTime) {
Vector AccelVec;
Vector GyroVec;
Vector Rotate;
mpu6050_readBytes(MPU6050_RA_ACCEL_XOUT_H, 14, (uint8_t *)buffer);
AccelVec.x = (((int16_t)buffer[0]) << 8) | buffer[1];
AccelVec.y = (((int16_t)buffer[2]) << 8) | buffer[3];
AccelVec.z = (((int16_t)buffer[4]) << 8) | buffer[5];
GyroVec.x = (((int16_t)buffer[8]) << 8) | buffer[9];
GyroVec.y = (((int16_t)buffer[10]) << 8) | buffer[11];
GyroVec.z = (((int16_t)buffer[12]) << 8) | buffer[13];
#if MPU6050_CALIBRATEDACCGYRO == 1
AccelVec.x = (double)(AccelVec.x-MPU6050_AXOFFSET);
AccelVec.y = (double)(AccelVec.y-MPU6050_AYOFFSET);
AccelVec.z = (double)(AccelVec.z-MPU6050_AZOFFSET);
GyroVec.x = (double)(GyroVec.x-MPU6050_GXOFFSET)/MPU6050_GXGAIN;
GyroVec.y = (double)(GyroVec.y-MPU6050_GYOFFSET)/MPU6050_GYGAIN;
GyroVec.z = (double)(GyroVec.z-MPU6050_GZOFFSET)/MPU6050_GZGAIN;
#else
GyroVec.x = (double)(GyroVec.x)/MPU6050_GGAIN;
GyroVec.y = (double)(GyroVec.y)/MPU6050_GGAIN;
GyroVec.z = (double)(GyroVec.z)/MPU6050_GGAIN;
#endif
if(bKalmanFiler){
KalmanPredict2D(&KalmanAngX, GyroVec.x, DeltaTime);
KalmanPredict2D(&KalmanAngY, GyroVec.y, DeltaTime);
KalmanUpdate2D(&KalmanAngX, atan2(AccelVec.x, AccelVec.z) * 180 / 3.141592);
KalmanUpdate2D(&KalmanAngY, atan2(AccelVec.y, AccelVec.z) * 180 / 3.141592);
HighPassFilter(&YawHF, YawAngle + GyroVec.z * DeltaTime, DeltaTime);
YawAngle = YawHF.FilterData;
Rotate.x = KalmanAngX.X_0;
Rotate.y = KalmanAngY.X_0;
Rotate.z = YawAngle;
return Rotate;
}
YawAngle = YawAngle + GyroVec.z * DeltaTime;
Rotate.x = atan2(AccelVec.x, AccelVec.z) * 180 / 3.141592;
Rotate.y = atan2(AccelVec.y, AccelVec.z) * 180 / 3.141592;
Rotate.z = YawAngle;
return Rotate;
}
참고자료
https://ko.wikipedia.org/wiki/%EC%B9%BC%EB%A7%8C_%ED%95%84%ED%84%B0
http://blog.naver.com/heennavi1004/10183064672
http://egloos.zum.com/spaurh/v/4488676
http://pinkwink.kr/781
http://hangondragon.blogspot.kr/2009/05/kalman-filter-part-2-kalman-filter.html
http://blog.naver.com/PostView.nhn?blogId=msnayana&logNo=80107534127
http://blog.naver.com/helloktk/80032439565
초음파센서의 1차원 칼만필터를 위한 자작 라이브러리 첨부함 HC_SR04 기준.
사용은 그냥 아래와같이 하면 됨.
KalmanPredictUpdate1D(&kalmanData, newData);
return kalmanData.X;
복사 붙여넣기하는데 들여쓰기가 자꾸 이상하게 되네..
p.s. Vector 구조체는 내가 임의로 만든거