Eigen Library 와 Simple Kalman Filter

 

Kalman Filter 에 대해서 처음 공부하고 있었을 때쯤, LabVIEW를 이용하여 매트릭스 계산을 하면서 편하게 그래프 찍으며? 일했다. 근데, 이걸 STM32에 탑재하려고 하니 매트릭스 연산을 코딩해야 되는 상황이 발생했다.

 

한번쯤 들어본 것이 Eigen 라이브러리이다. 이번 기회에 한번 적용해보고자 찾아보니, Mbed에서 이미 훌륭하신 분이 라이브러리를 만들어서 배포해주셨다(Yogi KURODA).

 

Eigen - Eigne Matrix Class Library | Mbed

 

단순한 Kalman을 만들면 Inverse, Transpose 동작을 확인할 수 있으니, 

임의의 데이터를 감소시켜가면서 동작하게 코딩해봤다.

 

그리고 해당 데이터를 랩뷰에서 만든 것과 비교하니 오차없이 잘 동작한다.

 

#include "mbed.h"
#include <iostream>
#include <Eigen/Dense.h>
using namespace std;
using namespace Eigen;
 
Serial pc(USBTX, USBRX);
 
MatrixXd A(3,3);
MatrixXd P(3,3);
MatrixXd Q(3,3);
MatrixXd H(1,3);
MatrixXd X(3,1);
MatrixXd X_pred(3,1);
MatrixXd P_pred(3,3);
MatrixXd Y(1,1);
MatrixXd R(1,1);
MatrixXd K(3,1);
MatrixXd HxXpred(1,1);
 
float move = 800;
    
////////////////////////////////////////////////////////////////////////////////
void KalmanFilter_Prediction()
{  
  X_pred = A * X;                       //(1)Project the state ahead
  P_pred = A * P * A.transpose() + Q;   //(2)Project the error covariance ahead
}
 
 
void KalmanFilter_Correction()
{
  Y = H * P_pred * H.transpose() + R; 
  Y = Y.inverse();
  
  K = P_pred * H.transpose() * Y;          //(1)Compute the Kalman gain
  
  HxXpred = H * X_pred;
  X = X_pred + K * (move - HxXpred(0));    //(2)Update estimate with measurement 'move'
  
  P = P_pred - K * H * P_pred;             //(3)Update the error covariance
}
 
 
 
////////////////////////////////////////////////////////////////////////////////
int main()
{
  A << 1,0.05,0.00125, 0,1,0.05, 0,0,1;
  P << 3,0,0, 0,3,0, 0,0,3;
  Q << 0.0001,0,0, 0,0.0001,0, 0,0,0.0001;
  H << 1,0,0;
  X << 850, 3, 1;
  R << 0.001;
 
    
  while(1)
  {
    KalmanFilter_Prediction();
    KalmanFilter_Correction();
    move--;
    
    pc.printf("Current:%g\n", X(0));
    pc.printf("Velocity:%g\n", X(1));
    pc.printf("Acceleration:%g\n\n", X(2));
    
    //pc.printf("X_pred\n[%g,%g,%g]\n", X_pred(0),X_pred(1),X_pred(2));
    //pc.printf("P_pred\n[%g,%g,%g]\n[%g,%g,%g]\n[%g,%g,%g]\n", P_pred(0),P_pred(3),P_pred(6),P_pred(1),P_pred(4),P_pred(7),P_pred(2),P_pred(5),P_pred(8));
    //pc.printf("Y: %g\n", Y(0));
    //pc.printf("K\n[%g,%g,%g]\n", K(0),K(1),K(2));
    //pc.printf("HxXpred: %g\n", HxXpred(0));
    //pc.printf("X\n[%g,%g,%g]\n", X(0),X(1),X(2));
    //pc.printf("P\n[%g,%g,%g]\n[%g,%g,%g]\n[%g,%g,%g]\n", P(0),P(3),P(6),P(1),P(4),P(7),P(2),P(5),P(8));
    
    
    wait_us(50000);   
  }
}