Skip to content

hellocybernetics/Tutorial_KalmanFilter

BranchesTags

Folders and files

NameName
Last commit message
Last commit date

Latest commit

 

History

2 Commits
experiments
experiments
 
 
kalman_filter
kalman_filter
 
 
tests
tests
 
 
.gitignore
.gitignore
 
 
README.md
README.md
 
 
pyproject.toml
pyproject.toml
 
 
timeseries.png
timeseries.png
 
 
trajectory_xy.png
trajectory_xy.png
 
 
uv.lock
uv.lock
 
 

Repository files navigation

はじめに

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

カルマンフィルタの䜿いこなし力が問われるのは䞋蚘の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平面䞊の軌跡

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.

About

No description, website, or topics provided.

Resources

Readme
Activity

Stars

4 stars

Watchers

0 watching

Forks

0 forks
Report repository

Releases

No releases published

Packages

 
 
 

Contributors

Languages