ちょいちょいベイズの定理との関係性や カルマンゲインの導出・証明を忘れてしまうのでメモ.

仮定するシステム


\begin{align} x[k+1] &=f(x[k],u[k],k) + v
y[k] &= h(x[k]) + w \end{align} \]

ってのを仮定. ここで$x,y$はスカラーor縦ベクトルならOK.もちろんランクは加味して下さい. $k\in\mathbb{Z}^+$はステップで,自然数(or0を含む)値を取るスカラーです. $v,w$はともに正方行列で,それぞれ$x,y$と同じサイズをもちます.

ここでモデルにおける誤差$v,w$に正規性を仮定しましょう. $v,w$の平均はどちらも0,分散はそれぞれ$Q,R$で正方行列になり, 後者は一般には共分散行列と呼ばれます. ちなみに独立性を仮定するため,対角成分にしか値が入りません. 要は$Q=diag(v_1,v_2,\dots,v_n)$ですね.

とまあシステムは備忘録でもなんでもなく, いつも考えてる話なのでサラッと流すとしましょう.

カルマンゲインの導出

それぞれのの各ステップにおける平均と分散も導出できるとして, カルマンゲインの導出がいまいちわかりづらい. ってどういうことやねんと.

で,カルマン先生の原論文読んでたらまとめたくなったのでテスト.

ってなるが計算できるんじゃね?って思想らしい. 1960年ってマジかよこれ.ルーエンバーガー型オブザーバより前なんだぞこれ.

ここで独立性の仮定から,の誤差との誤差は直行するので,推定対象の推定値に対して

ここでによる相互相関行列と自己相関行列

と定義してあげると,ラストの式はとなって, がめでたく導けました.

でもちょっとまって,独立性の仮定から導けるだけじゃ, ベイズの定理とか最尤かは言えなくなーい?と思ったり.

ちなみにWikipediaに乗ってることは,カルマンの原論文内の定理3に載ってました. この頃はまだ状態変数のトレース最小化を満たすかは,わかってなかったようですが. というよりもカルマゲインを分散最小化の意味で解析解から厳密に計算しようとしたら, 一致しちゃったパターンなんだろう(適当

片山先生はリカッチ方程式で証明してるし,うーん,ここはまた別の資料に譲りましょう.

最尤であること

いや最尤なのは問題なかった.正規分布の場合,

って条件がそのまんま最尤でしたね.

ベイズの定理との関係性

そもそもベイズの定理と言えるってのは足立先生の教科書にもあるので過信することにして, ベイズの定理は,

結果的にこれが言えてますが,調べた限りではベイズの定理から を導くものは見つけられませんでした. カルマンの原論文には,白色雑音,エルゴード性や直交性に関する記述が見られたましたが, ベイズという単語は載っていなかったです.

もちろん本人は知っていたかもしれないし,後々の人が見つけた可能性もありますが・・・ どちらにせよ,最小二乗解が分散のトレース最小化となることや, そもそもの期待値とパラメータの関係性などは,ベイズの定理でも説明がつくので, 勉強が必要そうです.


質問があったのでメモ

ヨー軸方向へのカルマンフィルタ実装方法です. ピッチ・ロール方向は加速度センサで重力加速度の方向を検出すれば 絶対角の計算が可能ですが, ヨー軸は重力加速度が検出できませんので 絶対角を地磁気(デジタルコンパス)から計算します.

しかしデジタルコンパスの追従性能が遅いので, 早いダイナミクスはジャイロセンサによる角速度の情報でカバーします.

毎度のことですが,間違いがあるかもしれません,あしからず

状態空間表現

まずはモデルを立てましょう. 角度を,角速度をとします. 単位はdegでもradでも統一していればOKです.

角速度と角度の関係はオイラー微分で

ですね.ここで, はサンプリング周期[sec],はステップ数です.

状態空間表現だと

となりますが, ここでとおけますね.

カルマンフィルタに適用するモデル

カルマンフィルタで考えるモデルは,

です. は,平均値が0,分散をそれぞれと仮定します.

実際ここでのは観測ノイズを表すので,センサの精度・分解能の2乗した値を設定します. 今回はデジタルコンパスの精度or分解能の大きい方の値を入れるといいかと.

は更新ノイズとか言われ,運動方程式に関する誤差の2乗値が入ります. 今回はジャイロセンサの精度or分解能の2乗値ですが, 厳密にはオイラー微分の誤差も含むので,もう少し大きい値になりますね. オイラー微分の定式化は誤差の大きい離散化なので, 本当に使う場合はもう少し制度の良い離散化を使う必要があります.

カルマンフィルタの手順

コンパスの角度データをcompass_yaw, ジャイロセンサの角速度データをjyro_yawとします.

void kf(double xhat,double px,double stp,double compass_yaw,double jyro_yaw){
  double q = 1, r = 1; //Q,R
  double a = 1, b = stp, c = 1; //A,b.C

  double x_ = a * xhat + b * jyro_yaw; // prior estimation of value
  double p1 = a * px * a + q ; // prior coveriance
  double p2 = c * p1 * c + r ; // posterior coveriance

  double kg = p1 * c / p2 ; // Kalman gain
  double e = compass_yaw - c * x_ ; //error between model & compass

  xhat = xhat + kg * e ; // estimating value for next step
  px =   ( 1 - kg * e ) * px ; // estimating coveriance for next step
}

xhat,pxはそれぞれ推定される状態変数とその分散になります. 宣言時には,xhatには絶対角の初期値を入れ,pxと同値を入れるか, 何回か実験してpxの収束先の値を得て,それを使うと良いと思います.

以上駆け足でしたがまとめてみました. てかwikipediaのまとめがよくまとまってるのでそれでいいです.


あれ?ブログまた変わった??

と思われた方は正解です. ドメインは一緒ですが,テンプレートを一新し,ブログのビルドエンジンもHugo→Jekyllに変更しました.

Hugoは,Go言語ベースの静的サイト生成ツールですが,Jekyll(ジキル)は,Rubyベースとなっており,比較的利用者の多いツールです.(参考:StaticGen)

もちろん導入にいたっては,東工大の某氏たちはじめ,多数のロボットやプログラムに携わる人が使っていたことも決め手になっています.

なんでJekyll??

Hugoでも良かったんですよ.Markdownという記法で手軽に記事が書けるし,Gitベースのクライアントで更新できるので,ローカルでデータ更新してプッシュするだけだし,問題はなかったんですが…

あるとき,プラグインでグラフやツリーを描写できることを知り,いろいろ調べていくうちに,現行で比較的プラグインが豊富なのがJekyllだったので,最終的にJekyllにすることになりました.(また変わるかも知れませんが(^_^;)

ブロック線図を書くには?

現状のツール

Jekyll向けプラグインとして,システムの流れを再現可能なツールは,MermaidGraphvizblockdiagなんかがあります.パッケージの上下関係がわからないので,細かい点は省略.

しかし,これらのツールは,ノードtoノードが基本となっており,頑張ってブロック線図を書こうとしても,

blockdiag blockdiag { u -> F -> y,G ; G -> F [folded] ; } u F y G

こんな感じになってしまう…

Tikz(Texパッケージ)

やっぱり,制御やってる人は,

とまあこんな感じでいきたいですよね.

こんなときはTikz(「てぃくす」と読むらしい).Texのパッケージとして使えて,ブロック線図も上の通り.Jekyllならプラグインもあるので,そこまで難しくなく扱えます.もちろん,Texがコンパイルできるパソコン前提での話ですよ.内部でTexコンパイルして,生成したsvgファイルを自動リンクしてくれるのでかなり便利です.

企み

「おいおい,Tex使うってことは,いまのCIツールじゃ難しいってことじゃないか.Gitプッシュからビルドサーバだけで完結できねえのか.」と思い,Dockerに環境を構築してみました.とビルドは出来たのですが,NetlifyにプッシュするためにSSHキーを保存せねばならず,外部のビルドサーバ経由で行うのは断念.現在サーバ検討中なので,近いうちには自前のビルドサーバで,プッシュからデプロイまで自動化しているかもしれません?

環境構築

私はMacユーザーなので,Windowsでは検証していません(が,入れるパッケージはそこまで変わらないと思います).自宅ビルド用にUbuntuでも環境を再構築したので,載せておきます.

  • Mac
    1. Texが動く環境を構築(MacTexなど)
    2. Tikzのサンプルなどを動作させる
    3. brew install pdf2svgなどして,pdf2svgをインストール
    4. (怪しい感じの)Jekyll-tikzを入手し,Jekyll用ディレクトリ内の_pluginにコピペ.
  • Ubuntu
    sudo apt-add-repository ppa:texlive-backports/ppa
    sudo apt-get install texlive texlive-lang-cjk ruby ruby ruby-dev make gcc nodejs wget
    sudo gem install jekyll bundle
    jekyll new tikz-test-site
    cd tikz-test-site
    sudo bundle install
    wget https://gist.githubusercontent.com/hack-ghost/6139753/raw/fb37dd05a5e0a4e8daf36d002c8f8dcd8072f0ca/jekyll-tikz.rb
    mv jekyll-tikz.rb _plugins/jekyll-tikz.rb
    
    • Graphvizとblockdiag使いたい人は,
      sudo apt-get python python-pip graphviz
      sudo pip install actdiag blockdiag nwdiag seqdiag
      gem install jekyll-diagrams
      
    • さらに,_config.yml内のgemにjekyll-diagramsと書き,Gemfile内に以下を書いて下さい.
      group :jekyll_plugins do
       gem "jekyll-diagrams"
      end
      


今回はここまで.ぽいちゃ


%3 S S A A S->A a B B S->B b D D A->D c B->D d C C
blockdiag blockdiag { A -> B -> C -> D; A -> E -> F -> G; } A B E C D F G

memo


このブログを作って

もう運用開始からおよそ3週間.いまだに大々的に広めていないのは,過去ブログからのデータをどう移行するか悩んでいるため. MovavleTypeなので,imageのリンクは後で変えるにしても,スクリプトでも書いてMarkdownファイル(md)にしてしまえば問題ないとは思うが...

ここのサイトは将来的な目標も見据えて,bitbucket→netlify(コンパイルサーバ)で公開しており,サイトはHugoで生成している. なぜかmd形式だと最初の挨拶とか最後のコメントを忘れがち. 最近は論文ばかり書いていたので,あまりにも淡々としすぎている. ですます調でブログを書くことが多少信念でもあったのだが...

Dato-CMS

本ブログは,先も述べたとおりHugoで生成された静的webサイトである. つまりstaticなので,時々刻々とかわる情報(ツイッターとかサイトカウンタ)には対応していない. いや頑張れば反映できないこともないが,まだjavascriptがよく扱えないのと,意外とこのシンプルな見た目を気に入っている.

しかし記事内容は,個々のmdで管理せねばならず,いちいちbitbucketを開いてコミットするのもどこかばかばかしい. Dato-CMSは,こうした静的webサイトに公開される記事内容を管理してくれるサービスで,なかなか使いやすそうである. とはいえまだまだユーザーが少ないのか,日本語サイトが上がらない.

私はといえば,使用方法がなんとなくわかったが,テキストにエクスポート出来ない点を嫌って,Dato-CMSで書くのはやめることにした.

つれづれ

最近は,Pythonをいじりはじめ,JavaScriptを学び,もとい回路などを作ってプログラムなどをして生きています. fc2より広告は出ないし,サイトも見やすくなったので,しばらくはこっちで書いていきます. Markdownならオフラインでもサイト記事のストックがしやすいので,意外とこのスタイルはあっているかも. ではまた