カルマンフィルタとは　pythonでの実装例

カルマンフィルタとは、現代制御理論におけるオブザーバによる状態推定方法の一種で、入力にノイズ(システムノイズ、駆動源雑音という)が含まれていても、カルマンゲインを用いることで精度よく状態推定できるものです。なお外乱を推定するような外乱オブザーバとは異なります。

ここでは、カルマンフィルタの形とカルマンフィルタの実装例を中心に説明し、カルマンゲインの導出方法についてはこちらで説明します。

＜一般的なオブザーバの形＞
以下のとおり。なおオブザーバは状態を推定する機能なのでこれだけでは制御対象物をフィードバックできません。オブザーバを用いた状態フィードバック方法はこちらで説明。

＜カルマンフィルタの形＞
カルマンフィルタの形は以下となります。一般的なオブザーバの形に対して、観測値やシステムにノイズを考慮した形となります。また前回推定値と観測値を元に今回値を推定する逐次推定型となります。

　　　

なおシステムノイズ、観測ノイズは、平均値が0の正規分布の白色雑音とし以下で表現されます。

■カルマンフィルタのイメージ

カルマンフィルタをイメージするため(2)式を以下の様に変形します。

これは逐次推定の指数移動平均の式となり、これは観測値と推定値をどちらをより信用して推定値を更新するかをカルマンゲインで調整する事を意味します。そしてカルマンゲインは以下(4)となります。

式の導出方法はこちらで説明しますが、イメージを掴む為に次の様に考えます。(4)の分母には観測ノイズの項wがあり、分子にはシステムノイズの項vがあります。仮に観測ノイズが大きいと分母が大きくなるのでカルマンゲインgとしては値が小さくなり、観測値は信用せずに推定値を強く反映します。逆にシステムノイズが大きくなればゲインgは大きくなり推定値を信用せずに観測値を強く反映するという考え方です。

＜ブロック図＞
これまで説明した結果をブロック図で表現すると以下になります。

■カルマンフィルタのpython実装具体例

以下1次遅れの伝達関数を持った制御対象物の動作をカルマンフィルタによって推定します。

＜s関数をz関数に変換＞
上記伝達関数を双一次変換によって以下の様に変換します。

＜可観測正準形に変換＞
z関数の可観測正準形はこちらで説明している式を用います。(6)式は、

となります。

＜pythonのプログラムコード＞
これで必要な情報は全て揃いました。プログラムコードはこちら。

シミュレーション結果は以下。システムノイズの分散値(Q)と観測ノイズの分散値(R)をそれぞれ幾つにするかがチューニング要素となりますが、今回はＱ=1、Ｒ=5として、観測ノイズの方が大きい設定にしました。従って動作としては、観測値に対して動きが鈍感になります。

サブチャンネルあります。⇒ 何かのお役に立てればと

カルマンフィルタとは pythonでの実装例