ロボットの実装でカルマンフィルタの触りをやってみよう！【カルマンフィルタ入門】

2024/11/16

Category : 制御工学ロボット工学

カルマンフィルタとは？

カルマンフィルタとはモデルが線形の状態方程式で表現されかつ、分布がガウシアンであるという条件下で使用できるフィルタです。

しかしすべての関数やモデルが線形であるはずもありません。ときには非線形なモデルの状態推定を行いたいときもあるでしょう。そこでテイラー展開によって非線形モデルを線形モデルに落とし込んだものが拡張カルマンフィルタです。

今回はカルマンフィルタを単純なロボットを例にし、C++でわかりやすく実装を踏まえつつ説明するという試みです。

より抽象的で理論的な解説はこちらで解説しているので、もしよければ参照してみてください。

カルマンフィルター入門

突然ですが我々が散歩しているとき、どんな情報を用いて歩行を制御するでしょうか？

ただなんとなく手足を動かしているのではありませんか？

しかしカルマンは言いました。

「お前がうまく歩けているのは、お前の信じる今のお前の姿勢(状態)とその目でみた観測のおかげだ」

と。

いきなり何言ってるんだという感じですが、これはどういうことかというと、例えば人が歩いているとき、右足の次に前に出すのは左足、次に右足…と左右の足を交互に出して前へ進みます。

カルマンはもし少し前に左足を前に出しているのなら、今は右足を前に出しているだろうと予想が立つと言います。

この様に一個前の状態から今の状態が求まる方程式を状態方程式といいます。

より抽象的な表現をすれば

S_t = f(S_{t-1})

つまり、時刻 $t$ における状態 $S_t$ が一個前の時刻 $t-1$ (まで)における状態 $S_{t-1}$ から求まるというわけですね。

しかし当然歩幅は常に一定ではないでしょう。思ったより早く歩いているかもしれないし、変な方向に向かっているかもしれません。

そこでカルマンはこう言います。

「お前の思い込みだけでは真実には到達できない。大切なのは観測することだ」

これはどういうことかというと、私達は目をつむっていてはまっすぐ歩けないということです。目を開いて方向を正しているから直進を維持できているのです。より抽象的な表現をすれば

S_t = h(z_t)

つまり時刻 $t$ における状態 $S_t$ は時刻 $t$ (までの)観測値からも求まるという意味です。

「自分の適切な状態更新と観測の補完によって、我々は本当の姿勢を推定できる」

これこそがカルマンフィルタの発想なのです。

Wikipedia 英語版に掲載されているカルマンフィルタのプロセスイメージ

ロボットでの実装

ロボットの自立走行を例に考えましょう。ロボットは最初 $x$ 軸方向を向いていて、左側 $90\degree$ に $y$ 軸があるとします。

このときロボットの状態 $S$ は

S = \begin{pmatrix} x\\ y\\ \theta\\ \end{pmatrix}

で表現されます。( $S_0 =(0,0,0)^\top$ )となるわけですね。

状態方程式

ロボットは中心アルゴリズムでどちらの方向へどのくらいの速さで移動するかを定めます。その司令をうけてロボットの足は動きます。この司令を入力信号 $u_t$ とここでは呼称しましょう。

するとロボットの状態方程式は

S_t = f(S_{t-1} , u_t)

で表現されます。

カルマンフィルタ(あるいは拡張カルマンフィルタ)を用いると線形の状態方程式で表現され

S_t = A S_{t-1} + B u_t

と表現されます。

ロボットを例に考えると、 $u$ が速度 $v$ で $\phi$ の方向に回転したとすると

\begin{pmatrix} x_t\\ y_t\\ \theta_t \end{pmatrix} = \begin{pmatrix} x_{t-1}\\ y_{t-1}\\ \theta_{t-1} \end{pmatrix} + \begin{pmatrix} v_{t} \cos\left(\theta_t + \phi_t\right)\\ v_{t} \sin\left(\theta_t + \phi_t\right)\\ \phi_{t} \end{pmatrix}

C++で実装するなら状態方程式は

class State{
    State(){
        this->x = 0;
        this->y = 0;
        this->theta = 0;
    }
    double x,y,theta;
}

State update( State pre_s, State u, double dt){//一個前の既知の状態,入力信号(速度と角度),時間変化量
     auto s = State();
     double radian = (pre_s.theta + u.theta * dt) * M_PI / 180.0;
     s.x = pre_s.x + u.x * cos(radian) * dt ; 
     s.y = pre_s.y + u.y * sin(radian) * dt ;
     s.theta = radian ;
     return s;
}

この様に表現されます。意外と簡単でしょう？

観測方程式

先述の通りカルマンフィルタには内部状態の更新と観測値の反映というものがあります。ロボットの実装ではGNSSを用いた衛生情報を利用することが多いので、今回はこれを例にします。 GNSSからはそのまま位置と向いている向きが取れるとすると

S_t = z_t

となります。(厳密にはｍ単位に変換するなど事前処理が求められますが、ここでは割愛します)

これをC++で表現すると

State obsevation(State gnss){
    auto s = State();
    s.x = gnss.x;
    s.y = gnss.y;
    s.theta = gnss.theta;
    return s;
}

となります。まんまですね！

状態と観測の結合

以降では状態方程式の推定値をS,観測方程式の数値をZとします。

さて、ここまでで状態方程式と観測方程式から２つの同じ時刻におけるロボットの状態(位置、向き)が取得できました。この２つの値は似ているでしょうが、微妙にノイズが含まれていて異なっています。

私達はどちらを信用すればよいのでしょうか？

最も単純でナイーブな考え方では平均を取ることです。つまり中点を取るんですね。

s_t = \frac{S_t + Z_t}{2}

としちゃうわけです。

しかし当然ですが、GNSSは屋根があると数値に大きなノイズが乗ってしまうし、あるいは滑りやすい道ではロボットの思ったとおりに進まない場合もあるでしょう。単純な平均ではこういったケースのノイズをそのまま1/2倍したまま含んでしまいます。これではよろしくありません。

そこで登場したのがカルマンゲインというものです。

カルマンゲイン

先述の通り、我々は状態方程式と観測方程式から得られる結果をそれぞれどのくらい信用すればよいのかという課題に直面しています。

カルマンは数値の信頼性を分散で表現しました。値が散らばっているほど信用できない数値だとしたのです。分散が小さければより信頼に値すると考えたわけですね。

状態方程式と観測方程式で得られる $S,Z$ それぞれの分散を $\sigma_s,\sigma_z$ とします。

このときカルマンゲインは

k = \frac{\sigma_s}{\sigma_s + \sigma_z}

で表現され、推定値は

\begin{matrix} s_t & = & S_t + k \left(Z_t - S_t \right) \\ & = & S_t \left(1 - k\right) + k Z_t\\ \end{matrix}

とされます。

ここで得られた $s_t$ こそがカルマンフィルタによる推定値なのです。

これをC++で表現すると

State kalmanfilter(State s,state z, double sigma_s,double sigma_z){//状態方程式、観測方程式、それぞれの分散
    double kalman_gain = sigma_s / (sigma_s + sigma_z);
    auto state = State();
    state.x = s.x + kalman_gain * (z.x - s.x);
    state.y = s.y + kalman_gain * (z.y - s.y);
    state.theta = s.theta + kalman_gain * (z.theta - s.theta);
    return theta;
}

となります。

なお、これは各パラメータに相関がないものとして扱っているので、多変数カルマンフィルタの場合は分散共分散行列を使ったより複雑な実装になることをご留意ください。

最後に

この記事は簡単なカルマンフィルタの実装にとどめていますが実際にロボットで使用してみると、意外と使えることに驚くと思います。

カルマンフィルタは条件を満たせば最良の解が得られることでも知られています。ぜひ頑張ってみてください。

また実装が楽なのにそこそこ良い推定値を出すとされるパーティクルフィルタの解説もよかったら読んでみてください。

それではまた。

参考文献

B Ristic, S Arulampalam , N Gordon “Beyond the Kalman Filter: Particle Filters for Tracking Applications”
KalmanFilter.net カルマンフィルターチュートリアル
Math Works “カルマンフィルター”
Wikipedia “カルマンフィルター”
Wikipedia “チャップマン＝コルモゴロフ方程式”
機械学習と情報技術 “【機械学習】モンテカルロ積分を理解する”
島田直希 “時系列解析　自己回帰型モデル・状態空間モデル・異常検知”