カルマンフィルタ｜雑音から真の状態を推定する逐次最適化手法

カルマンフィルタ

カルマンフィルタは、雑音を含む観測値から、直接は測れない系の内部状態を逐次推定するアルゴリズムである。1960年にハンガリー出身の数学者ルドルフ・E・カルマン（Rudolf E. Kálmán）が発表した。前の時刻の推定値と新しい観測値を、それぞれの不確かさに応じて重み付けして組み合わせ、誤差の分散が最小になる推定値を求める。航空宇宙、制御工学、ロボット、計測機器など幅広い分野で使われており、現在の組込みシステムでは欠かせない手法のひとつになっている。

Table Of Contents

アポロ計画で実用化されるまでの経緯

カルマンの論文「A New Approach to Linear Filtering and Prediction Problems」は、1960年にASMEのJournal of Basic Engineeringに掲載された。ウィーナーフィルタは定常過程と周波数領域での扱いを前提としていたが、カルマンは状態空間表現を用いて時間領域の再帰計算に置き換えた。翌1961年にはR・S・ビューシーとの共著で連続時間版が発表され、こちらはカルマン・ビューシーフィルタと呼ばれる。発表当初、学界の反応は冷ややかであった。理論の価値をいち早く見抜いたのはNASAエイムズ研究センターのスタンリー・シュミットで、非線形な軌道力学に適用するための線形化を工夫し、アポロ計画の航法コンピュータへの搭載につなげた。メモリが数十キロバイトしかない計算機で月往還の軌道を推定できたことが、この手法を一気に広めた。

予測と更新を繰り返す仕組み

計算は「予測」と「更新」の2段階を、時刻kごとに繰り返す。系の状態方程式と観測方程式は線形とし、プロセス雑音と観測雑音は互いに独立な、平均0の正規分布に従うと仮定する。この前提が成り立つとき、推定誤差の共分散が最小となる推定値が得られ、線形推定のなかでは最適となる。考え方は最小二乗法を逐次処理に拡張したものとみることもできる。

  1. 状態予測：x̂k|k−1 = F x̂k−1|k−1 + B uk
  2. 誤差共分散予測：Pk|k−1 = F Pk−1|k−1 FT + Q
  3. カルマンゲイン：Kk = Pk|k−1 HT（H Pk|k−1 HT + R）−1
  4. 状態更新：x̂k|k = x̂k|k−1 + Kk（zk − H x̂k|k−1）
  5. 誤差共分散更新：Pk|k =（I − Kk H）Pk|k−1

Fは状態遷移行列、Hは観測行列、Qはプロセス雑音、Rは観測雑音の共分散行列を表す。観測が正確（Rが小さい）ならゲインは大きくなり、観測値を重視する。逆にセンサが粗いとモデルの予測が優先される。全履歴を保存しなくてよく、直前の推定値と共分散だけで次の計算ができる点が実装上の大きな利点となっている。

非線形系への拡張

現実の系の多くは非線形であり、基本形はそのままでは使えない。そのため複数の派生手法が考案された。

手法 非線形の扱い 計算負荷 主な適用例
線形カルマンフィルタ 扱えない 小 速度推定、平滑化
拡張カルマンフィルタ（EKF） ヤコビ行列で1次近似 中 姿勢推定、電池残量推定
アンセンテッドカルマンフィルタ（UKF） シグマ点で分布を近似 中〜大 強い非線形の航法
粒子フィルタ 多数の粒子で分布を表現 大 非ガウス雑音、自己位置推定

UKFは1997年にジュリアーとウールマンが提案した。ヤコビ行列を導出する手間がなく、2次の精度まで近似できる。

製造現場での使われ方

産業機器で身近な例として、サーボモータの速度推定がある。エンコーダのパルスを差分しただけでは低速域で量子化誤差が目立つが、1 kHz程度の制御周期でフィルタを通すと、遅れの少ない滑らかな速度信号が得られる。推定した速度はPID制御の帰還信号として使われる。無人搬送車やドローンでは、ジャイロスコープと加速度センサを組み合わせた6軸IMUの姿勢推定に、EKFがよく使われる。ジャイロのドリフトを加速度の重力方向で補正するという役割分担である。

実務で手間がかかるのはQとRの調整である。Rはセンサのデータシート記載のノイズ密度や、静止状態で1,000点ほど取った実測値の分散から決めやすい。Qは理論的に求めにくく、試運転を繰り返して決めることが多い。状態の次元がnのとき、計算量はおおむねn3に比例するため、浮動小数点演算器のない安価なマイコンでは、状態を4次元程度に抑えたり、固定小数点で共分散行列が非対称に崩れないよう対策したりする必要がある。量産品では、こうした実装コストが手法を選ぶ決め手になることも少なくない。

電池管理と航法への応用

リチウムイオン電池の電池管理システム（BMS）では、等価回路モデルとEKFを組み合わせて充電率（SOC）を推定する方式が広く採用されている。電流積算だけで誤差が累積する問題を、端子電圧の観測で補正できる。航法分野では、GPSと慣性航法装置を統合するGNSS/INS複合航法の中核を担う。GPSの更新周期が10 Hz前後でも、慣性センサの数百Hzの出力で間を埋め、トンネル内などで衛星信号を失っても位置を出し続けられる。

カルマンの業績と評価

カルマンは1930年にブダペストで生まれ、のちにアメリカへ移ってMITとコロンビア大学で学んだ。可制御性・可観測性の概念の確立にも大きく貢献し、現代制御理論の基礎を築いた人物として知られる。1985年に京都賞先端技術部門を受賞し、2009年にはアメリカ国家科学賞を授与された。2016年に86歳で亡くなっている。

コメント（β版）