傾斜計 カルマン・フィルタ アルゴリズムの解きほぐし ー倒立振子への道 2ー

Home > 電子工作 > 傾斜計 カルマン・フィルタ アルゴリズムの解きほぐし ー倒立振子への道 2ー
すき 0
うんこ 0

前回トラ技を参考に作ったM5stickCを用いた傾斜計ではカルマン・フィルタの効能を堪能しただけで、そのカルマン・フィルタが何をしているかはすっ飛ばして見てみぬフリをしておりました。

カルマン・フィルタで M5stickC 傾斜計 ー倒立振子への道 1ー

ここでは前回用いたコードのカルマン・フィルタ アルゴリズムを少し解きほぐし何をしているのかをみてみたいと思います。

しかし内容がかなり難しくここでも様々な点をすっ飛ばさなくてはなりません。カルマン・フィルタが制御工学や統計学など様々な基礎学問の上に成り立っているからです。

でも気にしてはいられません。この道の目的はM5stickCを用いたオリジナルの倒立振子の実現です。今から統計や制御理論を1から学ぶのは得策ではありません。挫折するに決まっています(お父ちゃんの場合)。

漫画家見習いが鉛筆の素材の木を植えるところから始めるわけにはいかないのですから。

 

 

傾斜計コード

前回のコードを改めて記載いたします。

トラ技のソースコードInclinometer.cppをほぼ丸々参考にしています(p. 49 – 52)。

ここではカルマン・フィルタ アルゴリズム処理 update_theta()を解きほぐします。

カルマン・フィルタの概要

カルマン・フィルタは”センサの測定値”と”モデルから算出した予測値”を元に真値を推定するフィルタです。下は傾斜計の場合のブロック図です。

実際どういった処理を施して傾斜角の推定値を得ているのかコードより解きほぐしてみましょう。

まずはフィルタに導入する測定値と予測値についてみていきましょう。

センサによる測定値

M5stickCに内蔵されている6軸の慣性センサ(加速度、ジャイロ) SH200Q を使用して傾斜角と角速度を測ります。

傾斜角θは加速度センサから重力加速度の傾きを検出して導出します。

角速度はジャイロセンサの値(x軸)をそのまま使用します。

数理モデルによる予測値

予測値を得るために物理現象を記述する式(数理モデル)を立てます。

傾斜角の関係式は以下の通りです。

$$θ_{k+1} = (ω_k – ω_{offset})・Δt + θ_k$$

$θ_k $は時刻kのときの傾斜角で$ω_k $は時刻kのときの角速度です。$w_{offset}$は角速度のオフセット誤差です。
時間kのときの角速度に微小時間$Δt $(コードでは2.5msec)をかけて$θ_k $を足すと$Δt $後(時間 k+1)の傾斜角$θ_{k+1}$が得られるということを示しています。

またここで簡単のため角速度(ジャイロセンサ)のオフセットは時刻によらず一定と仮定します。

$$ω_{offset}(k+1) = ω_{offset}(k)=ω_{offset}$$

ここですっ飛ばしブチかましますが、現代制御理論では数理モデルを行列で表現する習わしがあるそうです。

状態方程式

物理状態の移り変わりを示すのが状態方程式です。上記2式より以下で表せます。

$$\left( \begin{array}{c}  θ_{k+1}\\ ω_{offset} \end{array} \right)=\left( \begin{array}{cc}  1&-Δt\\0&1 \end{array} \right)\left( \begin{array}{c}  θ_{k}\\ ω_{offset} \end{array} \right)+\left( \begin{array}{c} Δt\\ 0 \end{array} \right)ω_k$$

一般式では以下のように書きます。

$$x_{k+1}=A・x_k+B・u_k$$

ここでの物理状態$x$ ($θ_k$と$ω_{offset}$)は行列Aをかけて2項目の入力$ω_k$(角速度)に関する値を足すと次の状態があられることを示しています。

出力方程式

物理状態で実際に観測できる状態(値)を示す行列式です。

$$θ_k=\left( \begin{array}{cc} 1&0\end{array} \right)\left( \begin{array}{c}  θ_{k}\\ ω_{offset} \end{array} \right)$$

$θ_{k}$だけがセンサから直接得られることを示しています。$ω_{offset}$だけを得ることはできませんのでね。

一般式は以下で書きます。

$$y_k=C・x_k$$

以上で数理モデルが用意できました。また行列A, B, Cは時間依存のない定数でした。

 

カルマン・フィルタ アルゴリズム

やっとコードのカルマン・フィルタ アルゴリズムupdate_theta()が何をやっているかをみてみます。かなりすっ飛ばします。とにかくここではコード上でどんな処理をしているかをみるのが目的です。よく理解できないし。。ここでは丸飲みします。

update_theta()は以下の5つの処理を実行しています。

  1. カルマン・ゲイン$G_k$の算出
  2. 現時点kの推定値$\hat x_k$の算出
  3. 現時点kの共分散行列$P_k$の算出
  4. 次時刻k+1の予測値$\tilde x_{k+1}$の算出
  5. 次時刻k+1の共分散行列$P’_{k+1} $の算出

1. カルマン・ゲイン算出

カルマン・ゲイン$G_k$とはモデルによる予測値$\tilde x_k$とセンサによる測定値$y_k$を混ぜ合わせて推定値$\hat x_k$を求める際の重みづけ配分比を意味します。

そして詳細は分かりませんが$G_k$は以下の式で得られます。

$$G_k=P’_k C^T(W + C P’_k C^T)^{-1}$$

   $P’_k$:前時刻に算出された共分散行列(5. で算出)
   $C^T$:出力方程式の係数行列C = $\left( \begin{array}{cc} 1&0\end{array} \right)$の転置。つまり$\left( \begin{array}{c} 1\\0\end{array} \right)$
   $W$:観測雑音の程度を表す量。つまり加速度センサによる角度の分散(theta_variance)

2. 推定値算出

前時刻に算出された予測値(4. で算出)$\tilde x_{k}$と測定値$y_k = θ_{k}$からカルマン・ゲイン$G_k$を絡めて推定値$\hat x_k$を求めます。

$$\hat x_k=\tilde x_k+G_k(y_k – C \tilde x_k)$$

3. 共分散行列算出

共分散行列とはデータをxとyとすると$\left( \begin{array}{cc}  {σ_x}^2 & {σ_{xy}}^2\\ {σ_{xy}}^2 & {σ_y}^2\end{array} \right)$で表されます。

要するにデータをxとyの変動の相関を表します。
データxとyに相関がない場合は分散$ {σ_{xy}}^2 = 0$となります。

物理状態$θ_k$の共分散行列$P_k$は以下で表されます。例によってなんでかは知りません。

$$P_k = (I_2 – G_k C) P’_k$$

   $I_2$:2行2列の単位行列$\left( \begin{array}{cc} 1&0\\0&1\end{array} \right)$
   $G_k$:カルマン・ゲイン
   $C$:出力方程式の係数行列C = $\left( \begin{array}{cc} 1&0\end{array} \right)$
   $P’_k$:前時刻に算出された共分散行列(5. で算出)

4. 次時刻の予測値算出

モデルを用いて次時刻k+1の傾斜角予測値$\tilde x_{k+1}$を算出します。

$$\tilde x_{k+1} = A \hat x_k + B \bar u_k$$

   $A$:状態方程式の係数行列$\left( \begin{array}{cc}  1&-Δt\\0&1 \end{array} \right)$
   $B$:状態方程式の係数行列$\left( \begin{array}{c}  Δt\\0\end{array} \right)$
   $ \hat x_k$:現時刻の傾斜角推定値(2. で算出)
   $ \bar u_k$:入力(角速度)の平均。ここでは現時刻の角速度測定値$ω_k$で近似。

5. 次時刻の共分散行列算出

これもよくわかりませんが次時刻k+1の共分散行列は以下で得られます。

$$P’_{k+1} = A P_k A^T + B U B^T$$

   $P_k$:現時刻に算出された共分散行列(3. で算出)
   $U$:入力雑音の程度を表す量。つまり角速度の分散(theta_dot_variance)

カルマン・フィルタの初期設定

カルマン・フィルタの処理は前回算出した予測値と共分散行列を用いて推定を行います。常に前のデータを必要とするので最初に起動(k=0)する際には手動で初期値を指定する必要があります。

アルゴリズムの4. と5. で算出する次時刻の予測値が時刻0の時点ではないのであらかじめそれらしい値を決める必要があるということです。

コードでは初期の傾斜は直立であると条件付けして初期値を与えています。

状態予測値の初期設定

$\tilde x_0 = \left( \begin{array}{c}  θ\\ ω_{offset} \end{array} \right)  = \left( \begin{array}{c}  0\\ \bar ω \end{array} \right)$

直立を仮定しているため $θ = 0$、 動かしていないと仮定して$ω_{offset}$をジャイロの平均値としています。

共分散行列の初期設定

$P’_{0} =\left( \begin{array}{cc}  {σ_θ}^2&0\\0&{σ_{ω_{offset}}}^2 \end{array} \right) =\left( \begin{array}{cc} 1&0\\0&{σ_ω}^2 \end{array} \right)$

角度のばらつきは1°程度と想定し、$ω_{offset}$のばらつきはジャイロの測定値のばらつきとしています。傾斜角と$ω_{offset}$に相関はないので対角の値は0です。

初期値の値は多少ずれがあっても時刻が進み測定データが増えると推定値は真値付近に収束するようです。

 

おわりに

傾斜計コードで使用したカルマン・フィルタ アルゴリズムの処理内容を追いかけました。

数式などは分からないことが多々ありますが、確かにその処理をしていい感じの結果を得ているのです。

とにかくカルマン・フィルタ アルゴリズムを飲み込んで数理モデル方程式を立てて初期値の設定をそれらしく実施すればよいとのことなので応用が利くことでしょう。

さて次は倒立振子に向けて動きたいと思います。

次の記事

M5StickC で倒立振子 PID制御編 ー倒立振子への道 3ー

コメントはこちらから

メールアドレスが公開されることはありません。コメントのみでもOKです。

このサイトはスパムを低減するために Akismet を使っています。コメントデータの処理方法の詳細はこちらをご覧ください