部活を引退した、らしい

先日、部活を引退した、らしい。

らしい、というのは胃腸炎で死んでいて納め稽古に行けないうちに代替わりしてしまったかです。

大学での部活は躰道部(たいどうぶ)というところでした。 躰道をする部活、だから躰道部なんだけどそもそもの話、大体の人にとって躰道って何？ってなる。

そこでまずは、躰道について説明します。

躰道って？

躰道とは、躰道部のホームページを見ると

躰道（たいどう）は、祝嶺 正献（しゅくみね せいけん）という空手家が空手の一派「玄制流」を母胎として1965年に体系化した新しい武道です。
「体軸の変化によって攻防を展開する」武道で、あらゆる三次元的動作が取り入れられた魅力的な攻防が特徴です。
躰道には「体軸の変化」として「5つの操体」があり、それらを利用した「3つの競技」が存在します。

と書いてあります。ちなみにこのページ、僕の同期たちが中心になって作ったホームページです。

この説明では、躰道を4年間やっていた自分でもサッパリだから、まずは動画を見てもらうのが早いでしょう。ちなみにこの動画、僕の同期たちが中心になって作ったものです。

www.youtube.com

どうでしょう？
随分と激しいですね。これを見せた友達からは必ずこう聞かれます。

「バク転できるの？」

答えはnoです・・・。バク宙とかバク宙半捻りならできるんですけどね。

・・・イキリオタク感がありますね。
はい。気を取り直して躰道部の話に戻ります。

躰道部の生活

躰道部での一年はどんな感じだったか、思い出してみました。
といっても最近の一年が強烈すぎてそれ以前の三年の記憶が全然ない・・・。

一年生の頃

四年間の大部分を躰道に注いできたとはいえ、このころはまだそれほど腰を入れていなかった覚えがあります。
新歓期にアメフトサークルと躰道部でめちゃくちゃ迷った挙句、躰道部の当時の主将にジャンケン勝負で負けたら入部しますといい、しっかり負けてそのまま入部してしまったが最後、ものすごいエネルギーを注ぎこむことになりました笑

躰道部に入っていなかったらロボテックかどこかに入っていたかもしれません。ていうかかなりそのころは迷っていた。

なんか高校の先輩いるし、やってみっか〜で入った躰道部、しかしそこで思い知ることになる。
これ予想外に難しい。

躰道部では毎年6月頭くらいに銀杏杯なるものがあったりします。東大のほかのサークルとかでもメジャーだったりしますね、銀杏杯。

この銀杏杯に、一年生も出ることになります。
入部して一ヶ月の新米に何ができるのか笑

正直恥さらしをする儀式みたいなところがある気がしますがとても楽しい行事です。一年生は習いたての『旋体の法形』(女子は『旋陰の法形』)を通すことになります。

www.youtube.com

動画では5人で通していますが、これは団体法形という競技。銀杏杯では一人で通します。

この、バク転もバク宙もしない、この型が、とにかく難しい。
ただ回転するだけやろ、と思いきや自分の体はまっすぐ前に行かずに空転するわ畳に足巻き込むわで散々でした。

で、同期内で順位がつくのですが見事に入賞できず・・・w
10月にある学生大会の新人団体法形の枠争いで早くも同期に水をあけられたかと思って、結構ショックな経験でした。

6月や7月にある地方大会でも同期が活躍する中、あまり良い結果も残せず焦りのみが募るまま、学生大会シーズンに入りました。

学生大会？

この学生大会、これ東大が現在までに10連覇している学生の全国大会なんです！

「マイナースポーツだしそんな大きくはないやろ」

・・・まあその通りなんですけど。それでも会場の東京武道館で6コート作って競技する大会なので躰道の大会の中では大きい方です。
躰道部でも最重要イベントの一つでした。

多くの躰道部の一年生にとっての目標は、同期との争いに勝ち抜いてこれに新人団体法形競技で出場することです。

新人団体法形競技とは？・・・各大学の一年生が先ほどの『旋体の法形』か『旋陰の法形』を五人出会わせて通す競技です。

ともかく、ただ通すだけではなく5人で合わせてというのがなかなかに難しい。大会当日までひたすらに合わせる練習を重ねます。スピードや強さを落とさないままに合わせる・・・合わせる・・・。
これを週5くらいで練習します。

挫折

結果から言うと、僕は新人団体法形競技には出られませんでした。補欠だったので6人目。5人目までに入れていれば出られたのにざんね〜んって話です。

この経験が兎にも角にもひたすらに悔しかった。学生大会はその年、前人未到の七連覇目を達成して、打ち上げではお祭り騒ぎだったけどそんな中僕はひとりで必死で悔し泣きするのをこらえていました。受験に落ちたときの次くらいには悔しかった。その時から実は3年がかりの大きな目標を立てていました。

４年生の学生大会でMVPを取る。

その日から目標に向かっていろいろと計画を立て実行してきました。

計画

MVPを取るというのはとても難しいことです。
歴代のMVPをとった選手に関して考えているうちにある仮説が浮かび上がってきました。

すなわち、

個人法形と個人実戦の両方に出場していて、かつ少なくとも一方で優勝している。

この仮説の真偽は確かめられていませんが結構あたっているのではないかと思っています。
そしてこの条件をクリアするのはとても難しい。

まず、部内での争いに勝たないと行けない。僕の同期たちはとても優秀で、最終的には世界大会に個人で出たものもいました(後述)。
人に実力を認めてもらうにも、結果を出せばすぐ認めてもらえるなんてことはなくて実績を積み重ねて長期的に世論を作っていかなければならない。

そして、純粋に法形(説明をしていませんでしたが型の競技です)と実戦の両方で優れた成績を残すということがとても難しいこともあります。 両方をトップレベルまでにあげる練習は途方もなく、そして特に実戦はケガのリスクがつきものでした。ケガをしてしまうと、長いときには数ヶ月、動けない時期が出てしまいます。

このような検討をした上で、僕は各年ごとに中期目標を、おきました。

• 二年生の時は、上級生との争いに勝って学生大会で一競技出場する。

• 三年生の時は団体の法形、実戦、展開の三競技に出場する。

• 四年生の時は、個人の法形、実戦、団体展開の三競技に出場する。

これはどれも相当にチャレンジングな目標です。

二年生からは全員同じ土俵で競い合うことになります。自分より一年、二年と経験が長い上級生を差し置いて大会に出るのは難しく、毎年1人か2人いるかいないかという話でした。

そして、三年生の団体三競技に出る話も純粋に練習量の観点からとてもキツイです。三競技の全ての練習に出ることは物理的に不可能な場合もあり、週7回以上の練習が約束されています。

そして、四年生の個人二競技は特に東大躰道部ではほぼ不可能でしょう。仮に実力が伴っていたとしても相当に圧倒的でなければ、出場を許してもらえないのではないかと思います。

それが分かった上であえてこの目標を掲げたのは、おそらく個人的な思想として目標は大きい方がいいという考えがあるからだと思います。
とにかく、これを達成できるほどの実力があればMVPも夢ではない、という考えでした。

二年生の頃

転体の法形

二年生のときは、その年の方針で初めのうちは二年生男子は全員団体法形の練習を学生大会に向けてすることになっていました。

団体法形では、東大は毎年『転体の法形』というものを練習しています。

youtu.be

転体の法形は、筋の中でバク宙などを飛ぶところもあり比較的難しい法形と言えます。

僕は初めのうち、これと男子実戦の練習に出ていました。体力がとことんない自分にとって全法形の中でも屈指の体力消費量を誇る、転体の法形の練習は毎回とても辛いものでしたが、それでも食らいついていったのは先に掲げた目標があったからでしょうか？

一年生のうちにバク宙を飛べるようにしておいたこともあり、自分は幸運にも七月の地方大会で団体法形競技で出ることができました。実はこの時にとった銀メダルが自分にとっての初メダルでした。三チーム中2位という、なんとも締まらない話ですが...笑

七月の地方大会に出られたことは、順調に実力をつけられているということを示しているといえます。下級生も出す、という方針が少し働いていることをおいても、同期の中での順位としてはいい位置にいるということだったのでしょう。

2度目の挫折

ここまで順調だったのですが、学生大会前はやはり過酷でした。ちょうど先輩の中ではあまり団体法形をやっていない人が多かったのもあり、二年生の自分たちにとっては枠をいただく千載一遇のチャンスが巡ってきていた折でもあります。

自分は5人目か6人目、つまりメンバーになれるかなれないかのボーダーにいました。

しかし、大会1週間前の調整で最初の枝(筋の中の短いまとまりのこと)の側転を片手側転に変えるという変更があって、僕はその変更に適応できず腰を痛めてしまいました。

結果としてその年の団体法形は優勝し、東大は総合優勝8連覇目を達成しましたが、僕はやっぱりやりきれないものを感じてしまっていました。

全日本大会

ところが、この話には続きがありました。

毎年、その一年を締めくくる大会として全日本大会というものがあります。
学生大会は学生の全国大会ですが、全日本大会は全躰士にとっての全国大会、20年以上続けている高名な先生も出場したりと、まさに躰道界での頂点を決める大会とも言えるでしょう。

この全日本大会に僕が出られるかもしれない、というのです。

僕はその話に飛びつきました。競技はやはり団体法形です。学生大会と違うのは二回出てくる高等運身(バク転、バク宙などを筋に決められたところで飛びます)がバク宙から捻宙(バク宙半捻りのこと)に変わったことでしょうか。

僕は捻宙を早いうちから飛べるようになっていたこともあってなんとか5番目に食らいつきました。当時のリーダーは、その時点で全日本大会で個人法形で3位に入った大先輩で、そんな伝説的先輩と一緒に団体法形を通すというのはまたとない機会でした。

2ヶ月の突貫工事の末、全日本大会では団体法形競技で優勝しました。全日本大会のメダルは学生大会のものよりも一回り大きく、そのずっしりとした重みはその結果が学生大会で入賞することよりも優れていると言っているようでしたが、それでも僕は「学生大会で一競技出場する」という目標を達成できなかったことに少しモヤモヤしていました。

三年生の頃

三年生にもなると、だいぶ落ち着いてきて色々な動きができるようになってきました。これは純粋に躰道の動きに慣れてきたというのもありますし、転体の激しい動きで体がかなり作られたというのもあるでしょう。

ともかく、黒帯にもなったので色々と挑戦したい欲が湧いてきました。自分は幸運にも展開競技の正規メンバーとして選ばれていました。

展開競技というのは少し特殊な競技で、6人でチームを構成するのですが全員やることが違います。団体法形競技ならば全員が同じ動きをするので、怪我人が出ても交代することができましたが展開競技では容易に交代は出来ません。結果として、チーム発足時にメンバーになっていることが、学生大会での出場メンバーになることに強く結びついているのです。

展開競技の練習は、競技の特性上あまり効率が良くありません。30秒の筋をみんなで作り上げて行くのですが、ああでもないこうでもないと議論しながらやっていくので1時間半のうち45分は話していたなんてこともあります。それでも展開競技は自分にとってはかなり為になるものでした。以下は展開の練習風景の様子です。

自分はこの時期、先の目標もあったので団体法形競技、団体実戦競技、団体展開競技の三競技の練習をしようとしていましたが、結局団体法形競技の練習にはほとんど出ずじまいになってしまいました。三競技の練習に出ることは週7日毎日練習をしていることを意味し、身体を休める時間もなくパフォーマンスがみるみる落ちて行くことに気づいてしまったからです。

実戦

競技を絞ることは正直なところ大きな決断でした。僕の掲げていた目標からは遠ざかることを意味していたからです。それでも僕は自分が三競技の練習をやり切る体力も気力もないことを悟り、比較的すんなりとその決断をしました。

競技をしぼった以上、残りの二競技には全力を注ぎました。実のところ実戦競技は一つ上の先輩がかなり強く、学生大会に出場するメンバー入りできるかどうかはかなり怪しいところでした。一番強いと思われ、実戦チームのリーダーを務めていた先輩にはついぞ勝てなかったものです。

自分は対外試合で勝った経験も少なく、大会直前になるまではメンバーには入っていませんでした。しかし、大会一ヶ月ほど前になりチームの主力だった先輩が相次いで大きな怪我をしてしまい、僕にも出場のチャンスが回ってきました。実際のところ、部活としてみた場合この状況はよくないものでしたが、自分にとっては千載一遇の好機であったと言えます。

学生大会出場

この年、僕は三年目にして初めて学生大会の畳を踏むこととなりました。結局展開と実戦の両方に出ることができ、その結果として両競技で優勝をすることができました。この結果をもって、三年間の努力がようやく報われたような心持ちになりました。

意識をしていなかったことですが、この時点で僕は団体法形と団体実戦、そして団体展開の３つの団体競技すべての金メダルをとったことがある部員になっていました。部員数がかなり増加していたこのとき、全国レベルの大会で三競技全てに出場したことがある部員は僕一人になっていました。このことは後々の過ごし方に少し大きな影響を与えました。

再び全日本

そして予想していなかったことが起きました。自分が再び全日本の団体法形競技にでられるかもしれないチャンスが巡ってきたのです。この時、団体法形競技のメンバーはあまり多くなく人が足りていない状況でした。

自分は一年のブランクがあり正直それほど良くなかったのですが、メンバーが足らなくなる可能性もあってチームに入らないか、とお誘いを受けたのです。これもまた、またとないチャンスとばかりに僕はその話を受けました。しかし、練習期間は一年前の2ヶ月と比べても1ヶ月短く、一年前にリーダーを務めていた先輩は他の競技に専念するとのことで現役の部員のみでチームを組む必要がありました。

結果としてこのときの全日本では僕達の団法は予選敗退をしました。東大は団体競技がとても強く、予選敗退は近年では一切なかったのもあってこの結果はとても衝撃的でしたが実際のところ、他のチームと比べても納得の行く結果であったことは事実です。この年の最後を締めくくる大会ではあまり良い結果を出せませんでしたが、これまでのところを振り返ると自分の設定した目標は少し形を変えながらも大枠は外れていなかったように思えます。

「学生大会で一競技に出場する」、代わりに全日本大会で団体法形に出場し、「学生大会で団体の法形、実戦、展開競技に出場する」、代わりに学生大会で団体の実戦と展開に、全日本大会で団体の法形に出場していたのは、後から考えればかなり幸運で、そして上手く行っていたのでしょう。

４年生の頃

年が開けた頃、状況は大きく変わりました。その年は夏に四年に一度の世界大会が開かれる年でした。世界大会の出場の要件はかなり厳しいものでした。個人競技に関してはまた別になりますが、団体競技は以下の通りです。

• 前年の全日本大会で優勝していること。

• あるいは、前年の全国レベルの大会で入賞をしているチームで選考会を行いその中で一番になること。

四年前にも東大では女子チームがこの条件をクリアし出場を果たしていましたが、男子チームは選考会まで団体法形と展開競技が行けたものの世界大会に出場する権利は獲得できませんでした。

今年は、前年度の学生大会の優勝をもって展開競技が世界大会の選考会に参加する権利をもっていました。また、部全体を見ると女子チームは展開と団法の二競技で世界大会の出場権を獲得し、個人では僕の同期が一人法形競技の参加権を持っていました。その他にもOBOGの先輩で世界大会や選考会の出場権を獲得している先輩もいて、東大躰道部の地力が着実についていることを物語っていました。

さて、展開競技で選考会に出場する機運が高まっていた中、僕もちょっとした軽い気持ちで選考会にのぞむチームにエントリーしました。前年度のチームのメンバーであったこともあって僕はそのチームのメンバーになりました。このことが一年を通して大きく影響を残しました。

1月に始まった展開の練習は3月初旬に選考会があることもあって白熱し、かなり高負荷の練習が続きました。

例年はまだ、あまり練習頻度も多くなく週3,4の練習ペースが保たれている時期も週6,7の練習があるような状況でした。そして、選考会では幸いなことにも、出場権を獲得したのですがこのことは同じような練習が７月末の世界大会まで続くことを示していました。

www.youtube.com

忙しい季節

先にも挙げたとおり僕は三競技すべてを経験していて、そのときの部内ではかなり稀有な存在でした。すべての競技に顔を出していることを、ひとつの競技を引っ張るには不適当と判断されたのもあって、僕は競技リーダーを務めることは結局ありませんでした。競技リーダーを務めなかったことは、その分時間に余裕が出るという点ではメリットでした。というのも、この時僕は躰道に関連のない大学関連のタスクでもたくさんタスクを抱えていて、首が回っていないような状況だったからです。

忙しさのピークは６月でした。世界大会がいよいよ二ヶ月弱にせまり、練習の苛酷さは増していました。一方で、その時期にはまだ卒論の検討もしなければならない時期でしたし、一方で世界大会であまり時間が取れないことも考えると院試勉強も本格的にしなければならない時期でした。

お金の問題も有りました。僕は両親との約束で大学関係の費用はほとんど自分で出すことになっておりそのためのお金を稼ぐ必要が有りました。そのためのアルバイトも週に4回ほど入れていました。

このようなこともあって僕はすっかり参ってしまっていたのですが、そんな中頼りになったのは展開チームを全力でサポートしてくれる部員の存在でした。

もともと展開チームは競技の特性上補欠メンバーを作りづらいのもあって、世界大会の展開チームでは補欠メンバーは明確には設定していませんでしたが、その一方でサポートシステムというものを充実させていました。サポートの部員は、動画をとったり動画の分析をしたり筋を一緒に考えたりと非常に重要な役割でしたが同時に、展開の出場メンバーと同様に朝早くに練習に来なければならなかったりと大変な役でも有りました。

そのような大変な役割を文句ひとつも言わずに引き受けてくれ、そして僕が精神的に不安定になっている時期にもどっしりとした安定感をもって支えてくれたのには感謝してもしきれません。ともかく、そのような部員たちのおかげで大きな山を乗り越え、世界大会にのぞむことができたのです。

世界大会

世界大会でのライバルとなるのは同じく日本チームの山梨県チームでした。全日本大会では過去何度も優勝し、その時点で七連覇をしていました。メンバー一人ひとりの躰道歴が、こちらのチームの全員の躰道歴を足してようやく届くか届かないかという、ベテランぞろいのチームでした。

実のところ、自分たちがまだ黒帯にもなっていない頃に春合宿にも来ていただいて教えを請うたような先生もいらっしゃるようなチームで、そんなチームに挑むことができる時点で奇跡のような話でした。

そして迎えた世界大会の当日、僕達は完敗を喫しました。山梨県チームは圧倒的でした。最終得点では大きく水をあけられての敗北でした。

www.youtube.com

山梨県チームの展開です。

www.youtube.com

ここで敗北したことで、展開チームメンバーの闘志にはさらに火が付きました。その年の全日本大会でも、山梨県チームとの対戦の機会があるかもしれなかったからです。全日本大会での逆転を狙っての猛練習が始まろうとしていましたが、僕にはその前にやることが有りました。

院試、研究

世界大会の終わった7月末は、院試の1ヶ月前でもありました。勉強の進捗で大きく遅れをとっていた僕はやむを得ず部活を二週間ほど休ませていただくことになりました。その間に勉強をし、院試に後顧の憂い無く合格して全日本大会までの練習をする、ということだったのですが、もう１つ懸念事項がありました。

全日本大会のある日は卒業論文の提出の前日でもあったのです。卒業研究と全日本大会に向けた練習の両立が可能か悩んだ僕は数日の間、展開チームから降りるかどうかを真剣に検討したのち、自分が一番ふさわしいと思った後輩に展開の枠を任せる申し出をしたのですが断られてしまいました。今から思えば当然とも言えるでしょう。

ひとまず院試は突破することができたのですが、その後も様々な試練がありました。卒業研究に、学生大会の練習と休む間はありませんでした。

そう、学生大会のことはすっかり忘れてしまっていたのですが、重要な行事として確かに存在していました。自分も目標のことは覚えていたのですが、半年にも及ぶ展開の練習の末に疲れきってしまい気力も限界に達していました。それでも、実戦競技には確実に出場するという思いで練習には臨んでいて個人実戦競技にも出る気まんまんでした。一方で個人の法形競技は諦めていました。練習が足らないことも有りましたし、後輩はとても優秀でした。

最後の学生大会

もはや先に掲げた目標は達成できなくなった上でも僕は個人の実戦競技にどうしても出場をしたかったのです。そして、練習を重ねて学生大会の個人競技の出場者を決める段になって、僕は譲る気は一切ないつもりで選考に臨みました。

そして・・・、結局のところ僕は個人の出場枠を取ることができませんでした。二枠あるその出場枠の１つは同期に、もうひとつは後輩が占め、自分の夢は潰えてしまったと言えます。

それからの毎日はどこか空虚なものを感じながら、練習は続きました。それまでずっと何かの目標を追ってきた自分にとって、目標が潰えてしまった後は少しやりづらいものでした。それでも、厳しい練習は続き、自分はどこか後ろめたさを感じながらも学生大会の団体実戦競技の枠を占めました。

最後の学生大会は、部全体で見た結果は圧倒的でした。総合10連覇だけではなく、団体競技は団体実戦を除いて優勝し、部員から優秀選手賞が二人も出るという大記録でした。自分が出場していた、団体実戦競技が３位に終わってしまった原因の中には、どこか自分の熱意に欠けがあったからかもしれません。そういう意味で僕は後ろめたい気持ちでいっぱいでした。展開競技では優勝をしましたが、世界大会にまででたチームが、学生大会で負けるようなことがあってはいけないというプレッシャーもあったからか、これっぽっちも嬉しくなかったのも印象的でした。

最後の大会

こうして臨んだ最後の大会が全日本大会でした。僕は、卒論の提出の期限も近かったこともあって息も絶え絶えでした。

この時点で世界大会で優勝した山梨県チームは出場しないこともわかっていて、雪辱を晴らすという当初の目的もどこかに行っていました。ただ、世界大会で二位になったチームが負けてはならない、という呪いのような義務感だけがあり、とてもつらい期間でした。

こうして迎えた、11ヶ月にもおよぶ展開の幕引きは新潟県チームに敗北、というものでした。同じチームでやってきた後輩たちには大変なものをおしつけてしまったなあ・・・

卒部

というわけで、僕の四年間はこんな感じなわけです。自分の設定した目標に振り回されて、ずっと競争の中に身をおいているような状況でした。なので、四年間が終わった今はとても気が楽であると同時に、目標が達成できなかったことがちょっと心残りでもあります。

そんなわけでたかが胃腸炎ごときで最後の練習に参加できなかったのはとても悔しいし、なんだか物悲しさがすごくてこんな長文を書いてしまいました。卒部後はのんびり生きたいものですが、面倒事を抱え込んで苦しむのは生来の気質のような気も、最近はしてきました。躰道は続けるつもりですが、ほどほどにしておきたいものです。

部活、楽しかったなあ・・・辛かったなあ・・・楽しかったなあ・・・

カルマンフィルタについて

この記事は東京大学航空宇宙工学科/専攻 Advent Calendar2017の七日目の記事として書かれています。

adventar.org

最近流行りの自動運転とか気象予測なんかにも使われていて古くはアポロ計画にも用いられたことで有名なカルマンフィルタについてまとめてみました！・・・と言いたいところですが、半分くらい自分向けの備忘録の様相を呈しているので全然まとまっていません。あしからず。

はじめに

卒論も終わって久々にtwitterを眺めていたら同期の某うさぎさんがなにやら面白いことを初めていたので興味本位で応じてみました。

とは言ってもブログなんて一切やったことがない身なのでなにを書こうかな...
と悩んでいたら初日の記事が弊学科のとある課題についてだったのでそれに習って、一年くらい前にやったカルマンフィルタの課題を引っ張り出してきました。

基本的には詳しい理論の方に突っ込まないように(深く理解しているとは言えないので）しているので理論的にガバガバなのはお許しください。また、個人的な備忘録として実装も載せていますが効率や可読性などは一切考えておりませんのでご容赦を・・・

そもそもカルマンフィルタってなに？

カルマンフィルタとは[1]によると

カルマンフィルター (Kalman filter) は、誤差のある観測値を用いて、ある動的システムの状態を推定あるいは制御するための、無限インパルス応答フィルターの一種である。

ということのようです。
これを読んで「ああ、なるほど」となる方はこれ以降の記事はあまり参考にならないかと思います。また、実装が知りたいという方もこの先しばらくは説明が続きますので読み飛ばしてくださって構いません。

さて、個人的な理解ではカルマンフィルタとは「ベイズの定理の応用によって数理モデルと観測値から精度よくシステムの状態推定を行うプログラム」と考えています。ポイントとなるのはベイズの定理、数理モデル、状態推定、と言ったところでしょうか？

ベイズの定理

ベイズの定理とは条件付き確率に関する定理です。最近ではベイズ主義統計学が一部で流行しているようなのでご存知の方は多いかも。
この定理の特徴は確率というものとして事前確率と事後確率というものを仮定しているところでしょう。事前・事後の「事」というのは、情報の入手を表しています。すなわち情報が入ってくる前と後で確率がどのように変化するかを記述した定理がベイズの定理です。数式で表すと $$ \begin{equation} P(B|A)=\frac{ P(A|B) }{P(A)}\cdot P(B) \end{equation} $$

上の式の説明を少ししますとのように表される記号は|の右に書かれた事象が真であるときに、左側に書かれた事象も真である確率を表しています。そして、式の左辺の確率を事後確率、式の右辺のを事前確率と呼び事前確率から事後確率を求める方法を記しているということになります。
このベイズの定理を基礎に置いた統計学はベイズ主義統計と呼ばれるのですが、この統計学は高校で習うような頻度主義統計学とは大きく異なる特徴がいくつかあります。

まず、頻度主義統計学では確率的事象の裏にはなんらかの分布が存在していて、事象はその確率分布に従って発生するという思想のもとに発展しています。この思想のもとでは、仮にデータから分布を推定しようとした場合に無限点のサンプルが必要となってきます。

一方で、ベイズ主義統計学では最初に人間の主観的な尺度で分布を定めて置いてもサンプリングによってその分布の推定を修正していくことができます。ここで、分布を人間が主観的な尺度で定めると書いたのですが、やや暴論とも言えるこの仮定のためにベイズ主義統計と頻度主義統計はしばしば対立してきたそうです。一方でこの仮定によって少ないサンプリング数でも比較的精度よく推定を行うことができるため、あまり多くのサンプリングを行うことができない事例に対しても適用できるなど使い勝手がいい面も多くあります。

数理モデル

カルマンフィルタはとても強力な推定器ですが、取り扱うシステムのモデルが分かっているという前提があります。ここでいうモデルとは、状態遷移や観測の数学的表現、あるいはノイズの性質などが挙げられます。(通常の)カルマンフィルタの扱うことができるモデルにはいくつかの制約がかかっています。一つ一つ上げていきます。

1. システムは線形の動的システムであること

2. ノイズはゼロ平均のガウス分布に従い、他と無相関であること(白色ノイズであること)

3. 初期状態の事前分布は(多変量)ガウス分布であること

4. フィルタ後の分布もガウス分布に従うこと

まず1.の制約について、カルマンフィルタは状態空間表現されたシステムを扱うといいかえてしまうことができます。それ以外の表現でも適用可能かもしれない・・・
このようなシステムは以下のような数学的表現ができます(離散時間系)

$$ \begin{cases} \mathbf{x}_{t+1}=\mathbf{A}\mathbf{x}_t+\mathbf{B}\mathbf{u}_t+\mathbf{w}_t \\ \mathbf{y}_{t} =\mathbf{C}\mathbf{x}_t+\mathbf{v}_t \end{cases} $$

なお、状態方程式や観測方程式に少し操作を加えることで非線形システムへの拡張が可能であり、これらには拡張カルマンフィルタ(Extended Kalman Filter, EKF)や無香カルマンフィルタ(Unscented Kalman Filter, UKF)などの種類があります。EKFはともかくとしてUKFはあまり実装も落ちていないようなので実装も上げておこうと思います。

2.の制約に関しては、

$$ \begin{align} \mathbf{w}_t\sim N(\mathbf{0},\mathbf{Q}) \\ \mathbf{v}_t\sim N(\mathbf{0}, \mathbf{R}) \end{align} $$

と表現できます。
3.4.の制約に関しては

$$ \begin{align} p(\mathbf{x}_1)&=N(\mathbf{x}_1|\mathbf{m}_0,\mathbf{V}_0) \\ p(\mathbf{x}_t|\mathbf{y}_{1:t})&=N(\mathbf{x}_t|\mathbf{m}_t,\mathbf{V}_t) \end{align} $$

となります。この表現からも分かる通り、カルマンフィルタではベイズの定理を用いて現在の状態の分布から次の状態の分布を推定することを行います。

状態推定

カルマンフィルタは上述したベイズの定理と数理モデルを用いて状態量の推定を行うプログラムです。実装という観点から見れば関数として表現できます。現在の状態を受け取って次の状態の推定値を出力する関数です。
個人的に重要だな、と思っている点として

• ベイズの定理を用いることで事後確率分布の分散が事前確率分布の分散よりも減少すること

• 最初の事前確率分布は人間が恣意的に決めて良いということ

• モデルが分かっている必要があること

が挙げられます。1点目に関しては証明の詳細は載せませんが、この性質によりカルマンフィルタによる推定が意味のあるものになります。
2点目に関しては、初期の状態量の共分散行列は自分で設定するものだ、ということを表しています。すなわち、初期状態の共分散行列はハイパーパラメータの一種であるということです。
また、3点目は当たり前なのですが、モデルが分かっていない系に関しては適用できないということです。この場合は別の手法を用いてノイズの性質を調べたり、物理モデリングによってモデルを推測するなどしてある程度信頼できるモデルを用意しておく必要があるということを意味しています。

まとめ

ここまでの話をまとめます。カルマンフィルタとは状態量の確率分布を推定するプログラムです。推定は2つのステップからなります。それぞれ、状態の遷移に伴う分布の更新(ここで事前分布を求めます)を行うステップと、観測情報によって事前分布を修正して事後分布を求めるステップです。この事後分布がそのステップにおける推定された分布ということになります。
カルマンフィルタはこれを繰り返すことにより、次々と状態を推定して刻々と変化する状況に対応するのです。

余談ですが、カルマンフィルタのカルマンはルドルフ・エミール・カルマンであり、カルマン渦のカルマン(フォンカルマン)とは別人のようです。どちらもハンガリー出身ということなのでハンガリーではメジャーな姓なのかもしれません。

アルゴリズム

定理の用意

アルゴリズムの詳細に入る前に多変量ガウス分布の定理に関して2つ定理の用意をします。これらはアルゴリズムの中で使われカルマンゲインの決定などに重要な役割を果たす定理です。

1.多変量ガウス分布の線形結合に関する定理

2.多変量ガウス分布の条件付き分布に関する定理

1.に関しては以下のように表現できます。
状態量とその線型結合で表されるベクトルがあり両者の間には $$ \begin{equation} \mathbf{y}=\mathbf{A}\mathbf{x}+\mathbf{b}+\mathbf{v} \end{equation} $$

なる関係が成り立つとします。、であるとき

$$ \begin{equation} p(\mathbf{y})=N(\mathbf{y}|\mathbf{A}\mathbf{m}+\mathbf{b},\mathbf{A}\mathbf{P}\mathbf{A}^T+\mathbf{R}) \end{equation} $$

となるというものです。これにより線型結合結合で表された状態方程式や観測方程式を用いて状態量の確率分布の遷移を考えることができるようになります。

一方、2.に関しては以下のようなものです。ベクトルの同時確率密度分布が次式のような多変量ガウス分布に従うとします(縦棒が長くなってくれない・・・)。 $$ \begin{equation} p(\mathbf{x},\mathbf{y})=N\left(\left[\begin{matrix} \mathbf{x} \\ \mathbf{y} \end{matrix} \right]\mid \left[\begin{matrix} \mathbf{a} \\ \mathbf{b} \end{matrix} \right], \left[\begin{matrix} \mathbf{P} & \mathbf{R}\\ \mathbf{R}^T & \mathbf{Q} \end{matrix} \right]\right) \end{equation} $$

このとき、、の周辺分布は $$ \begin{align} p(\mathbf{x})=N(\mathbf{x}|\mathbf{a}, \mathbf{P}) \\ p(\mathbf{y})=N(\mathbf{y}|\mathbf{b}, \mathbf{Q}) \end{align} $$

となります。ここで一方の値が得られた時、もう一方の条件付き分布は、 $$ \begin{align} p(\mathbf{x}|\mathbf{y})=N(\mathbf{x}|\mathbf{a}+\mathbf{R}\mathbf{Q}^{-1}(\mathbf{y}-\mathbf{b}),\mathbf{P}-\mathbf{R}\mathbf{Q}^{-1}\mathbf{R}^T) \\ p(\mathbf{y}|\mathbf{x})=N(\mathbf{y}|\mathbf{b}+\mathbf{R}^T\mathbf{P}^{-1}(\mathbf{x}-\mathbf{a}),\mathbf{Q}-\mathbf{R}^T\mathbf{P}^{-1}\mathbf{R}) \end{align} $$

となります。この定理により、状態量の事前分布が分かっていて観測が得られたときに事後分布を求めることができるようになります。

さて、定理の用意ができたところでカルマンフィルタアルゴリズムの詳細に移って行きます。カルマンフィルタの目的はフィルタされた分布の再帰的な更新式、つまり入力と同じ形式の出力を得ることになります。数式を用いると

$$ \begin{align} p(\mathbf{x}_t|\mathbf{y}_{1:t})&=N(\mathbf{x}_t | \mathbf{m}_t, \mathbf{V}_t)\ \ (at\ Time\ t ) \\ p(\mathbf{x}_{t+1}|\mathbf{y}_{1:t+1}&)=N(\mathbf{x}_{t+1} | \mathbf{m}_{t+1}, \mathbf{V}_{t+1})\ \ (at\ Time\ t+1 ) \end{align} $$

のようなフィルタ分布を求めて行きます。すなわち、からを求めることが目的です。
カルマンフィルタのアルゴリズムは二つのステップからなります。

1. 次の時刻の状態をモデルを用いて予測する: (時刻tにおいて)

2.次の時刻の観測に基づき推定を更新する:

一つずつ見て行きます。

予測ステップ

まず予測ステップですが、時刻におけるフィルタ分布を以下のように仮定します。 $$ \begin{equation} p(\mathbf{x}_t|\mathbf{y}_{1:t})=N(\mathbf{x}_t|\mathbf{m}_t,\mathbf{V}_t) \end{equation} $$

ここで状態方程式は以下のように表されるのでした。 $$ \begin{cases} \mathbf{x}_{t+1}=\mathbf{A}\mathbf{x}_t+\mathbf{B}\mathbf{u}_t+\mathbf{w}_t \\ \mathbf{w}_t \sim N(\mathbf{0},\mathbf{Q}) \end{cases} $$

これに多変量ガウス分布の線形結合に関する定理を適用すると、1ステップ後の状態量の予測分布が計算されます。 $$ \begin{align} p(\mathbf{x}_{t+1}|\mathbf{y}_{1:t})&=N(\mathbf{x}_{t+1}|\mathbf{A}\mathbf{m}_t+\mathbf{B}\mathbf{u}_t, \mathbf{A}\mathbf{V}_t\mathbf{A}^T+\mathbf{Q}) \\ &=N(\mathbf{x}_{t+1}|\check{\mathbf{m}}_{t+1}, \check{\mathbf{V}}_{t+1}) \end{align} $$

ここでは予測分布に関する量を表すとします。

観測更新ステップ

続いて観測更新ステップについて説明します。
観測更新ステップにおいては前のステップにおいて計算した状態量の予測分布から状態量と観測量の予測同時分布を計算します。この計算には多変量ガウス分布の線形結合に関する定理を用います。
その後得られた観測値を用いて多変量ガウス分布の条件付き確率に関する定理からフィルタ分布を求めます。これが目的としていた分布になります。

数式を用いて追って行きます。
観測方程式は $$ \begin{cases} \mathbf{y}_{t+1}=\mathbf{C}\mathbf{x}_{t+1}+\mathbf{v}_{t+1} \\ \mathbf{v}_{t+1}\sim N(\mathbf{0}, \mathbf{R}) \end{cases} $$

これを状態量の予測分布に適用します。このときであったことより $$ \begin{equation} p(\mathbf{x}_{t+1}, \mathbf{y}_{t+1}|\mathbf{y}_{1:t})=N\left(\left[\begin{matrix} \mathbf{x}_{t+1} \\ \mathbf{y}_{t+1} \end{matrix}\right] \mid \left[\begin{matrix} \check{\mathbf{m}}_{t+1} \\ \mathbf{C}\check{\mathbf{m}}_{t+1} \end{matrix}\right], \left[\begin{matrix} \check{\mathbf{V}}_{t+1} & \check{\mathbf{V}}_{t+1}\mathbf{C}^T \\ \mathbf{C}\check{\mathbf{V}}_{t+1} & \mathbf{C}\check{\mathbf{V}}_{t+1}\mathbf{C}^T+\mathbf{R} \end{matrix}\right] \right) \end{equation} $$

こうして得られた予測同時分布と実際の観測値を用いてフィルタ分布は以下のように求められます。

$$ \begin{align} p(\mathbf{x}_{t+1}|\mathbf{y}_{t+1}, \mathbf{y}_{1:t})&=p(\mathbf{x}_{t+1}|\mathbf{y}_{1:t+1}) \\ &=N(\mathbf{x}_{t+1}|\check{\mathbf{m}_{t+1}}+\mathbf{K}_{t+1}(\mathbf{y}_{t+1}-\mathbf{C}\check{\mathbf{m}}_{t+1}),(\mathbf{I}-\mathbf{K}_{t+1}\mathbf{C})\check{V}_{t+1}) \\ \mathbf{K}_{t+1}&=\check{\mathbf{V}}_{t+1}\mathbf{C}^T(\mathbf{C}\check{\mathbf{V}}_{t+1}\mathbf{C}^T+\mathbf{R})^{-1} \end{align} $$

ここで出てきたはカルマンゲインと呼ばれ、観測値と予測値のどちらをより信頼するかという重みのようなものになります。

まとめ

これでカルマンフィルタアルゴリズムはおしまいです。あとは繰り返してステップを更新していくだけです。

実装

これまでの流れをひとまずPythonで実装して見ました。ちなみに弊学科の標準語はPythonです。for文が遅いとか色々ありますが僕は好きです。

問題設定

XY平面上で]に原点を出発し、45度方向に秒速]で移動している物体があるとします。この移動には外乱による影響があるとします。また、1秒ごとに位置座標の観測があるとします。この観測にもノイズが含まれているとします。この物体の位置をカルマンフィルタを用いて推定して見ます。

学科の人が見たら、これは・・・となってしまいそうな問題設定ですが気にしないで行きます。

系の設定

さて、全てをシミュレーションで行うとなると真値系と推定系を設定しなければなりません。真値系は真値を計算しつつ誤差の乗った観測値を吐き出します。推定系は誤差の乗った観測を受け取りながら推定値を計算し続けます。まずは真値系から。
なおモデルの更新式に関しては、カルマンフィルタのものと同一なので関数化します。

import numpy as np
import numpy.random as random
import matplotlib.pyplot as plt

# 関数
def model(x, A, B, u):
    v = 2.0
    return np.dot(A, x) + B * v

def true(x):
    noise = random.normal(0.0, 1.0, (2, 1))
    return x + noise

def system(x):
    true_val = true(model(x))
    obs_val = true(true_val)
    return true_val, obs_val

続いて推定系を検討します。カルマンフィルタは実装としては関数で表現できます。

def Kalman_Filter(m, V, y, A, B, u, Q, R):
    # 予測
    m_est = model(x, A, B, u)
    V_est = np.dot(np.dot(A, V), A.transpose()) + Q
    
    # 観測更新
    K = np.dot(V_est, np.linalg.inv(V_est + R))
    m_next = m_est + np.dot(K, (y - m_est))
    V_next = np.dot((np.identity(2) - K), V_est)
    
    return m_next, V_next

最後に状態量などを定義して与えてやります。

if __name__ == "__main__":
    A = np.identity(2) # 状態方程式のA
    B = np.ones((2, 1)) # 状態方程式のB
    u = 2.0 # 速度
    x = np.zeros((2, 1)) # 真値
    m = np.zeros((2, 1)) # 推定値
    V = np.identity(2) # 推定値の初期共分散行列(勝手に設定して良い)
    Q = 2 * np.identity(2) # 入力誤差の共分散行列(今回はtrue()の中でnoiseの分散を2.0に設定したので)
    R = 2 * np.identity(2) # 上と同じ
    
    # 記録用
    rec = np.empty((4, 30))
    
    # main loop
    for i in range(30):
        x, y = system(x, A, B, u)
        m, V = Kalman_Filter(m, V, y, A, B, u, Q, R)
        
        rec[0, i] = x[0]
        rec[1, i] = x[1]
        rec[2, i] = m[0]
        rec[3, i] = m[1]
        
    # 描画
    plt.plot(rec[0, :], rec[1, :], color="blue", marker="o", label="true")
    plt.plot(rec[2, :], rec[3, :], color="red", marker="^", label="estimated")
    plt.legend()
    plt.show()

この結果は以下の図のようになります。

グラフがかなりうねっていますが追従できているのがわかります。
なお、上の実装では観測方程式がない(状態がそのまま観測になる)ようになっているのでご注意ください。観測方程式がある場合は観測更新ステップが

K = np.dot(V_est, np.dot(C.transpose(), np.linalg.inv(np.dot(C, np.dot(V_est, C.transpose())) + R)))
m_next = m_est + np.dot(K, (y - np.dot(C, m_est)))
V_next = np.dot((np.identity(2) - np.dot(K, C)), V_est)

となります。ドット積をnp.dotで書かなければならないのがnumpyの悪いところですね・・・。Python3系では@演算子でドット積を表現できるようなので後方互換性を気にしないならその方がいいでしょう。

拡張カルマンフィルタ

カルマンフィルタを非線形システムを扱えるように拡張したのが拡張カルマンフィルタです。EKFとか呼ばれます。
拡張はとても簡単で状態方程式のや観測方程式のを非線形関数のヤコビアンに変えるだけです。
数式で表すと、 $$ \begin{align} \mathbf{x}_{t+1}&=f(\mathbf{x}_t, \mathbf{u}_t) + \mathbf{w}_t \\ \mathbf{y}_t &= h(\mathbf{x}_t)+\mathbf{v}_t \\ \mathbf{A}_t&=\frac{\partial f(\mathbf{x}, \mathbf{u})}{\partial \mathbf{x}}\mid_{\mathbf{x}=\mathbf{m}_t, \mathbf{u}_t} \\ \mathbf{C}_t&=\frac{\partial h(\mathbf{x})}{\partial \mathbf{x}}\mid_{\mathbf{x}=\check{\mathbf{m}}_{t+1}} \end{align} $$

EKFの例題

EKFを使った推定を行って見ます。
2次元平面を等速円運動「しようと」している物体があるとします。この物体には外乱が加わっているとします。
原点にいる観測者からは、各時刻で、物体への距離と方位角が計測できるとします。この観測には観測ノイズが加わります。
円運動の半径は]とし、角速度は、物体位置に対するノイズの分散は、物体への距離の観測のノイズの分散は、物体の方位角の分散はとします。

実装

線形問題の時と同様にしてまずは真値系の実装をします。

状態方程式は $$ \begin{equation} \left[ \begin{matrix} x_{1, t} \\ x_{2, t} \end{matrix}\right] = \left[\begin{matrix} x_{1, t-1} \\ x_{2, t-1} \end{matrix}\right] + \left[\begin{matrix} -L\omega\sin\omega t \\ L\omega\cos\omega t \end{matrix}\right] + \mathbf{w}_t \end{equation} $$ また、観測方程式は $$ \begin{equation} \left[\begin{matrix} y_{1, t} \\ y_{2, t} \end{matrix}\right] = \left[\begin{matrix} \sqrt{x^2_{1, t}+x^2_{2, t}} \\ \tan^{-1}\frac{x_{2,t}}{x_{1, t}} \end{matrix}\right] + \mathbf{v}_t \end{equation} $$

です。

import numpy as np
import numpy.random as random
import matplotlib.pyplot as plt

# 関数
def state_eq(x, L, omega, t):
    x_new = x + np.array([-L*omega*np.sin(omega*t),
                          L*omega*np.cos(omega*t)]).reshape([2, 1])
    return x_new
    
def obs_eq(x, obs_noise_y1, obs_noise_y2):
    x1 = x[0]
    x2 = x[1]
    y = np.array([np.sqrt(x1**2 + x2**2),
                  np.arctan(x2/x1)]).reshape([2, 1])
    noise = np.array([random.normal(0, obs_noise_y1), 
                      random.normal(0, obs_noise_y2)]).reshape([2, 1])
    return y + noise
    
def obs_eq_noiseless(x):
    x1 = x[0]
    x2 = x[1]
    y = np.array([np.sqrt(x1**2 + x2**2),
                  np.arctan(x2/x1)]).reshape([2, 1])
    return y
    
def true(x, input_noise):
    noise = random.normal(0, input_noise, (2, 1))
    return x + noise

冗長なコードが見えていますが、目を瞑ってください。
続いて推定系。推定系では状態方程式のヤコビアンが必要になってきます。

ヤコビアンは、状態方程式に関しては $$ \begin{equation} \frac{\partial f(\mathbf{x}, \mathbf{u})}{\partial \mathbf{x}}\mid_{\mathbf{x}=\mathbf{m}_t, \mathbf{u}_t}= \left[ \begin{matrix} 1 & 0 \\ 0 & 1 \end{matrix}\right] \end{equation} $$

観測方程式に関しては、 $$ \begin{equation} \frac{\partial h(\mathbf{x})}{\partial \mathbf{x}}\mid_{\mathbf{x}=\check{\mathbf{m}}_{t+1}}= \left[ \begin{matrix} \frac{ x_{1,t} }{ \sqrt{ x^2_{1,t} + x^2_{2,t} } } & \frac{ x_{2,t} }{ \sqrt{ x^2_{1,t} + x^2_{2,t} } } \\ -\frac{ x_{2,t} }{ x^2_{1,t} + x^2_{2,t} } & \frac{ x_{1,t} }{ x^2_{1,t} + x^2_{2,t} } \end{matrix} \right] \end{equation} $$

これとEKFの実装をすると

def state_jacobian():
    jacobian = np.identity(2)
    return jacobian
    
def obs_jacobian(x):
    jacobian = np.empty((2, 2))
    jacobian[0][0] = x[0] / np.sqrt(x[0]**2 + x[1]**2)
    jacobian[0][1] = x[1] / np.sqrt(x[0]**2 + x[1]**2)
    jacobian[1][0] = -x[1] / (x[0]**2 + x[1]**2)
    jacobian[1][1] = x[0] / (x[0]**2 + x[1]**2)
    
    return jacobian
    
def system(x, L, omega, t, input_noise, obs_noise_y1, obs_noise_y2):
    true_state = true(state_eq(x, L, omega, t), input_noise)
    obs = obs_eq(true_state, obs_noise_y1, obs_noise_y2)
    return true_state, obs
    
def EKF(m, V, y, Q, R, L, omega, t):
    # 予測ステップ
    m_est = state_eq(m, L, omega, t)
    A = state_jacobian()
    V_est = np.dot(np.dot(A, V), A.transpose()) + Q
    
    # 観測更新ステップ
    C = obs_jacobian(m_est)
    temp = np.dot(np.dot(C, V_est), C.transpose()) + R
    K = np.dot(np.dot(V_est, C.transpose()), np.linalg.inv(temp))
    m_next = m_est + np.dot(K,(y - obs_eq_noiseless(m_est)))
    V_next = np.dot(np.identity(V_est.shape[0]) - np.dot(K, C), V_est)
    
    return m_next, V_next

最後に状態量などの設定を行って完了です。

if __name__ == "__main__":
    x = np.array([100, 0]).reshape([2, 1])
    L = 100
    omega = np.pi / 10
    input_noise = 1.0**2
    obs_noise_y1 = 10.0**2
    obs_noise_y2 = (5.0 * np.pi / 180)**2
    m = np.array([100, 0]).reshape([2, 1])
    t = 0.0
    dt = 1.0
    V = np.identity(2) * 1.0**2
    Q = np.identity(2) * input_noise
    R = np.array([[obs_noise_y1, 0],
                  [0, obs_noise_y2]])
                  
    # 記録用
    rec = np.empty([4, 21])
    
    for i in range(21):
        rec[0, i] = x[0]
        rec[1, i] = x[1]
        rec[2, i] = m[0]
        rec[3, i] = m[1]
        
        x, y = system(x, L, omega, t, input_noise, obs_noise_y1, obs_noise_y2)
        m, V = EKF(m, V, y, Q, R, L, omega, t)
        
        t += dt
        
    plt.plot(rec[0, :], rec[1, :], color="blue", marker="o", label="true")
    plt.plot(rec[2, :], rec[3, :], color="red", marker="^", label="estimated")
    plt.legend()
    plt.show()

この結果は以下の図のようになります。

ノイズの元でも比較的正確に推定ができていそうです。

無香カルマンフィルタ

最後に紹介するのが、EKFと同じく非線形に拡張されたカルマンフィルタである、無香カルマンフィルタ(Unscented Kalman Filter, UKF)です。
UKFではヤコビアンを計算してガウス分布の線形結合による変化を考察する代わりに、シグマ点と呼ばれる代表点をガウス分布から抽出し、その点群を状態方程式や観測方程式を直接通すことによってガウス分布の変形を検討します。

具体的な理論に関しては、割愛させていただきますが、[2]などに詳しく載っています。
卒論で使ったので実装の備忘録としてこちらに載せさせていただきます。

# UKF Parameters
ukf_lambda = 10.0  # UKFのλパラメータ
ukf_kappa = 0.1  # UKFのκパラメータ
ukf_alpha2 = (2.0 + ukf_lambda)/(2.0 + ukf_kappa)  # UKFのα^2パラメータ
ukf_w0_m = ukf_lambda/(2.0 + ukf_lambda)  # UKFの重みパラメータ
ukf_w0_c = ukf_w0_m + (1.0 - ukf_alpha2 + 2.0)  # UKFの重みパラメータ
ukf_wi = 1.0/(2.0*(2.0 + ukf_lambda))  # UKFの重みパラメータ
ukf_wm = np.zeros([5, 1])  # UKFの重みパラメータw_m
ukf_wc = np.zeros([5, 1])  # UKFの重みパラメータw_c
ukf_gamma = np.sqrt(3.0 + ukf_lambda)  # γ = √(n + λ)

# UKFの重みパラメータの設定
ukf_wm[0] = ukf_w0_m
ukf_wc[0] = ukf_w0_c
for i in range(1, 5):
    ukf_wm[i] = ukf_wi
    ukf_wc[i] = ukf_wi

def gen_sigma_point(mu, sigma):
    """
    Function to generate Matrix chi which is the set of the sigma point Vector.

    arguments :
    mu    : Vector of the mean values.
    sigma : Correlation Matrix.

    return :
    chi   : Matrix chi.
    """
    n = len(mu)  # 次元
    chi = np.zeros([n, 2*n+1])  # χ行列
    root_sigma = np.linalg.cholesky(sigma)  # √Σの計算

    chi[:, 0] = mu[:, 0]
    chi[:, 1:1+n] = mu + ukf_gamma*root_sigma
    chi[:, 1+n:2*n+1] = mu - ukf_gamma*root_sigma

    return chi
    
def gen_correlation_mat(mu, chi, R):
    """
    Function to describe the change of the correlation matrix.

    arguments :
    mu    : Vector of the mean values.
    chi   : Destribution of the sigma point.
    R     : Correlation Matrix of the observation noise.

    return :
    sigma_bar : Correlation Matrix after the change.
    """
    sigma_bar = R

    for i in range(chi.shape[1]):
        x = chi[:, i].reshape((chi.shape[0], 1)) - mu
        sigma_bar = sigma_bar + ukf_wc[i]*(np.dot(x, x.T))

    return sigma_bar
    
def UKF(mu, sigma, u, z, state_func, obs_func, R, Q, dt):
    """
    Unscented Kalman Filter Program.

    arguments :
    mu         : Vector which is the set of state values.
    sigma      : Correlation Matrix of the state values.
    u          : Control inputs.Vector/Float.
    z          : Observed values.
    state_func : Function which describe how the state changes.
    obs_func   : Function to get observed values from state values.
    R          : Correlation Matrix of the noise of the state change.
    Q          : Correlation Matrix of the observation noise.
    dt         : Time span of the integration.

    returns :
    mu    : Vector which is the set of state values.
    sigma : Correlation Matrix of the state values.
    ---------------
    """
    # Σ点生成
    chi = gen_sigma_point(mu, sigma)  # χ
    

    # Σ点の遷移
    chi_star = state_func(u, chi, dt)

    # 事前分布取得
    mu_bar = np.dot(chi_star, ukf_wm)  # 平均の事前分布
    sigma_bar = gen_correlation_mat(mu_bar, chi_star, R)  # 共分散行列の事前分布

    # Σ点の事前分布
    chi_bar = gen_sigma_point(mu_bar, sigma_bar)

    # 観測の予測分布
    z_bar = obs_func(chi_bar)

    # 観測の予測値
    z_hat = np.dot(z_bar, ukf_wm)

    # 観測の予測共分散行列
    S = gen_correlation_mat(z_hat, z_bar, Q)

    # Σ_xzの計算
    sigma_xz = np.zeros([mu.shape[0], z.shape[0]])
    for i in range(chi_bar.shape[1]):
        x_ = chi_bar[:, i] - mu_bar[:, 0]
        z_ = z_bar[:, i] - z_hat[:, 0]
        x_ = x_.reshape((x_.shape[0], 1))
        z_ = z_.reshape((z_.shape[0], 1))
        sigma_xz = sigma_xz + ukf_wc[i]*(np.dot(x_, z_.T))

    # カルマンゲインの計算
    K = np.dot(sigma_xz, np.linalg.inv(S))

    # 戻り値の計算
    mu = mu_bar + np.dot(K, (z - z_hat))
    sigma = sigma_bar - np.dot(np.dot(K, S), K.T)

    return mu, sigma

実際に使う際には問題に合わせてシグマ点の遷移を記述する関数の用意が必要です。
今回は先ほどのEKFの例をUKFでも推定して見ましょう。状態方程式と観測方程式を記述した関数を用意します。

def state_func(u, chi, dt):
    L = 100
    omega = np.pi / 10
    return chi + np.array([[-L*omega*np.sin(omega*u)],
                           [L*omega*np.cos(omega*u)]])
                           
def obs_func(chi_bar):
    obs = np.zeros_like(chi_bar)
    for i in range(chi_bar.shape[1]):
        obs[0][i] = np.sqrt(chi_bar[0, i]**2+chi_bar[1, i]**2)
        obs[1][i] = np.arctan(chi_bar[1, i] / chi_bar[0, i])
        
    return obs

これを用いて推定をしてみると・・・

このようになります。この例ではEKFの方が性能が良さそうですね・・・。

実際にところ、UKFによる推定はEKFより計算量が多いものの精度は高くなることが多いようです。
一方で、ノイズが強かったりすると共分散行列が正定値行列ではなくなってコレスキー分解ができなくなり推定が止まってしまうようなことも起きます。 今回の問題設定では、状態方程式や観測方程式があまり性質の良いものではなく、ノイズも強いためあまり推定がうまくいっていないようです。

出典

[1] Wikipedia. カルマンフィルター. Retrieved December 7, 2017, from, https://ja.wikipedia.org/wiki/カルマンフィルター [2]確率ロボティクス. Sebastian Thrun, Wolfram Burgard, Dieter Fox. 上田隆一訳. マイナビ出版, 2015. ISBN 4839952981, 9784839952983