【倒立振子part13】カルマンフィルタで角度導出

前回は角速度と角加速度から角度の導出を行った。
その際にそれぞれデメリットがあるため改善のためにカルマンフィルタの導入する。
今回カルマンフィルタを導入するにあたり「トラ技2019/7月」を元にまとめていく。
(過去、倒立振子を作ろうと断念して放置してあった。。。)

カルマンフィルタとは
状態空間モデル
アルゴリズム
プログラム
実測
次回

カルマンフィルタとは

カルマンフィルタとは簡単にはセンサの測定値から誤差を取り除いて真値を推定する計算する方法のことである。
ジャイロセンサで角度を取得するためのカルマンフィルタは下記のようなイメージである。
(加速度センサとジャイロセンサから得られる測定値は正規分布に従うとする。)

測定した値のほかに真の角度が従うモデルから推定値を確立統計的に求める手法のことである。

状態空間モデル

上の図でもあるが測定した角度のほかに真の角度が従うモデル(状態空間モデル)を作る必要がある。

角度、角速度、角速度オフセットの関係式

ある時刻kでの傾斜角 $\theta_k$ 、角速度 $\omega_k$ 、角速度オフセット $\omega_{offset}$ が分かっているとする。
角速度オフセットは時間によらず一定であるとするとセンサが静止している状態で平均から算出できるとする。
この時の関係式は
$\theta_{k+1}=\theta_k+(\omega_k-\omega_{offset})\Delta{t}$
$\omega_{offset}(k+1)=\omega_{offset}(k)$

状態方程式

上式を角度 $\theta_k$ と角速度オフセット $\omega_{offset}$ を状態ベクトルとした状態方程式の形に変形すると
$\displaystyle{\left(\begin{array}{c}\theta_{k+1}\\ \omega_{offset}\end{array}\right)=\begin{pmatrix}1 & -\Delta{t} \\0 & 1 \end{pmatrix}\left(\begin{array}{c}\theta_k\\ \omega_{offset}\end{array}\right)+\left(\begin{array}{c}\Delta{t}\\ o\end{array}\right)\omega_k}$
となる。
この時ジャイロセンサから得られる角速度 $\omega_k$ が角度を求めるための入力であると考えられる。

観測方程式

状態ベクトルの $\theta_k$ は加速度センサから求めることができるが、角速度オフセットについては時間ごとに直接求めることはできないため観測方程式は
$\displaystyle{\theta_k=(1　0)\left(\begin{array}{c}\theta_k\\ \omega_{offset}\end{array}\right)}$
となる。