アルゴリズム – Sasapea's Lab

３軸センサー(QMC5883L)の測定値って結構ばらつくよね？

でもローパスフィルターかますと応答性が悪くて実時間制御には使えなくなるし．．．なんかいい方法ないかな．．．と。

そこで前回の測定値との差が小さければ平滑度を高くし大きければ平滑度を低くする適応型フィルターを考えてみた。

測定値を眺めてみると前回との差が±25程度ブレていたので、前回との差が0(0%)-128以上(100%)としたフィルターを試してみたところ安定度と応答性が両立できるような結果となった。二次関数なども試してみたけどこの方法のほうが簡単で結果もいい感じ。

int32_t d = 測定値 – 平滑値;
平滑値 += (d * (abs(d) < 128 ? abs(d) : 128)) / 128;

【試験プログラムの一部の抜粋】

　...
  int16_t axis[3] = {};
  while (1)
  {
    if (qmc5883l.readRaw(qmc5883l.output))
    {
      for (uint8_t i = 0; i < 3; ++i)
      {
        int32_t  d = qmc5883l.output.axis[i] - axis[i];
        axis[i] += (d * (abs(d) < 128 ? abs(d) : 128)) / 128;
      }
      print("X = ");
      print(axis[0]);
      print(" (");
      print(qmc5883l.output.axis[0]);
      print("), Y = ");
      print(axis[1]);
      print(" (");
      print(qmc5883l.output.axis[1]);
      print("), Z = ");
      print(axis[2]);
      print(" (");
      print(qmc5883l.output.axis[2]);
      print(")");
    }
    delay(500);
  }

　...

int16_t axis[3] = {};

while (1)

{

if (qmc5883l.readRaw(qmc5883l.output))

{

for (uint8_t i = 0; i < 3; ++i)

{

int32_t d = qmc5883l.output.axis[i] - axis[i];

axis[i] += (d * (abs(d) < 128 ? abs(d) : 128)) / 128;

}

print("X = ");

print(axis[0]);

print(" (");

print(qmc5883l.output.axis[0]);

print("), Y = ");

print(axis[1]);

print(" (");

print(qmc5883l.output.axis[1]);

print("), Z = ");

print(axis[2]);

print(" (");

print(qmc5883l.output.axis[2]);

print(")");

}

delay(500);

}

【手で傾けたりしてみたとき】
()内はQMC5883Lの測定値

【固定状態】
()内はQMC5883Lの測定値

カテゴリー: アルゴリズム

センサー値の平滑化に使える適応型フィルターを考えてみた。