目次
はじめに
あるロジックをプログラムに実装するとして、自分の場合だったらまずはMATLABやPythonでプロトタイプを作って有効性をシミュレーションします。そして最終的にはC言語に移植をする訳ですが、ここで悩ましいのがMATLABやPythonで書いたコードと同じような書き方をC言語では出来ない事です。そして、その違いを特に感じるのが行列演算になります。3次元だろうが4次元だろうが、MATLABやPythonでは簡単に実装できてしまうのに対して、C言語で実装しようとするのはかなり面倒だし、デバッグも大変です。正直言ってプロトタイピングした甲斐がありません。そこで、面倒な行列演算を何とかシンプルに実装できないかを考える事にしました。
手計算
コードをシンプルにしたかったり、プログラムの計算負荷を軽くしたい時に良く取られる手段の一つとして、事前に計算出来る部分は予めやっておくというのがあります。少々面倒ではありますが、上記で書いたような行列演算も2次元、3次元くらいなら手計算出来ますし、その結果得られた形というのはかなりシンプルになり、それくらいであればC言語で書くのもあまり苦ではならなくなります。また、事前に計算してみる事によって理論の理解がより深まるし、リファレンスとしていた理論式なんかに万が一誤植があったとしても気付く事ができるので、まずは頑張って手計算してみるという事には多くのメリットがあります。そこで今回の記事では、自動運転技術としては一般的なカルマンフィルタの理論を例として、事前に手計算する事の大切さを紹介してみようと思います。
カルマンフィルタについて
カルマンフィルタとは、誤差を持った観測値を用いて、システムの状態を推定する技術です。詳しいことについては、以下の記事や書籍が参考になると思います。
予測
今回の例題として取り上げるのは、速度入力が与えられる事によって動く物体の位置座標(X, Y)をセンサでΔt秒ごとに観測し続ける2次元のシステムとします。ここで、ある時刻tにおける位置座標(Xt, Yt)を求めるシステムの状態方程式は以下のようになります。
この式を用いて、一時刻前に最終的に推定された位置をベースに次の時刻での物体の位置を予測する訳ですが、ここでは同時に予測された位置情報が含んでいるであろう誤差の共分散行列(予測誤差共分散)も以下の式で計算します。この時、Qはシステムへの速度入力が含んでいるとする誤差の分散パラメータ(システムノイズ)、𝑃(𝑡−1|𝑡−1)を一時刻前に推定された誤差共分散行列としています。
この予測誤差共分散行列を計算するコードを書く場合、例えばMATLABなら以下のように書くことが出来ます。
predP = F * estP * F' + G * Q * G'
もとの数式とほぼ同じような形で書くことができていいですね。しかしながら、C言語ではこう簡単に書くことができません。前後の次元がちゃんと一致するように、入力引数と返り値の行列 or ベクトルを定義したり、転置行列を計算するのに行と列を入れ替えるための関数をわざわざ作ったり、とかなり面倒です。
こういう場合、今回の例のように2次元あるいは3次元くらいであれば、事前に計算できるところは手計算しておいて、その計算して得られた最終的な形をコードに書いた方がシンプルで、コードを書く量も少なくできるし、デバッグなどもしやすくなります。
ちなみに、上記の予測誤差共分散行列の計算式を手計算すると以下のような形が得られます。
状態方程式の次元やシステムノイズパラメータ、刻み時間Δtは定数であるので、上記の計算から得られた形である、
の部分だけをコーディングすればいいことになります。これなら書くのが楽になっていいですね。もし途中で観測値が得られず、予測のみの状態が継続するとシステムの誤差共分散はどんどん大きくなっていく訳ですが、この計算結果を見ればそうなるのが分かりますね。このように、自分の手で事前に計算してみることによって、システムの挙動がよりイメージしやすくなるというメリットもあります。
観測
一時刻前の状態をベースに予測値を計算したら、今度はセンサからの観測値を扱うステップに移ります。ここでいうセンサとは、車やロボットが自己位置を知るためのGPSや、周囲の物体との距離や角度を測るミリ波レーダやスキャンレーザなどの事です。今回の例では、物体の位置座標をセンサで測量し、それによって得られる2次元のX, Y座標を観測値とします。
このステップでは、センサから実際に得られた観測値と、予測値をベースに観測方程式より計算した観測予測値の差分(イノベーション)を計算し、その差分のどれだけを補正量とするのかを観測予測誤差の共分散から決定します(カルマンゲイン)。まずは、観測値Ztと観測方程式の行列H、イノベーションeを以下のように定義します。
次に、観測値が含んでいるであろう観測誤差の共分散パラメータと、それにより計算する観測予測誤差共分散行列を計算するのですが、ここでも先ほどの予測誤差共分散と同様に手計算します。観測誤差パラメータをR、観測予測誤差共分散をSとすると、
というように計算することができ、最初は2次元の行列演算ですが、計算してみれば結局は予測誤差共分散と観測誤差共分散の足し合わせという形に落ち着くことが分かります。これを見れば、観測予測誤差と言われるのも納得ができますね。
更新
そして、ここで得られた観測予測誤差共分散行列を利用してカルマンゲインを計算するのですが、まずは改めてその計算式を見てみましょう。計算するカルマンゲインをKとするなら、
という行列演算になりますが、さあ、ここで面倒なのが出てきましたね。そう、逆行列の計算です。学生の頃に数学で習った逆行列を求める公式を丸々定義する方法もありますが、2次元ならまだしも3次元で掃き出し法なんて実装しようとしたら面倒ですよね。なので、逆行列計算は先に計算しておいちゃいましょう。先程求めた観測予測誤差の逆行列なので、
となり、あとはこれをカルマンゲインの計算式に入れると、
という形で求まります。この計算結果を見ると、カルマンゲインというのは予測誤差共分散と観測誤差分散の合計に対する予測誤差共分散の割合と解釈できます。予測誤差の方が観測誤差よりも大きいとカルマンゲインの値は大きくなり、そのゲインをイノベーションにかけ合わせると、その補正による推定結果は観測値に近い結果となります。逆に、予測誤差の方が観測誤差小さくなるとゲインは小さくなるので、イノベーションの内の少量分しか補正されないことになり、推定結果は予測値に近いものとなります。計算式とその最終形は以下の通りです。
これにより、最終的な推定位置を求めることができるので、あとはその時の推定誤差共分散行列を計算して一連のカルマンフィルタの処理は完了です。推定誤差共分散行列の計算は以下のようになります。
観測誤差Rはパラメータなので固定値です。そのため、観測値が得られず予測状態が継続すれば予測誤差が蓄積していき、それによって最終的な推定誤差共分散が大きくなるということがイメージできる形が得られました。
まとめ
いかがだったでしょうか?面倒でも、できるだけ手計算を事前に行っておくことで、実装したい計算式が実はシンプルな形に落とし込めるということ、そしてそれを見ることで自分が実装するロジックの挙動がイメージできるということが少しでも伝われば幸いです。