前回トラ技を参考に作ったM5stickCを用いた傾斜計ではカルマン・フィルタの効能を堪能しただけで、そのカルマン・フィルタが何をしているかはすっ飛ばして見てみぬフリをしておりました。アルゴリズムを少し解きほぐし何をしているのかをみてみたいと思います。内容がかなり難しくここでも様々な点をすっ飛ばさなくてはなりません。カルマン・フィルタが制御工学や統計学など様々な基礎学問の上に成り立っているからです。気にしてはいられません。この道の目的はM5stickCを用いたオリジナルの倒立振子の実現です。今から統計や制御理論を1から学ぶのは得策ではありません。漫画家見習いが鉛筆の素材の木を植えるところから始めるわけにはいかないのですから。初期設定おわりに傾斜計コード前回のコードを改めて記載いたします。update_theta()を解きほぐします。カルマン・フィルタの概要カルマン・フィルタは”センサの測定値”と”モデルから算出した予測値”を元に真値を推定するフィルタです。下は傾斜計の場合のブロック図です。実際どういった処理を施して傾斜角の推定値を得ているのかコードより解きほぐしてみましょう。フィルタに導入する測定値と予測値についてみていきましょう。使用して傾斜角と角速度を測ります。傾斜角θは加速度センサから重力加速度の傾きを検出して導出します。角速度はジャイロセンサの値(x軸)をそのまま使用します。予測値を得るために物理現象を記述する式(数理モデル)を立てます。傾斜角の関係式は以下の通りです。$は時刻kのときの角速度です。$w_{offset}$は角速度のオフセット誤差です。k+1)の傾斜角$θ_{k+1}$が得られるということを示しています。ここで簡単のため角速度(ジャイロセンサ)のオフセットは時刻によらず一定と仮定します。移り変わりを示すのが状態方程式です。上記2式より以下で表せます。\right)ω_k$$一般式では以下のように書きます。実際に観測できる状態(値)を示す行列式です。\right)$$$θ_{k}$だけがセンサから直接得られることを示しています。$ω_{offset}$だけを得ることはできませんのでね。一般式は以下で書きます。$$y_k=C・x_k$$以上で数理モデルが用意できました。Cは時間依存のない定数でした。アルゴリズムupdate_theta()が何をやっているかをみてみます。ここではコード上でどんな処理をしているかをみるのが目的です。ここでは丸飲みします。update_theta()は以下の5つの処理を実行しています。x_k$を求める際の重みづけ配分比を意味します。詳細は分かりませんが$G_k$は以下の式で得られます。\right)$の転置。\right)$$W$:観測雑音の程度を表す量。\right)$で表されます。要するにデータをxとyの変動の相関を表します。物理状態$θ_k$の共分散行列$P_k$は以下で表されます。例によってなんでかは知りません。x_{k+1}$を算出します。u_k$:入力(角速度)の平均。ここでは現時刻の角速度測定値$ω_k$で近似。次時刻の共分散行列算出これもよくわかりませんが次時刻k+1の共分散行列は以下で得られます。算出)$U$:入力雑音の程度を表す量。角速度の分散(theta_dot_variance)カルマン・フィルタの初期設定カルマン・フィルタの処理は前回算出した予測値と共分散行列を用いて推定を行います。常に前のデータを必要とするので最初に起動(k=0)する際には手動で初期値を指定する必要があります。算出する次時刻の予測値が時刻0の時点ではないのであらかじめそれらしい値を決める必要があるということです。コードでは初期の傾斜は直立であると条件付けして初期値を与えています。動かしていないと仮定して$ω_{offset}$をジャイロの平均値としています。\right)$角度のばらつきは1°程度と想定し、$ω_{offset}$のばらつきはジャイロの測定値のばらつきとしています。傾斜角と$ω_{offset}$に相関はないので対角の値は0です。初期値の値は多少ずれがあっても時刻が進み測定データが増えると推定値は真値付近に収束するようです。アルゴリズムの処理内容を追いかけました。数式などは分からないことが多々ありますが、確かにその処理をしていい感じの結果を得ているのです。アルゴリズムを飲み込んで数理モデル方程式を立てて初期値の設定をそれらしく実施すればよいとのことなので応用が利くことでしょう。次は倒立振子に向けて動きたいと思います。コメントをキャンセルメールアドレスが公開されることはありません。コメントのみでもOKです。コメントデータの処理方法の詳細はこちらをご覧ください。