Moving horizon estimation

• 別名: MHE, receding horizon estimation
• カテゴリ: State estimation & navigation › Localisation
• タグ: state-estimation, optimization, parameter-estimation, sensor-fusion, model-predictive-control

直近の時刻ステップからなるスライディングウィンドウ内の測定にダイナミクスモデルを当てはめる、最適化に基づく状態推定器。各ステップで解き直し、制約を課すことができる。

定義

移動ホライズン推定（Moving horizon estimation、MHE）は、直近 N 個の測定からなるウィンドウ上で最適化問題を解くことで、システムの現在の状態と、必要に応じて未知のパラメータや外乱を推定する。決定変数はウィンドウ内の各時刻の状態であり、同等の表現としては最初の状態と各ステップのプロセスノイズである。コストは、予測出力と測定出力の不一致、データを説明するのに必要なプロセスノイズの大きさ、そしてウィンドウの最初の状態の事前推定からのずれにペナルティを課す。次の時刻ステップではウィンドウが 1 サンプル分前に進み、最も古い測定が捨てられ、問題が再び解かれる。

ウィンドウの最初の状態をつなぎ留める項は到着コスト（arrival cost）と呼ばれる。これは、すでにウィンドウから外れたすべての測定の代わりとなる。線形モデル、ガウスノイズ、制約なし、厳密な到着コストという条件では、MHE はカルマンフィルタと同じ推定を与える。実際には到着コストは近似され、多くの場合は拡張カルマンフィルタの共分散更新や固定の 2 次重みが使われる。ウィンドウ長は推定品質と計算量のバランスをとるように選ばれる。

MHE はしばしばモデル予測制御の推定版と説明される。MPC は未来のホライズンにわたって最適化して制御入力を選び、MHE は過去のホライズンにわたって最適化して測定を説明する。再帰的なフィルタに対する利点は、正の質量、関節の可動限界、摩擦円錐の内側にある接触力といったハード制約を課せること、そしてステップごとに 1 回の線形化に固定するのではなく、解くたびにウィンドウ全体を再線形化することで、これは強い非線形性をもつモデルで役立つ。その代償は各時刻ステップで非線形最小二乗問題を解く必要があることで、リアルタイム実装は構造を利用するソルバー、ウォームスタート、ステップあたりの限られた反復回数に頼る。推定する状態にモデルパラメータを加えると MHE はオンラインのシステム同定ツールになり、Parameterized DDP の論文はこのように使っている。

重要な理由

ロボットの推定問題の多くは、再帰的なフィルタでは扱いにくい制約や非線形性を含む。MHE はフィルタと完全なバッチスムーザの中間に位置する。データのウィンドウと最適化器を使いながら計算量を有界に保つため、モデル予測制御コントローラに入力する状態やパラメータの推定に実用的である。

主な用途

• MPC のためのペイロード質量、摩擦その他のパラメータのオンライン推定
• 接触中の脚ロボットの制約付き状態推定
• 車両とドローンの状態および外乱の推定
• 遅延や不規則な測定を伴うセンサフュージョン
• モデル残差からの故障と異常の検出

実際の使われ方

空中マニピュレータを構築するチームが、未知のペイロードを把持した後にその質量と重心を推定したいと考える。チームはこれらのパラメータを MHE 問題の状態に加える。この問題は直近 1 秒弱の IMU、モーター回転数、位置姿勢のデータを使い、質量を正に、重心をグリッパーの届く範囲内に保つ制約をもつ。各制御周期で、ソルバーは前回のウィンドウを 1 サンプルずらしたものからウォームスタートし、acados のようなコード生成ソルバーで Gauss-Newton 反復を数回実行して、推定値を飛行 MPC に渡す。到着コストの重みの調整が主なトレードオフである。低すぎると推定値にノイズが多くなり、高すぎるとペイロードが変わったときの反応が遅くなる。

よくある誤解

• MHE はデータバッファ付きのカルマンフィルタである — MHE は各ステップでウィンドウ全体にわたる最適化を解き、制約を課すことができ、ウィンドウ全体を再線形化する
• ウィンドウの外の情報は単に失われる — 到着コストがそれを引き継ぐ。ただし近似的にである
• ウィンドウは長いほど常に良い — 長いウィンドウは計算量が増え、実際の変化への応答を遅らせることがある。パラメータを定数としてモデル化している場合は特にそうである

知っておくべき人

• 状態推定エンジニア
• モデル予測制御エンジニア
• 空中ロボットと脚ロボットの開発者
• オンラインのシステム同定に取り組む研究者

関連技術

• acados
• CasADi
• カルマンフィルタ（Kalman Filter）
• ファクターグラフによる平滑化（Factor Graph Smoothing）
• Gauss-Newton 法（Gauss-Newton Method）

関連用語

• Extended Kalman Filter
• Model Predictive Control
• Pose Graph Optimization
• Sensor Fusion
• Differential dynamic programming
• System Identification

関連プロジェクト

• FAST-LIVO2: Fast, direct LiDAR-inertial-visual odometry from HKU-MARS, published in IEEE T-RO
• Drake: MIT robotics toolbox for model-based dynamics, optimization, and control system design
• GTSAM: Factor-graph optimization library powering many modern SLAM backends
• OpenVINS: The reference open platform for visual-inertial state estimation research
• manif: Header-only C++ Lie theory library for robotics state estimation
• MuJoCo MPC: DeepMind's interactive real-time model predictive control framework for MuJoCo

動画

• Moving Horizon Estimation in MATLAB, Python, and Simulink: APMonitor.com