Arduino カルマンフィルタライブラリの中身を解説

　Arduinoのライブラリにカルマンフィルターがあるのですが、中身がどういうことをやっているのか勉強を兼ねて見てみました。

　IDEでライブラリ検索するとカルマンフィルターでいくつかヒットするのですが、今回はこのライブラリのプログラム（ソース）をカルマンフィルターの説明を加えながら見てみたいと思います。

　https://github.com/TKJElectronics/KalmanFilter

　このカルマンフィルターですが、加速度、角速度と制御周期を入力として、カルマンフィルターを通して、きれいな角度を推定しているライブラリとなります。ただ後述しますが、このライブラリは3軸（3D）回転がある場合には対応していないようですね。

　カルマンフィルターの説明を加えながら中身を見ていきたいと思います。（説明といっても基本式を展開して、ソースと一致しているか見ているだけです・・・。）

状態空間モデル
STEP1（状態の予測）
STEP2（誤差共分散の予測）
STEP3（観測残差）
STEP4（観測残差の共分散）
STEP5（カルマンゲインの更新）
STEP6（状態の更新）
STEP7（誤差共分散の更新）

状態空間モデル

▼状態方程式はこんな感じで定義しているようです▼

　（※以降、添え字は適当です。無視して下さい・・・。）

　ωbはバイアス誤差です。

▼それぞれこのように置いて▼

▼行列で表現すると▼
　

　このような感じになります。状態ベクトルは角度とバイアス誤差、入力は角速度で定義。バイアス誤差は直接測定できないので、過去の値を持ってくるようです。少し腹落ちしない感じですが、、、。

　▼観測方程式は▼

加速度センサーから角度を観測してます。

　ここからライブラリのソースと絡めて見ていきたいと思います。（式を展開していき、ソースと一致していくか見ていきます）

　ステップ毎に見ていきたいと思います。まずは予測ステップから。

STEP1（状態の予測）

▼ソース▼

    /* Step 1 */
    rate = newRate - bias;
    angle += dt * rate;

　角速度から角度計算している部分です。ソースの表現（式）を少し変えてみると

    angle = angle + dt*(newRate -bias);

となって、状態方程式

と同様です。単純に角速度（バイアス誤差を加味）からの積算で現在の角度を予測してます。

STEP2（誤差共分散の予測）

▼ソース▼

    /* 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;

　誤差共分散の予測部のソースはこのようになってます。

　カルマンフィルターの式に当てはめていくと

　となって、ソースと同様になります。

　ここから更新ステップを見ていきます。

STEP3（観測残差）

▼ソース▼

    /* Step 3 */
    float y = newAngle - angle; // Angle difference

　このようになってます。「観測値（加速度からの角度）」から「STEP1で予測した角度」の差分となります。

　観測残差の当てはめて、式を展開していくと

　このようになります。少し腹落ちしないですが、ソースと同様なので間違いではないのかと・・・。

STEP4（観測残差の共分散）

▼ソース▼

    /* Step 4 */
    float S = P[0][0] + R_measure; // Estimate error

　ソースはシンプルですね。

　カルマンフィルターの式に当てはめて展開していくと

　このようになってソースと一致します。

STEP5（カルマンゲインの更新）

▼ソース▼

    float K[2]; // Kalman gain - This is a 2x1 vector
    K[0] = P[0][0] / S;
    K[1] = P[1][0] / S;

　ここもソースはシンプルですね。

　同様にカルマンフィルターの式を展開していくと

　2×１の行列となり、ソースと同様です。

STEP6（状態の更新）

▼ソース▼

    /* Step 6 */
    angle += K[0] * y;
    bias += K[1] * y;

予測値を更新している部分です。角度、バイアス誤差の更新をしてます。

　カルマンフィルターの式から展開していくと

　このような感じになり、ソースと同様になりますが、ここも少し消化不足です・・・。

STEP7（誤差共分散の更新）

▼ソースではこのように計算してます▼

    /* 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;