カルマンフィルターとは?

カルマンフィルターは、逐次ベイズフィルターの一種であり、測定データからシステムの状態を推定するアルゴリズムです。これは、ハンガリーのエンジニアであるルドルフ・カルマン(Rudolf Kalman)によって提唱されました。このカルマンフィルターはNASAのアポロ計画で使われたことで有名で、アポロを月へ導いた数式とも言えます。アポロ計画では、センサーの情報から宇宙船の正しい位置を推定し、進行方向の調整などを行う際に使用されました。

現在、カルマンフィルターにはいくつかのバリエーションがあり、これらのフィルターは、コンピュータービジョン、誘導・航法システム、バッテリー充放電状態、計量経済学、および信号処理などの、推定に依存するアプリケーションで広く使用されています。

フィルターとは

フィルターという言葉を聞くと、信号処理のノイズ除去等を思い浮かべる方が多いと思いますが、ここでいうフィルターとは、時間変化を伴う現象の状態を観測結果を用いて推定することを言います。

カルマンフィルターとは

カルマンフィルターとは、過去(1時刻前)の推定値と現在のシステムへの入力およびシステムから計測した値からシステムの状態を推定する方法になります。ただし、入力には雑音が乗っており、計測した値にも雑音が乗っているものとします。

例えばセンサーで採取した信号には、必ず雑音が混入されます。カルマンフィルターはこのような実現象の影響を加味した考え方で構築されるアルゴリズムで、確率的な枠組みで状態推定を行うことでシステマティックに最適設計をすることができます。

まず初めに、カルマンフィルターを構築するにあたり、入力から推定値を算出するために、制御対象となる数学モデルを算出する必要があります。

数学モデルの表現はシステムの入出力の関係を表現するにあたり、制御工学でよく使用する状態方程式を算出します。この数学モデルを使用して動的システムに対し状態xを推定します。

※デジタル制御で実装することを前提に、ここでは離散時間系にフォーカスした説明とします。

[補足]

A：状態行列

B：入力行列

C：出力行列

G：プロセス雑音行列

ブロック線図で表現すると以下になります。

次に、ノイズは平均値0の正規分布型の白色雑音を仮定(ガウス過程)します。

$$E(w[n]) = E(v[n]) = 0$$

\[E(w[n]w[n]^T) = Q,\quad E(v[n]v[n]^T) = R, \quad E(w[n]v[n]^T) = 0\]

[補足]

$E()$：確率変数の平均値 ※$E$は、Expectationの頭文字です。

$Q,R$：雑音$v,w$の共分散行列

そして、カルマンフィルターのアルゴリズムを先ほど算出したプラントモデルの数学モデルに適用し、以下図の概念に沿って状態を推定していきます。

システムの入力値uと観測値$y_v$により推定値を更新し、時間更新で次のステップの状態の予測を行います。

推定値の更新は、予測した観測値と実測した観測値を比較してカルマンゲインで内分比を決定後に推定値を更新します。カルマンゲインを決める際、リカッチ方程式を解くことで最適なカルマンゲインが求まります。そして、また次のステップで新たな観測値を入手してその予測値を補正していきます。以降、このような処理を繰り返していくことで、状態推定を逐次行います。

カルマンフィルターのバリエーションと特徴

以下に、代表的なカルマンフィルターのバリエーションと、その特徴を記します。

拡張カルマンフィルター(EKF:Extended Kalman filter): 非線形システムを各時刻で線形化し、状態方程式のAとCを時変にしたもの
アンセンテッドカルマンフィルター(UKF:Unscented Kalman filter): 状態方程式のAとCの代わりに現在のシステム状態の近傍点（シグマポイント）を用いて、集合平均的に確率分布を近似する。
アンサンブルカルマンフィルター(EnKF:Ensemble Kalman Filter):アンサンブル予報誤差の情報を用いたデータ同化と、データ同化による解析誤差の情報からアンサンブル予報の補正を行う。
粒子フィルター(PF:Particle Filter)：多数のサンプル点(粒子)を用いて、確率分布による時系列データの予測手法。逐次モンテカルロ法とも呼ばれる。

モデル	カルマンフィルター	拡張/アンセンテッドカルマンフィルター	アンサンブルカルマンフィルター	粒子フィルター
非線形性	×	〇	〇	◎
非ガウス性	×	×	△	◎
計算負荷	〇	〇	△	△

カルマンフィルターの活用例

バッテリーのSOC推定

バッテリーの内部状態は直接観測することが出来ないため、カルマンフィルターを使用してSoCを正確に推定します。バッテリーは充放電を適切に管理しないと重大事故や使用寿命に大きく影響がでてしまうため、このような手法はとても広く使われています。

以下のモデルは、充放電を繰り返すことで劣化する非線形なバッテリーモデルを作成し、2種類のカルマンフィルターを使用することで、精度よくSOCを推定するモデルです。

このシステムは、バッテリーのSOCが30%になると自動的に充電され、90%になると放電するように設定されており、また劣化を表現するために、この充放電のサイクルごとにバッテリーの容量が1Ah減少するように設計されています。バッテリーの計測した電流の積算値からカルマンフィルターを使用してバッテリー容量を推定し、アンセンテッドカルマンフィルターでSOCを推定します。結果を確認すると、初期推定誤差の後は、推定したSOCは実際のSOCに収束していることが分かります。(詳細は例を参照)

航法・誘導・制御

航空・宇宙の分野では、カルマンフィルターを、GNC(Guidance、Navigation、Control)システムで一般的に使用します。カルマンフィルターは、GPSとIMU（慣性測定装置）の測定値を融合することによって位置信号と速度信号を合成する、GNC (Guidance、Navigation、Control)システムのセンサーフュージョンなどでよく使用されています。フィルターは、航空機エンジンのタービン内の温度など、測定できない信号値の推定にも使用されます。

以下は、レーダーを使用した航空機の追跡をシミュレーションする例です。航空機の位置を、カルマンフィルターを使用してノイズを含むレーダーの測定値から推定します。

コンピュータービジョン

コンピュータービジョン・アプリケーションでは、カルマンフィルターを物体追跡に使用します。物体の将来の位置を予測し、不正確な検出から生じる雑音(ノイズ)の削減や、複数の物体を軌跡に一致させる処理の簡略化をします。

以下の図は、ボール軌跡の追跡を行っています。ボールの移動が途中で箱により隠されており、その部分をカルマンフィルターを使用して位置を予測しています。物体検出は黒色、カルマンフィルターからの出力は赤丸で示されています。(詳細は例を参照)

MATLAB環境で制御開発

MATLAB環境を活用した、カルマンフィルターを活用した制御開発の一般的なステップを以下に示します。

プラント(制御対象)モデル構築：バッテリーモデルの作成
制御設計：カルマンフィルターを設計しプラントモデルと組み合わせて評価・検討
RCP(Rapid Control Prototyping)：制御モデルを汎用ECUに実装し評価・検討

それぞれのステップで最適なツールを選択し、開発効率を向上させることが可能です。

Simscape Electrical™を使用してバッテリーの物理モデリングを素早く構築することが可能です。
作成したプラントモデルのパラメータ設定を測定データと合わせるために、Optimization Toolbox™とSimulink Design Optimization™を使ってパラメータ同定することが出来ます。
Control System Toolbox™　もしくは　System Identification Toolbox™にある各種フィルターブロックを使用してSimulink環境で素早くフィルターを構築します。
RCPする場合は、Simulink Coder™やEmbedded Coder™で自動Cコード生成して汎用ECUに実装します。