はじめに

カルマンフィルタ、解析的に計算ができるにも関わらず応用の範囲が極めて広い状態推定手法である。おそらく、これほどシンプルで使いこなしに技術者の差が出るものは無いと思う。

カルマンフィルタの使いこなし力が問われるのは下記の3点の要素に集約される。

1. 状態方程式のモデリング
2. 観測方程式のモデリング
3. 事前分布の設定
4. 座標系の取り方

である。カルマンフィルタでは状態方程式によって計算される状態量と観測方程式によって計算される状態量をカルマンゲインを使って上手に混ぜ合わせて推定を行う。ここで混ぜ合わせるとは、各方程式によって計算される状態の不確実性を加味して最終的な推定値を定めるということである。直感的には状態方程式の予測と観測方程式の補正の言っていることが「ピタッとは一致しない」ということが起こるが、それらの内分点を定めようとするのがカルマンゲインであると考えてよい。

カルマンゲインは事前分布と状態方程式と観測方程式を定めれば、あとはデータによって自動で定まる。この計算が解析的であることが見事であり、オンラインのリアルタイム推定の主役として現在も君臨し続けている。

問題設定

今回は、この中でも「リアルタイム推定」に使うことを考慮した「状態方程式」と「観測方程式」のモデリングに着目しようと思う。

具体的には、観測が「Δtだけ過去のセンサ値に基づく」場合に、その値をリアルタイムに現在の状態推定にどのように用いるのかを題材にする。

今回はこれを観測遅延と呼ぶことにする。このようなことをは次のようなケースで生じうる。

• センサ値の取得から、カルマンフィルタを実行するプロセスに対する通信遅延
• センサ値の取得から、状態量の計算アルゴリズムに時間を要する（e.g.カメラでの位置推定）

要するに Event Time と Process Time が無視できないほどずれているということである。このような時刻のズレをカルマンフィルタ側に吸収してもらうのが今回の問題である。今回は簡単のため、遅延の時刻はアルゴリズムの外で分かっているものとする。これは「カメラの画素データから移動量を算出する」とか「衛星の生データから位置情報を算出する」とかを想定する。データの発生時刻（Event Time）と計算されて位置の情報になる時刻（Process Time）が異なることを想定するが、アルゴリズムの計算時間を見積もって各回出力できることを想定する。時計の正しさが妥当であれば、使わないよりはマシだろうという戦略である。

ちなみに観測遅延時間自体を状態方程式に埋め込むこともやろうと思えばできる。ただ、よほど情報がそろっていない限り可観測性を悪くするだけである。事前分布側には凡その遅延度合は盛り込む方が良い。遅延時間が分かっている設定というのは、つまるところ、観測遅延時間を状態方程式に埋め込んでいるが、事前分布で不確実性がほぼ無いと設定しているに等価である（むしろそうならば計算量が減る分、状態方程式に埋め込まない方が良い）。

ちなみにカメラでもGNSSでも位置の情報を各時刻差分取れば速度が出る。速度が出るならば、観測ができていない区間で等速直線運動を仮定することで、観測遅延を補正してからカルマンフィルタに渡すこともできる。だけど、大抵の場合、観測の周期よりも状態方程式での予測の周期の方が速い。予測の周期側の情報を有効活用して観測遅延を補正できる方が「等速直線運動の仮定」が緩くなる期待がある。

状態方程式と観測方程式の定式化

状態方程式：車両の運動をどうモデル化するか

今回は車両の運動を題材とする。まず「車両の状態とは何か」を定義しなければならない。今回は以下の4つの量で車両の状態を表現する。

$$ x = \begin{pmatrix} p_x \ p_y \ \psi \ v \end{pmatrix} $$

• $p_x, p_y$：車両の位置（どこにいるか）
• $\psi$：方位角、つまり車両がどっちを向いているか
• $v$：速度、つまりどれくらいの速さで進んでいるか

これは「車両を空から見下ろしたときに、どこにいて、どっちを向いていて、どれくらいの速さで動いているか」を表している。方位角と速度が分かれば、次の瞬間にどの方向にどれだけ進むかが分かる、というわけだ。

次に入力、つまり「運転者が何をしているか」を定義する。

$$ u = \begin{pmatrix} f \ \delta \end{pmatrix} $$

• $f$：駆動力（アクセルを踏む力）
• $\delta$：ステア角（ハンドルをどれだけ切っているか）

運動モデルは Kinematic Bicycle Model を採用する。これは「車両を自転車のように前輪と後輪の2輪でモデル化する」という簡略化である。4輪車でも、左右のタイヤをまとめて考えれば2輪として近似できる。

$$ x_{k+1} = f(x_k, u_k) = \begin{pmatrix} p_x + \Delta t \cdot v \cos\psi \\ p_y + \Delta t \cdot v \sin\psi \\ \psi + \Delta t \cdot \frac{v}{L} \tan\delta \\ v + \Delta t \cdot \left(\frac{f}{m} - c \cdot v\right) \end{pmatrix} $$

この式が何を言っているかを一つずつ見ていこう。

位置の更新 $p_x + \Delta t \cdot v \cos\psi$ について。これは「今の位置から、今の速度 $v$ で、今向いている方向 $\psi$ に、微小時間 $\Delta t$ だけ進む」ということ。$\cos\psi$ と $\sin\psi$ で方向をX成分とY成分に分解している。当たり前だが、速度が大きいほど、また時間が長いほど、たくさん進む。

方位角の更新 $\psi + \Delta t \cdot \frac{v}{L} \tan\delta$ について。ハンドルを切ると車は曲がる。どれだけ曲がるかは「速度」と「ステア角」と「ホイールベース（前輪と後輪の距離）」で決まる。速度が速いほど、ステア角が大きいほど、ホイールベースが短いほど、急に曲がる。軽自動車が小回りが利くのはホイールベース $L$ が短いからだ。

速度の更新 $v + \Delta t \cdot \left(\frac{f}{m} - c \cdot v\right)$ について。アクセルを踏めば加速する（$f/m$）、速度に比例した抵抗がある（$c \cdot v$）。これはニュートンの運動方程式そのものだ。

これをコードにすると次のようになる。

import numpy as np

def bicycle_dynamics(x, u, L, m, c, dt):
    """車両の状態を1ステップ進める"""
    px, py, psi, v = x
    f, delta = u
  
    # 位置：今の速度と方向で進む
    px_next = px + dt * v * np.cos(psi)
    py_next = py + dt * v * np.sin(psi)
  
    # 方位角：ステア角に応じて曲がる
    psi_next = psi + dt * v / L * np.tan(delta)
  
    # 速度：駆動力で加速、抵抗で減速
    v_next = v + dt * (f / m - c * v)
  
    return np.array([px_next, py_next, psi_next, v_next])

ヤコビアン：非線形を線形で近似する

カルマンフィルタは本来、線形システム用の手法である。しかし上の状態方程式は $\cos\psi$ や $\tan\delta$ を含む非線形関数だ。そこで拡張カルマンフィルタ（EKF）では、現在の状態の周りで線形近似を行う。これがヤコビアンである。

ヤコビアンとは何か？ 「ある量を少し変えたとき、結果がどれだけ変わるか」を表す行列である。たとえば「方位角を1度変えたら、次のX位置はどれだけ変わるか？」という感度を全ての組み合わせについて並べたものだ。

状態に関するヤコビアン $F$ は次のようになる。

$$ F = \frac{\partial f}{\partial x} = \begin{pmatrix} 1 & 0 & -\Delta t \cdot v \sin\psi & \Delta t \cos\psi \\ 0 & 1 & \Delta t \cdot v \cos\psi & \Delta t \sin\psi \\ 0 & 0 & 1 & \frac{\Delta t}{L} \tan\delta \\ 0 & 0 & 0 & 1 - \Delta t \cdot c \end{pmatrix} $$

たとえば $(1,3)$ 成分の $-\Delta t \cdot v \sin\psi$ は「方位角 $\psi$ を少し変えると、X位置の変化量がどれだけ変わるか」を表している。車が北を向いているとき（$\psi = 0$）は $\sin\psi = 0$ なので、方位角を少し変えてもX方向の移動量はほとんど変わらない。しかし車が東を向いているとき（$\psi = \pi/2$）は、方位角を少し変えるとX方向の移動量が大きく変わる。向いている方向によって、同じ「方位角のずれ」でも影響が違うのだ。

def compute_jacobians(x, u, L, m, c, dt):
    """ヤコビアン F と G を計算する"""
    psi, v = x[2], x[3]
    delta = u[1]
  
    F = np.array([
        [1.0, 0.0, -dt * v * np.sin(psi), dt * np.cos(psi)],
        [0.0, 1.0,  dt * v * np.cos(psi), dt * np.sin(psi)],
        [0.0, 0.0,  1.0,                  dt / L * np.tan(delta)],
        [0.0, 0.0,  0.0,                  1.0 - dt * c],
    ])
    return F

観測方程式：GNSSは何を見ているか

観測としてはGNSSによる位置観測を考える。GNSSは空から「この車はここにいる」と位置 $(p_x, p_y)$ を教えてくれる。方位角や速度は直接は分からない。

$$ z = H x + v = \begin{pmatrix} 1 & 0 & 0 & 0 \ 0 & 1 & 0 & 0 \end{pmatrix} x + v $$

この $H$ という行列は「状態ベクトルから、観測できる部分だけを取り出す」という操作を表している。4次元の状態から最初の2つ（位置）だけを取り出す。$v$ は観測ノイズで、今回は非常に小さいと仮定する。つまり「GNSSはほぼ正確に真の位置を教えてくれる」という設定だ。

これは現実的な仮定だろうか？ 最近のGNSSは条件が良ければセンチメートル級の精度が出る。一方、ホイールオドメトリ（車輪の回転から推定する速度）はタイヤのスリップや空転で大きく狂う。だから「GNSSを信頼して、オドメトリは補助的に使う」という戦略は条件によっては現実的である。（そもそも観測遅延の影響を見る例題だから、まあ多めに見てケロ。実際はビルたが立ち並ぶマンハッタンとかだとマルチパスで厳しかったりするけど）

カルマンフィルタの更新則：予測と観測を混ぜ合わせる

カルマンフィルタは「予測」と「更新」の2ステップを繰り返す。

予測ステップ

まず、状態方程式を使って「次の状態はこうなるだろう」と予測する。

$$ \hat{x}_{k|k-1} = f(\hat{x}_{k-1|k-1}, u_k) $$

同時に、予測の不確実性（共分散）も更新する。

$$ P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q $$

この式は「不確実性がどう伝播するか」を表している。予測だけをしていると、不確実性はどんどん大きくなっていく。 モデルが完璧ではないから、予測を重ねるほど「本当はどこにいるか分からなくなる」のだ。

更新ステップ

観測が得られたら、予測と観測を混ぜ合わせて推定を改善する。

$$ K_k = P_{k|k-1} H^T (H P_{k|k-1} H^T + R)^{-1} $$

このカルマンゲイン $K$ が「混ぜ合わせの比率」を決める。$R$ が小さい（観測を信頼する）とゲインが大きくなり、観測に強く引っ張られる。

$$ \hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k (z_k - H \hat{x}_{k|k-1}) $$

$(z_k - H \hat{x}_{k|k-1})$ は「観測と予測の差」で、イノベーションと呼ばれる。「思っていたのと違う」度合いだ。カルマンゲインとイノベーションを掛けた分だけ、予測を修正する。

たとえ話で言えば、天気予報と実際の気温を混ぜ合わせるようなものだ。天気予報（予測）が「明日は20度」と言っていて、実際に温度計（観測）を見たら22度だった。でも温度計もあまり信頼できないとしたら、「たぶん21度くらいだろう」と推定する。温度計がすごく正確なら観測を信じて22度に近い値を、温度計が怪しければ天気予報を信じて20度に近い値を採用する。この「どれくらい信じるか」を自動で決めてくれるのがカルマンフィルタだ。

def ekf_predict(mu, P, u, Q, L, m, c, dt):
    """EKFの予測ステップ"""
    mu_pred = bicycle_dynamics(mu, u, L, m, c, dt)
    F = compute_jacobians(mu, u, L, m, c, dt)
    P_pred = F @ P @ F.T + Q  # 不確実性が増える
    return mu_pred, P_pred

def ekf_update(mu, P, z, R):
    """EKFの更新ステップ"""
    H = np.array([[1, 0, 0, 0], [0, 1, 0, 0]])
  
    y = z - H @ mu  # イノベーション（予測と観測の差）
    S = H @ P @ H.T + R
    K = P @ H.T @ np.linalg.inv(S)  # カルマンゲイン
  
    mu_upd = mu + K @ y  # 状態を修正
    P_upd = (np.eye(4) - K @ H) @ P  # 不確実性が減る
    return mu_upd, P_upd

ここで重要なのは、観測 $z_k$ は時刻 $k$ における状態 $x_k$ の観測であるという暗黙の仮定が置かれているということだ。観測と予測を比較するとき、同じ時刻の状態について比較しているはずだ、という前提がある。

観測遅延の問題：時間のズレが引き起こすこと

さて、ここからが本題である。GNSSの観測には遅延がある。時刻 $k$ に「到着する」観測値 $z_k$ は、実際には時刻 $k - d$ における状態を観測したものである。

$$ z_k = H x_{k-d} + v $$

ここで $d$ は遅延ステップ数である。たとえば500Hzで動くシステムで0.5秒の遅延があれば、$d = 250$ ステップとなる。

これは何を意味するか？ 今手元に届いた「位置情報」は、実は0.5秒前の位置なのだ。 車が時速36km（秒速10m）で走っていたら、0.5秒で5m進む。つまり「今ここにいる」と思って使っている位置情報は、実は5m後ろの位置かもしれない。

Naive アプローチ：何が問題か

一番単純なアプローチは、遅延を無視して、到着した観測を現在の状態の観測として扱うことだ。

$$ \hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k (z_k - H \hat{x}_{k|k-1}) $$

これを図で考えてみよう。車が北に向かって走っているとする。

1. 時刻 $k-d$：車は位置Aにいる。GNSSがこの位置を観測する。
2. 時刻 $k-d$ から $k$ まで：車は北に進み続ける。カルマンフィルタは予測を繰り返し、「車は今、位置Bにいるだろう」と推定する。
3. 時刻 $k$：GNSSの観測が届く。「位置はAだ」と言っている。
4. Naiveアプローチ：「あれ、今Bにいると思ったけど、GNSSはAだと言っている。GNSSを信頼しよう」と、推定位置をAの方に引き戻す。

これは明らかに間違っている。 GNSSが言っている「A」は0.5秒前の位置であって、今の位置ではない。GNSSを信頼すればするほど（$R$ が小さいほど）、この「過去への引き戻し」が強くなる。

たとえ話で言えば、友人から「今どこにいる？」と聞かれて、LINEの位置情報を送ったら、実はそれは5分前の位置で、今はもう駅に着いているのに、友人は「まだあのカフェにいるのか」と思い込むようなものだ。友人があなたの言葉を100%信じるタイプなら、「カフェから動いていない」と確信してしまう。

def naive_filter_loop(mu, P, u_all, z_all, has_gnss, Q, R, L, m, c, dt):
    """Naiveフィルタ：遅延を無視する（間違ったアプローチ）"""
    mu_history = [mu]
  
    for k in range(len(u_all)):
        # 予測
        mu, P = ekf_predict(mu, P, u_all[k], Q, L, m, c, dt)
  
        # 更新（遅延を無視して、到着した観測を「今の位置」として扱う）
        if has_gnss[k]:
            mu, P = ekf_update(mu, P, z_all[k], R)  # これが間違い！
  
        mu_history.append(mu)
  
    return np.array(mu_history)

正確な情報を間違った時刻に使っているのが問題の本質である。皮肉なことに、良いセンサを使うほど推定が狂う。GNSSの精度が高いから信頼する、でも時刻が間違っているから結果的に大きく外す、という悪循環だ。

遅延補正の手法

観測遅延を適切に扱う手法として、今回は2つの手法を実装した。

1. 履歴再伝播（Replay）: 観測が到着したら、過去の状態に遡って更新し、現在まで再計算する
2. 確率的クローニング（Stochastic Cloning）: 過去の状態のコピー（クローン）を保持し、クロス共分散を追跡する

手法1: 履歴再伝播（Replay）

考え方

最も直接的なアプローチは、タイムマシンに乗って過去に戻り、そこで観測を反映させてから、また現在に戻ってくるというものだ。

具体的には、

1. 時刻 $k$ に観測 $z_k$ が到着。これは時刻 $k-d$ の観測だと分かっている。
2. 保存しておいた時刻 $k-d$ の状態推定値を取り出す。
3. その状態に対して観測更新を行う。
4. 保存しておいた入力 $u_{k-d}, u_{k-d+1}, ..., u_{k-1}$ を使って、時刻 $k$ まで予測を再計算する。

$$ \hat{x}_{k-d|k-d} = \hat{x}_{k-d|k-d-1} + K_{k-d} (z_k - H \hat{x}_{k-d|k-d-1}) $$

この更新された過去の状態から、現在まで再伝播する。

なぜこれが正しいのか

この方法が正しい理由は、観測を正しい時刻の状態に対して適用しているからだ。GNSSが「時刻 $k-d$ の位置はAだ」と言っているなら、時刻 $k-d$ の推定を修正するのが正しい。そして、その修正された過去から、入力の履歴を使って現在まで「やり直す」。

夏休みの日記を書いていて、3日前の出来事の記憶が間違っていたことが判明したら、3日前のページを書き直して、それ以降のページも整合性を取るために書き直すようなものだ。帳尻合わせ、やったことあるでしょ。

def replay_filter_loop(mu, P, u_all, z_all, has_gnss, delay_steps, Q, R, L, m, c, dt):
    """Replayフィルタ：履歴を遡って再計算する"""
    mu_history = [mu]
    P_history = [P]
  
    for k in range(len(u_all)):
        # 通常の予測
        mu, P = ekf_predict(mu, P, u_all[k], Q, L, m, c, dt)
        mu_history.append(mu)
        P_history.append(P)
  
        # GNSS観測が到着したら、過去に遡って更新
        if has_gnss[k] and k >= delay_steps:
            obs_step = k - delay_steps
      
            # 過去の状態を取り出して更新
            mu_past = mu_history[obs_step]
            P_past = P_history[obs_step]
            mu_upd, P_upd = ekf_update(mu_past, P_past, z_all[k], R)
      
            # 更新した過去から現在まで再伝播
            for i in range(delay_steps):
                mu_upd, P_upd = ekf_predict(mu_upd, P_upd, u_all[obs_step + i], Q, L, m, c, dt)
      
            # 現在の状態を更新
            mu = mu_upd
            P = P_upd
            mu_history[-1] = mu
            P_history[-1] = P
  
    return np.array(mu_history)

利点と欠点

利点：理論的に正しい。再伝播の各ステップで最新の状態を使って線形化し直すので、非線形性も正確に反映できる。

欠点：計算コストが高い。遅延が $d$ ステップなら、GNSS観測が届くたびに $d$ 回の予測を再計算する。今回の設定では250ステップの再計算が必要で、これが計算時間を大きく増やす原因になる。

手法2: 確率的クローニング（Stochastic Cloning）

考え方

Replay の計算コストを避けるため、過去の状態のコピー（クローン）を保持し、現在の状態との相関を追跡するという手法がある。

アイデアはこうだ。GNSSが観測を行う時刻に、その時点の状態推定値を「クローン」として保存しておく。その後、現在の状態は予測で進んでいくが、クローンは動かさない。ただし、現在の状態とクローンの相関（クロス共分散）は追跡し続ける。

なぜ相関が重要か？ 現在の状態は過去の状態から予測で計算されているから、両者は無関係ではない。「過去の状態が実は少しずれていた」と分かったら、「じゃあ現在の状態もそれに応じてずれているはず」と推論できる。この「連動してずれる度合い」がクロス共分散だ。

数式での説明

観測時刻にクローンを作成すると、拡張状態ベクトルは次のようになる。

$$ x^{\text{aug}} = \begin{pmatrix} x_k \ x_{k-d}^{\text{clone}} \end{pmatrix} $$

共分散行列も拡張される。

$$ P^{\text{aug}} = \begin{pmatrix} P_{k|k} & P_{\text{cross}} \ P_{\text{cross}}^T & P_{k-d}^{\text{clone}} \end{pmatrix} $$

ここで $P_{\text{cross}}$ がクロス共分散で、「現在の状態とクローンがどれだけ連動するか」を表す。

予測ステップでは、現在の状態だけが更新される。クローンは過去の状態なので動かない。しかしクロス共分散は更新する必要がある。

$$ P_{\text{cross}}^{+} = F_k \cdot P_{\text{cross}} $$

この式は「状態遷移で現在の状態が変わると、クローンとの相関もそれに応じて変化する」ことを表している。ヤコビアン $F$ を掛けることで、状態の変化に伴う相関の変化を追跡できる。

観測が到着したら、クローンに対して更新を行う。そして、クロス共分散を通じて現在の状態も更新する。

$$ K_{\text{current}} = P_{\text{cross}} H^T (H P_{\text{clone}} H^T + R)^{-1} $$

$$ \hat{x}_{k|k} = \hat{x}_{k|k-1} + K_{\text{current}} (z - H \hat{x}_{\text{clone}}) $$

直感的に言えば、「過去の状態がこれだけずれていたなら、現在の状態もこれだけずれているはず」という推論を、クロス共分散を使って行っているのだ。

たとえ話

間違え方が似ていると考えているのである。受験に励む君は、次の試験で何点取れるかをいつも予測してから試験に臨む。良い姿勢だ。**1年前に受けた試験の結果は予想よりも10点高かったとする。**なるほど、予想は控えめなようである。**更に半年前の試験の予想は80点であった。**さて、今君は新たな試験に挑もうとしている。**今回の試験の予想は85点だ。ここで、前回の試験の点数の結果が今得られ、88点だったとわかった。**また結果の方が高い。今得られた情報は半年前の情報であるが、傾向からして予測よりも上振れした点が取れるだろうということに確信が持てそうではないか？つまり今回の試験の予想は85点だとしていたが、過去の試験も引き続き上振れした結果が得られていることを観測したことにより、今回の結果を94点くらいに更新してもよさそうだ。

10年後、親の身長の正確な値が分かった（観測が到着）。このとき、「兄の身長が思っていたより5cm高かった」と分かれば、「じゃあ弟の身長も、相関の分だけ高いかもしれない」と推論できる。これがクロス共分散を使った更新だ。

def cloning_filter_loop(mu, P, u_all, z_all, has_gnss, should_clone, delay_steps, Q, R, L, m, c, dt):
    """確率的クローニング：クロス共分散を追跡する"""
    mu_history = [mu]
  
    # クローンの状態
    mu_clone = mu.copy()
    P_clone = P.copy()
    P_cross = P.copy()  # 初期は同じ状態なので P_cross = P
    clone_valid = False
  
    for k in range(len(u_all)):
        # クローンを作成するタイミング（GNSS観測の delay_steps 前）
        if should_clone[k]:
            mu_clone = mu.copy()
            P_clone = P.copy()
            P_cross = P.copy()
            clone_valid = True
  
        # 予測：現在の状態を更新
        F = compute_jacobians(mu, u_all[k], L, m, c, dt)
        mu, P = ekf_predict(mu, P, u_all[k], Q, L, m, c, dt)
  
        # クロス共分散の伝播（これがキモ！）
        P_cross = F @ P_cross
  
        # GNSS観測が到着したら、クローンを使って更新
        if has_gnss[k] and clone_valid:
            H = np.array([[1, 0, 0, 0], [0, 1, 0, 0]])
            y = z_all[k] - H @ mu_clone  # クローン（過去の状態）との差
      
            S = H @ P_clone @ H.T + R
            S_inv = np.linalg.inv(S)
      
            # クロス共分散を使って現在の状態を更新
            K_current = P_cross @ H.T @ S_inv
            mu = mu + K_current @ y
            P = P - K_current @ S @ K_current.T
      
            clone_valid = False
  
        mu_history.append(mu)
  
    return np.array(mu_history)

利点と欠点

利点：計算コストが低い。履歴の再計算が不要で、行列演算のみで済む。リアルタイム処理に向いている。

欠点：クローン作成時の線形化を使い続けるため、強い非線形性がある場合に近似誤差が蓄積する可能性がある。ただし、今回の実験では実用上問題ないレベルだった。

実験

実験設定

以下の条件で3手法を比較した。

パラメータ	値
シミュレーション時間	30秒
オドメトリ更新レート	500 Hz
GNSS更新レート	1 Hz
GNSS遅延	0.5秒（250ステップ）
GNSSノイズ	σ = 0.001 m（ほぼ真値）
観測共分散 R	σ = 0.01 m（GNSSを信頼）

モデルパラメータの誤差

現実のシステムでは、カルマンフィルタが使うモデルパラメータは真値と一致しない。今回は以下の誤差を設定した。

パラメータ	真値	推定に使う値	誤差
ホイールベース$L$	2.5 m	2.3 m	-8%
車両質量$m$	1500 kg	1400 kg	-7%
抵抗係数$c$	0.1	0.2	+100%

これは何を意味するか？ カルマンフィルタは「間違ったモデル」で予測しているということだ。

たとえば、ホイールベースが実際より短いと思っているから、「同じステア角ならもっと曲がるはず」と予測する。質量が実際より軽いと思っているから、「同じ駆動力ならもっと加速するはず」と予測する。抵抗係数が2倍だと思っているから、「速度はもっと落ちるはず」と予測する。

このようなモデル誤差がある状況こそ、観測による補正が重要になる。予測だけに頼ると、モデルの間違いがそのまま推定誤差になる。だからこそ、GNSSのような外部観測で定期的に補正することが必要なのだ。

ポイントはGNSSのノイズが非常に小さいという設定だ。これは「GNSSを信頼する」ということを意味する。信頼できるセンサの情報を正しく使えるかどうかが、遅延補正の価値を示すことになる。

軌道は円軌道上を蛇行するパターンとした。ステア角は正弦波で滑らかに変化させ、旋回しながら左右に大きく蛇行する動きを模擬している。速度が出ている状態での旋回は、遅延の影響が大きく出やすい条件だ。

結果

XY平面上の軌跡

黒線が真の軌跡、緑線がNaive（遅延無視）、青線がCloning、赤破線がReplayである。マゼンタの点はGNSS観測点を示す。

Naiveでは軌跡が真値から大きくずれているのが一目瞭然だ。緑の線が黒の線からはみ出して、まるで車が蛇行しすぎているように見える。これはGNSS観測のたびに「0.5秒前の位置」に引き戻されているためだ。一方、CloningとReplayはほぼ真値に追従しており、黒線にぴったり重なっている。

時系列での状態推定

上から順にX位置、Y位置、方位角、速度、駆動力、ステア角を示す。横軸（時間）は全てのプロットで揃えてある。

Naiveでは、GNSS観測のたびに推定位置がジャンプしているのが分かる。特にX位置とY位置のグラフで、緑線がギザギザになっている。これは正確な観測を間違った時刻に使っているためだ。1秒ごとに「過去に引き戻される」→「予測で進む」→「また引き戻される」を繰り返している。

CloningとReplayはこのジャンプがなく、滑らかに真値を追跡している。遅延補正が効いている証拠だ。

またNaiveも遅延補正も速度の推定は定常的なバイアスが出ているのもわかる。抵抗や車両質量のモデル化誤差が現れていると考えてよい。それでも位置の次元ではGNSSの時間遅延を織り込んだ推定することで、予測のドリフトを補正できていることが素晴らしい結果である。

定量的比較

指標	Naive（遅延無視）	Cloning	Replay
RMSE 位置 [m]	1.44	0.32	0.34
RMSE 方位 [deg]	4.64	1.06	1.16
計算時間 [s]	0.23	0.27	0.37

※計算時間はJITコンパイル済みの状態で計測（同一入力形状でのウォームアップ後）。両手法とも lax.scanを使用した最適化実装での比較。

数字で見ると差は歴然としている。Naiveの位置RMSEは約1.4mで、遅延補正手法（約0.34m）の約4倍である。方位角も同様で、Naiveは約5度ずれているのに対し、遅延補正手法は1度程度に収まっている。

なお、遅延補正手法でも約30cmのRMSEが残っているのは、モデルパラメータの誤差の影響である。GNSSは1秒に1回しか来ないので、その間はモデルで予測するしかない。モデルが間違っていれば、その分だけ誤差が蓄積する。これは遅延補正とは別の問題であり、モデルの精度向上やプロセスノイズのチューニングで改善できる余地がある。

考察

1. 遅延補正の重要性：なぜNaiveはこんなに悪いのか

Naive手法がこれほど悪い結果になる理由は、GNSSを信頼しているからだ。皮肉に聞こえるかもしれないが、これが本質である。

カルマンゲインの式を思い出してほしい。

$$ K = P H^T (H P H^T + R)^{-1} $$

$R$ が小さい（観測を信頼する）と、ゲイン $K$ が大きくなる。つまり「観測と予測が違ったら、観測を信じて大きく修正する」ということ。

ところが、この「信頼している観測」が時刻を間違えている。速度が10m/sの場合、0.5秒の遅延で5mもの差が生じる。GNSSが「5m後ろにいる」と言っているのを信じて、推定位置を5m後ろに引き戻す。でも実際には車は進んでいるから、また予測で進む。そしてまた引き戻される。

良いセンサを間違った使い方で使うと、悪いセンサを使うより悪い結果になるという教訓だ。

2. 計算時間の比較：Cloningは実用的

手法	計算時間	速度比
Cloning	0.27s	1.4x 高速
Replay	0.37s	1.0x

CloningはReplayの約1.4倍高速である。両手法とも lax.scanを用いた最適化実装での比較だ。

なぜこの差が生じるのか？Replayは各GNSS到着時に250ステップ分の再計算が必要だ。30秒間で30回のGNSS観測があるから、合計 $30 \times 250 = 7500$ ステップの余分な計算が発生する。元の15000ステップに加えて、これだけの再計算が走る。

一方、Cloningはクロス共分散の伝播 $P_{\text{cross}}^{+} = F \cdot P_{\text{cross}}$ という行列演算を各ステップで行うだけ。履歴を遡る必要がない。このアルゴリズム的な差が1.4倍という計算時間の差になって現れる。

リアルタイム処理では、この差は決定的だ。500Hzで動くシステムでは、1ステップあたり2ms以内に処理を終える必要がある。Replayだとこの制約を満たせない可能性があるが、Cloningなら余裕がある。

3. 精度の比較：CloningとReplayはほぼ同等

CloningとReplayの位置RMSEはほぼ同じ（0.34m）である。両手法の差は1cm未満で、実用上無視できるレベルだ。

今回はモデルパラメータに大きな誤差（特に抵抗係数が+100%）があるため、遅延補正手法でも約30cmのRMSEが残っている。これはモデル誤差の影響であり、遅延補正自体は正しく機能している。

なぜCloningの近似がうまく機能しているのか？ クロス共分散の伝播 $P_{\text{cross}}^{+} = F \cdot P_{\text{cross}}$ は、ヤコビアン $F$ を使った線形近似である。今回の設定では遅延が0.5秒で、その間の状態変化はそこまで大きくない。だから線形近似がよく効く。

ただし、以下の状況ではReplayが有利になる可能性がある。

• 遅延が非常に大きい（1秒以上）：線形近似の誤差が蓄積する
• 強い非線形性（急旋回、高速走行）：ヤコビアンが大きく変化する
• オフライン処理で最高精度が必要：計算時間に余裕がある

まとめ

観測遅延のあるカルマンフィルタについて、3つのアプローチを比較した。

手法	遅延補正	精度	計算コスト	推奨ケース
Naive	なし	✕	◎	遅延がない場合のみ
確率的クローニング	あり	○	◎	リアルタイム処理
履歴再伝播	あり	◎	△	オフライン処理、高精度要求

重要な知見

1. 遅延補正は必須：GNSSを信頼する設定では、遅延を無視すると精度が約4倍悪化する。良いセンサを間違った使い方で使うのは、悪いセンサを使うより悪い。
2. 多くの場合、クローニングで十分：Replayとの精度差は約1.5cmで、計算時間は約1.4倍高速。リアルタイム処理には最適な選択だ。
3. モデル誤差の影響：遅延補正をしても残る誤差（今回は約30cm）はモデルパラメータの誤差に起因する。モデルの精度向上は別途取り組むべき課題だ。
4. 実装上のポイント：クロス共分散の伝播 $P_{\text{cross}}^{+} = F \cdot P_{\text{cross}}$ がキモ。これを追跡することで、過去の観測の情報を現在の状態に正しく反映できる。
5. 数値安定性：共分散行列の更新にはJoseph形式 $(I-KH)P(I-KH)^T + KRK^T$ を使うことで、数値的な安定性を確保できる。単純な $P - KSK^T$ だと丸め誤差で正定値性が崩れることがある。

観測遅延は「無視すればいい」問題ではない。特にGNSSのような信頼できるセンサの情報を正しく活用するためには、遅延補正は必須である。確率的クローニングは、計算コストと精度のバランスに優れており、リアルタイム処理における実用的な選択肢となる。

参考文献

1. Bar-Shalom, Y., Li, X. R., & Kirubarajan, T. (2004). Estimation with Applications to Tracking and Navigation. Wiley.
2. Julier, S. J., & Uhlmann, J. K. (2004). Unscented filtering and nonlinear estimation. Proceedings of the IEEE, 92(3), 401-422.
3. Roumeliotis, S. I., & Burdick, J. W. (2002). Stochastic cloning: A generalized framework for processing relative state measurements. ICRA 2002.