ロボットが必要とする「状態」は、センサ画面にそのまま出ているとは限らない。位置センサは速度を直接示さず、IMUは加速度・角速度を示してもバイアスを持ち、カメラの自己位置は遅れたり途切れたりする。状態オブザーバは、対象モデルと測定値の差を使って、見えない状態を推定する仕組みである。線形カルマンフィルタ（KF）は、その中でもモデル誤差と測定ノイズを確率的な共分散で扱い、予測と更新を繰り返す代表的な方法である。
重要なのは「複数センサの平均」ではない。どの値をどの時刻の状態だと見なし、どのノイズをどれほど信用し、矛盾した測定にどう反応するかを明示することにある。推定器が自信満々に誤ると、LQR、PID、MPCがすべて誤った状態へ操作を加える。本稿は結論、状態空間と可観測性、カルマン予測・更新、数値例、制御器との使い分け、安全、実装点検を扱う。ROS 2の時刻・ノード境界はROS 2入門、より広い統合の話はセンサフュージョン入門を参照してほしい。
結論：推定値と「どれほど信用できるか」を一緒に渡す
- オブザーバはモデル予測と測定残差から状態 \hat{x} を更新する。可観測でない状態は、どれほど賢いフィルタでも測定履歴から一意に復元できない。
- KFは推定誤差共分散 P を持ち、プロセス雑音 Q と測定雑音 R に応じてモデルと測定の重みをカルマンゲイン K として計算する。
- Q,R は「正解率の調整ノブ」ではなく、モデルとセンサの誤差分布という仮定である。推定残差、共分散、時刻、外れ値をログで検証する。
- 推定が古い、共分散が大きい、残差が異常、座標変換が失敗した場合は、制御器へ通常状態を渡さず減速・停止・代替センサへ遷移する。
状態空間とルエンバーガーオブザーバ
離散時間の線形系を
とする。x は真の状態、u は既知入力、z は測定、w はモデル化していない加速度・バイアスなどのプロセス雑音、v は測定ノイズである。基本的なルエンバーガーオブザーバは、
と書ける。最後の項はイノベーション（測定残差）であり、L が測定をどれだけ信じるかを決める。L が大きいと測定へ素早く寄るがノイズを通し、低いとモデルに従うが誤差を修正しにくい。
図: Duskcoil作成の概念図。推定軌跡・共分散はモデルに基づく内部量であり、実測そのものではない。
可観測性はLQR記事と同じく \mathcal O=[C^T,(CA)^T,\ldots]^T のrankで調べる。位置しか測れない二重積分器でも、連続する位置測定から速度は推定できる。しかし観測可能という二値の性質だけでは足りない。低いサンプリング周波数、量子化、大きな遅れ、長い測定欠損では、速度推定の不確かさが急増する。必要な状態を「どの誤差で、何ms以内に」知る必要があるかが実装要件となる。
線形カルマンフィルタ：予測と更新
KFは白色ガウス雑音を仮定し、w\sim\mathcal N(0,Q)、v\sim\mathcal N(0,R) として、状態の平均と誤差共分散を再帰的に推定する。予測（prior）は
である。P は「状態値そのものの分散」ではなく、真の状態と推定値の差に関する不確かさである。モデルを何ステップも進めれば、通常は Q により P^- が膨らむ。
測定が届くと、イノベーション \nu_k=z_k-C\hat{x}_k^- とその共分散
を計算する。カルマンゲイン、更新は
である。数値計算では丸め誤差で P の対称性・半正定性が崩れ得るため、Joseph form P^+=(I-KC)P^-(I-KC)^T+KRK^T を使うことがある。逆行列を明示的に作らず、線形方程式・分解で解く方が安定な場合も多い。
Q と R を何として決めるか
R はセンサ仕様の一点の精度値をそのまま入れるだけではない。温度、振動、視野、距離、通信の再送、量子化を含め、実際の測定誤差を記録して推定する。Q は「モデルが一周期でどれほど外れるか」であり、未知加速度、バイアスのランダムウォーク、荷重変化を表す。Q を小さくし過ぎればフィルタはモデルを過信して外乱へ鈍くなり、大きくし過ぎれば測定ノイズを追う。
革新量の正規化二乗 \nu^TS^{-1}\nu は、測定が予測と統計的に整合するかの監視に使える。値が大きければ、外れ値、座標誤り、タイムスタンプ誤り、センサ故障、モデル不一致が疑われる。単に測定を捨てる前に、どのセンサがいつ、どの座標系で、どの遅れを持つかをログから調べる。ゲーティングは異常を隠す装置ではなく、安全な代替状態へ遷移する判定材料である。
数値例：位置測定から速度を推定する
周期 T の一次元等速モデルを x=[p\ v]^T とすると、
で、位置だけを測る。最初は速度が不確かなら P_{vv} を大きく置く。連続する位置測定の残差を使って、フィルタは速度を推定する。測定が欠損すると予測は続くが P は増える。ここで制御器が速度を必要とするなら、推定値 \hat v だけでなく P_{vv}、測定の経過時間、残差異常を見て、速度指令を下げる・停止する規則が必要である。
IMUを加えると短時間の加速度変化を追いやすくなる一方、バイアスを状態に入れなければ積分誤差が蓄積する。カメラ・LiDAR・GNSSは絶対位置を戻せるが、更新頻度、遮蔽、遅れ、座標整合が異なる。非線形な姿勢や距離観測には拡張カルマンフィルタ（EKF）やUKFが使われるが、線形KFの Q,R、時刻、外れ値、可観測性の問題が消えるわけではない。
IMU/INSの代表例
深度カメラの代表例画像: Xsens MTi-G（Kallap85, CC BY-SA 4.0）およびIntel RealSense D435（Marc Auledas, CC BY-SA 4.0）、Wikimedia Commons。推定に使われ得るセンサの外観例であり、同一システムでの実測・推奨構成を意味しない。
PID・LQR・MPCへどう渡すか
PIDは測定誤差を直接使えるが、微分項や速度内側ループでは推定速度が有用になる。LQRは多状態 \hat{x} を前提とし、分離原理の条件ではLQRゲインとカルマン推定器を別々に設計してLQG（linear quadratic Gaussian）構成にできる。ただし飽和・非線形・遅延が強ければ、分離原理だけで実機のロバスト性を保証しない。MPCも初期状態・外乱推定に依存するため、古い推定値や過小評価した共分散で未来を最適化してはならない。
| 目的 | 典型的な選択 | 推定器から必要なもの | 失敗時の要点 |
|---|---|---|---|
| 単軸の速度・温度 | PID | フィルタ済み測定、速度推定 | センサ断で積分を止める |
| 多状態の局所安定化 | LQR/LQG | 時刻整合した \hat{x} と妥当性 | 共分散増大でゲインを信じ過ぎない |
| 制約付き経路・配分 | MPC | 状態、外乱、遅れ、信頼度 | 古い状態・solver失敗で減速・停止 |
ロボット実装と故障時安全
ROS 2では、IMU、エンコーダ、カメラのメッセージ到着時刻と計測時刻を分ける。遅延したカメラ観測を現在時刻の状態へそのまま更新すると、ロボットが過去へ引かれる。履歴状態へ戻して再伝播する、遅延を許容しない、低位推定器と上位地図補正を分けるなど、方式を明示する。ROS 2入門とセンサフュージョン入門の通信・座標変換の設計は、KFの外側の必須条件である。
安全監視は、測定タイムアウト、共分散上限、革新量ゲート連続超過、バイアス推定の発散、座標変換不可、制御周期の逸脱を扱う。外れ値を除外しても、すべてのセンサが消えたなら予測だけで長く走り続けない。用途に応じて速度制限、停止、冗長センサ、操作者への引継ぎを選ぶ。推定器の再初期化では状態が飛ぶことがあるため、制御器の積分状態・参照・出力をバンプレスに管理する。
実装チェックリスト
- 状態、測定、入力、座標系、計測時刻・受信時刻を定義したか。
- 可観測性と、各状態の必要精度・最大遅れを定量化したか。
- Q,R,P_0 の根拠をログ・仕様・試験から記録したか。
- P の対称性・正定性、残差、ゲート、タイムアウトを監視するか。
- 欠損・外れ値・再局在化・バイアス変化を注入試験したか。
- 推定品質が悪いとき、PID/LQR/MPCへ渡す参照と安全状態が決まっているか。