KalmanFilter.cpp

// カルマンフィルタ
//
// 計算式の参考資料。
// http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/

#include "KalmanFilter.hpp"

KalmanFilter::KalmanFilter() {
  angle = 0.0;
  bias = 0.0;
  
  P[0][0] = 0.0;
  P[0][1] = 0.0;
  P[1][0] = 0.0;
  P[1][1] = 0.0;

  // variances
  Q_angle = 0.001;
  Q_bias = 0.003;
  R_measure = 0.03;
};

void KalmanFilter::setAngle(float newAngle) {
  angle = newAngle;
};

float KalmanFilter::getAngle(float newAngle, float newRate, float dt) {

  //予測フェイズ
  //状態方程式を解く(離散カルマン フィルターの時間更新方程式)
  // step 1
  rate = newRate - bias;
  angle += dt * rate;

  //事後推定誤差の共分散行列
  // step 2
  P[0][0] += dt * (dt * P[1][1] - P[0][1] - P[1][0] + Q_angle);
  P[0][1] -= dt * P[1][1];
  P[1][0] -= dt * P[1][1];
  P[1][1] += Q_bias * dt;

  //観測イノベーション（観測ノイズ）
  // step 3
  float y = newAngle - angle;

  //観測イノベーションの共分散誤差
  // step 4
  float S = P[0][0] + R_measure;

  //カルマンゲインを求める
  // step 5
  float K[2];
  K[0] = P[0][0] / S;
  K[1] = P[1][0] / S;
  
  //事後に対する状態方程式を解く（観測による事後推定）
  // step 6
  angle += K[0] * y;
  bias += K[1] * y;

  //事後推定誤差の共分散行列
  // step 7
  float P00_temp = P[0][0];
  float P01_temp = P[0][1];
  P[0][0] -= K[0] * P00_temp;
  P[0][1] -= K[0] * P01_temp;
  P[1][0] -= K[1] * P00_temp;
  P[1][1] -= K[1] * P01_temp;

  return angle;
};

//共分散set
void KalmanFilter::setQangle(float Q_angle) { this->Q_angle = Q_angle; };
void KalmanFilter::setQbias(float Q_bias) { this->Q_bias = Q_bias; };
void KalmanFilter::setRmeasure(float R_measure) { this->R_measure = R_measure; };