Kalman Filter

    基本上所有參數跟成員的意思跟Simple Kalman Filter都差不多（參考預測與更新七個步驟），初始化r_measure是觀測誤差q_angle與q_bias是在加速度計與陀螺儀的預測選擇較相信誰arg_angle是初始角度，而member function 的 Update沒有空行的code表示是同一組算式，第一組計算當前陀螺儀的角速度減去前次計算出的偏移，接著將角度加上這個差的角度，第二組P是一個2x2 Matrix，內容是推導後的結果用來計算預測估計誤差，第三組量測現在加速度計角度與上次角度的差，第四組計算預測與觀測的Covariance，第五組計算最佳卡爾曼增益，第六組由卡爾曼增益乘上前次與這次角度差決定角度與偏移要更新多少，第七組則是用卡爾曼增益乘上當前預測估計誤差然後更新下個預測估計誤差，這七個步驟的前兩個是計算預測，後五個才是真正的更新，順序都與上面的wiki參考預測與更新七個步驟一樣，然後這整個系統的觀測時間統計是個隱馬爾可夫模型。

Acc只有加速度計，CF用互補濾波加上陀螺儀修正，KF用卡爾曼濾波加上陀螺儀修正。

Kalman Filter：


#ifndef __KALMAN_FILTER__
#define __KALMAN_FILTER__

class KalmanFilter
{
  public:

  KalmanFilter(double r_measure,double q_angle,double q_bias,double arg_angle)
  {
    Q_angle = q_angle;
    Q_bias = q_bias;
    R_measure = r_measure;
    angle = arg_angle;
 
    bias = 0;
    P[0][0] = 0;
    P[0][1] = 0;
    P[1][0] = 0;
    P[1][1] = 0;
  }

  /* Accelerometer Angle , Gyro Rate , dt */
  double Update(double acc,double gyro,double dt)
  {
    rate = gyro - bias;
    angle += rate * dt;
 
    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;
 
    angle_diff = acc - angle;
 
    S = P[0][0] + R_measure;
 
    K[0] = P[0][0] / S;
    K[1] = P[1][0] / S;
 
    angle += K[0] * angle_diff;
    bias += K[1] * angle_diff;
 
    P[0][0] -= K[0] * P[0][0];
    P[0][1] -= K[0] * P[0][1];
    P[1][0] -= K[1] * P[0][0];
    P[1][1] -= K[1] * P[0][1];
 
    return angle;
  }

  private:

  double R_measure;
  double Q_angle;
  double Q_bias;

  double angle;
  double bias;
  double rate;

  double angle_diff;
  double S;
  double P[2][2];
  double K[2];
};

#endif


Used：

    下面初始參數適用幾乎所有的IMU除非有特別要自己設定。


KalmanFilter KFx(0.03f,0.001f,0.003f,0.0f);
KalmanFilter KFy(0.03f,0.001f,0.003f,0.0f);
unsigned long last_KF_computing_time;     //us





void loop()
{
  double KF_x_angle = KFx.Update(AccAngleX,GyroRateYaw,
  (double)(micros() - last_KF_computing_time) / 1000000.0f);

  double KF_y_angle = KFx.Update(AccAngleY,GyroRatePitch,
  (double)(micros() - last_KF_computing_time) / 1000000.0f);

  last_KF_computing_time = micros();
}