センサ、ノイズ、そして初めてのカルマンフィルタ

ロボットは自分自身の状態を直接見ることはできません。見えるのはセンサの読み値であり、どの読み値もある程度ずれています。このレッスンは、numpyでループを書けて、典型的なロボットのセンサにとって「ずれ」がどのようなものかを知りたい読者を対象としています。多数のずれた読み値を組み合わせて、どの1つよりも良い推定値を得る方法も扱います。3つのセンサをシミュレーションしてその誤差を測り、次にノイズを含む位置測定から移動するロボットを追跡するカルマンフィルタを作ります。フィルタはnumpyで約15行です。多くのロボットの自己位置推定スタックの内部で動いているのも、次元を増やしただけの同じアルゴリズムです。

最初に出会う3つのセンサ

エンコーダは軸の回転量を測ります。インクリメンタル型の光学式または磁気式エンコーダは、軸の回転に伴うパルスを数えます。直交（quadrature）エンコーダは位相が4分の1周期ずれた2つのチャネルを持つため、コントローラは回転量だけでなく回転方向も判別できます。読み値は整数のカウントなので、測定される角度は段階的に変化します。駆動輪のエンコーダからはオドメトリが得られます。これは次のレッスンで扱います。用語集のエンコーダの項も参照してください。

**慣性計測装置（IMU）**は3軸加速度計と3軸ジャイロスコープを組み合わせたもので、磁気センサを含むこともあります。ジャイロは角速度をrad/s単位で測ります。角度を得るには角速度を積分します。問題が始まるのはこの積分です。角速度に一定のオフセットがあると、それが時間に比例して増える角度誤差になります。低価格IMUのMEMSジャイロにはこの種のオフセット（バイアス）があり、温度や電源投入のたびに変化します。IMUも参照してください。

距離センサは距離を測ります。代表的なものは超音波センサ、赤外線のToF（time-of-flight）センサ、LiDARです。1回の距離の読み値には、扱いやすい対象であればほぼガウス分布に従うノイズが乗ります。さらに、反射、ガラス、暗い表面による外れ値がときどき混じります。

3種類の誤差

デバッグすることになるセンサ誤差のほとんどは、次の3つのいずれかに分類されます。

誤差	どう見えるか	平均化は効くか
ランダムノイズ	各読み値が真値の周りに、直前の値とは独立にばらつく	効く。N個の読み値の平均の標準偏差は、Nの平方根分の1になる
バイアス	すべての読み値が同じ量（またはゆっくり変化する量）だけずれる	効かない。平均は真値にバイアスを加えた値に収束する
量子化	読み値が一定の刻み（エンコーダの1カウントなど）で得られる	真値が動くか、ノイズによるディザリングで刻みをまたぐ場合に限り効く

以下のブロックは、静止したロボットの1分間をシミュレーションします。ジャイロには説明用の値として、0.5 deg/sのバイアスと0.2 deg/sの白色ノイズを与えています。実際の値は部品によって異なるので、使用するIMUのデータシートを確認してください。エンコーダは1回転あたり4096カウントです。距離センサは2 m先の壁を、20 mmのガウスノイズ付きで読みます。

import numpy as np

rng = np.random.default_rng(6)
dt = 0.01                      # 100 Hz sample rate
t = np.arange(0, 60, dt)       # one minute, robot standing still

# Gyro: true rate is zero. The reading has a constant bias plus white noise.
gyro_bias = np.deg2rad(0.5)    # 0.5 deg/s, an illustrative low-cost MEMS value
gyro_noise_std = np.deg2rad(0.2)
gyro = gyro_bias + rng.normal(0.0, gyro_noise_std, t.size)
heading = np.cumsum(gyro) * dt             # integrate rate to get heading

# Same gyro after a 5 s calibration at rest: subtract the mean of that window.
bias_estimate = gyro[: int(5 / dt)].mean()
heading_cal = np.cumsum(gyro - bias_estimate) * dt

# Encoder: 4096 counts per revolution, so angle comes in steps of 2*pi/4096.
counts_per_rev = 4096
true_angle = 0.123456
measured = np.round(true_angle / (2 * np.pi / counts_per_rev)) * (2 * np.pi / counts_per_rev)

# Range sensor: true distance 2.000 m, Gaussian noise 20 mm, 50 readings.
ranges = 2.0 + rng.normal(0.0, 0.02, 50)

print(f"gyro heading after 60 s, raw:        {np.rad2deg(heading[-1]):7.2f} deg")
print(f"gyro heading after 60 s, calibrated: {np.rad2deg(heading_cal[-1]):7.2f} deg")
print(f"estimated bias: {np.rad2deg(bias_estimate):.3f} deg/s (true 0.500)")
print(f"encoder step: {np.rad2deg(2 * np.pi / counts_per_rev):.4f} deg, "
      f"quantisation error here: {np.rad2deg(measured - true_angle):+.4f} deg")
print(f"range: one reading {ranges[0]:.4f} m, mean of 50 {ranges.mean():.4f} m, "
      f"std of one {ranges.std(ddof=1) * 1000:.1f} mm, std of mean {ranges.std(ddof=1) / np.sqrt(50) * 1000:.1f} mm")

gyro heading after 60 s, raw:          30.22 deg
gyro heading after 60 s, calibrated:    0.19 deg
estimated bias: 0.500 deg/s (true 0.500)
encoder step: 0.0879 deg, quantisation error here: -0.0423 deg
range: one reading 2.0301 m, mean of 50 1.9981 m, std of one 19.7 mm, std of mean 2.8 mm

数値はそれぞれの誤差の性質をよく表しています。

生のジャイロから求めた方位は、ロボットが静止していたにもかかわらず、1分間で30度ドリフトしました。これは0.5 deg/sのバイアスに60 sを掛けた値です。白色ノイズは積分すると平均化されるため、比較するとほとんど寄与しません。ロボットが静止していることがわかっている最初の5 sの読み値を平均すると、バイアスを1000分の数 deg/s以内の誤差で推定できます。これを差し引くと、1分後のドリフトは0.19度になります。多くのロボットが電源投入後の数秒間は動かさないよう求めるのはこのためです。ただし、温度によって変化するバイアスはこれでは補正できません。ロボットがジャイロと、方位を直接測る別のセンサを組み合わせる理由の1つはここにあります。

エンコーダの刻みは0.088度です。静止したエンコーダの読み値はすべて同じなので、いくら平均しても2つの刻みの間にある真の角度は復元できません。この例の量子化誤差は0.042度で、最悪の場合である半刻みより少し小さい値です。

距離センサは、平均化が最もよく効く例です。1回の読み値は30 mmずれています。50回の読み値の平均のずれは1.9 mmで、その平均の標準偏差の予測値は、19.7 mmを50の平方根で割った2.8 mmです。ただし、50回の読み値を平均できるのは壁が動かなかったからです。移動するロボットには、もっと良い方法が必要です。

平均化はフィルタリングではない

ロボットが廊下を走行し、センサが0.3 mのノイズを含む位置を1秒間に5回報告するとします。すぐに思いつく対策は、直近の数回の読み値の移動平均です。ノイズは減りますが、移動平均が報告するのは、ウィンドウ内でロボットが平均してどこにいたかです。ウィンドウが0.8 sにわたり、ロボットが1.2 m/sで動いていれば、平均値は約半メートル後ろに位置します。移動するロボットでは、移動平均はノイズを減らす代わりに遅れを生みます。

フィルタは、状態がどう変化するかのモデルを使うことで、この引き換えを避けます。ロボットが1.2 m/sで動きながら3.0 mの位置にいたとわかっていれば、0.2 s後には、センサを見る前から3.24 m付近にいると予想できます。新しい読み値は、この予測を補正します。予測と読み値のどちらをどれだけ信頼するかは、それぞれの不確かさの大きさで決まります。カルマンフィルタは、モデルが線形でノイズがガウス分布に従う場合に、この重み付けを最適に行います。R. E. Kalmanが1960年に発表しました。

カルマンフィルタの2つのステップ

状態はベクトルxで、その不確かさを共分散行列Pで表します。直線上のロボットでは位置と速度を使うので、xは2要素、Pは2×2の行列です。各時間ステップで2つの段階を実行します。

予測。 状態を運動モデルFに通します。時間ステップdtの間の等速運動では、位置は速度×dtだけ増え、速度は変わりません。共分散はプロセスノイズQの分だけ増えます。Qは、実際のロボットが1ステップでモデルからどれだけ外れうるかを表します。ここではその外れを標準偏差qの未知の加速度とし、Qはその加速度が位置と速度にどう入るかから求めます。

更新。 観測モデルHは、センサが何を見るかを取り出します。ここでは位置のみです。イノベーションz - H xは、読み値と予測の差です。カルマンゲインKは、イノベーションのうちどれだけの割合を反映するかを決めます。予測が不確か（Pが大きい）なときは大きく、センサのノイズが大きい（Rが大きい）ときは小さくなります。更新後は、推定に使う情報が1つ増えたので、共分散は小さくなります。

フィルタは速度を一度も測っていません。位置の変化から速度を推定し、それがどれだけ正確にわかっているかを共分散が追跡します。

ノイズを含む位置からロボットを追跡する

このブロックは、廊下を10 s走行するロボットをシミュレーションします。速度は1.0 m/sから始まり、ランダムな加速度（標準偏差0.5 m/s²）によってふらつきます。位置センサは0.2 sごとに、0.3 mのノイズ付きで位置を報告します。同じ測定値に対して、5サンプルの移動平均とカルマンフィルタを実行します。

import numpy as np

rng = np.random.default_rng(42)
dt = 0.2                 # s between measurements
n = 50                   # 10 s of data
q = 0.5                  # std of the random acceleration, m/s^2 (process noise)
r = 0.3                  # std of a position measurement, m

# Truth: a robot driving along a corridor. Velocity wanders because of random acceleration.
x_true = np.zeros(n)
v_true = np.zeros(n)
v_true[0] = 1.0
for k in range(1, n):
    a = rng.normal(0.0, q)
    x_true[k] = x_true[k - 1] + v_true[k - 1] * dt + 0.5 * a * dt**2
    v_true[k] = v_true[k - 1] + a * dt
z = x_true + rng.normal(0.0, r, n)          # noisy position measurements

# Constant-velocity model: state [position, velocity].
F = np.array([[1.0, dt], [0.0, 1.0]])       # state transition
H = np.array([[1.0, 0.0]])                  # we measure position only
G = np.array([[0.5 * dt**2], [dt]])         # how an acceleration enters the state
Q = G @ G.T * q**2                          # process noise covariance
R = np.array([[r**2]])                      # measurement noise covariance

x_hat = np.array([z[0], 0.0])               # start at the first measurement, velocity unknown
P = np.diag([r**2, 4.0])                    # large uncertainty on velocity
est = np.zeros(n)
vel = np.zeros(n)
for k in range(n):
    if k > 0:                               # predict
        x_hat = F @ x_hat
        P = F @ P @ F.T + Q
    y = z[k] - H @ x_hat                    # innovation
    S = H @ P @ H.T + R                     # innovation covariance
    K = P @ H.T @ np.linalg.inv(S)          # Kalman gain
    x_hat = x_hat + (K @ y).ravel()         # update
    P = (np.eye(2) - K @ H) @ P
    est[k], vel[k] = x_hat

# A moving average of the last 5 measurements for comparison.
ma = np.array([z[max(0, k - 4): k + 1].mean() for k in range(n)])

def rms(e):
    return float(np.sqrt(np.mean(e**2)))

print(f"RMS error, raw measurements:   {rms(z - x_true):.3f} m")
print(f"RMS error, 5-sample average:   {rms(ma - x_true):.3f} m")
print(f"RMS error, Kalman filter:      {rms(est - x_true):.3f} m")
print(f"final velocity: true {v_true[-1]:.3f} m/s, estimated {vel[-1]:.3f} m/s")
print(f"final position std from P: {np.sqrt(P[0, 0]):.3f} m, Kalman gain {K.ravel().round(3)}")
print("t, truth, measured, estimate (every second sample)")
for k in range(0, n, 2):
    print(f"{k * dt:.1f}, {x_true[k]:.3f}, {z[k]:.3f}, {est[k]:.3f}")

RMS error, raw measurements:   0.232 m
RMS error, 5-sample average:   0.465 m
RMS error, Kalman filter:      0.145 m
final velocity: true 1.449 m/s, estimated 1.191 m/s
final position std from P: 0.166 m, Kalman gain [0.306 0.278]
t, truth, measured, estimate (every second sample)
0.0, 0.000, 0.020, 0.020
0.4, 0.399, 0.588, 0.523
0.8, 0.801, 0.705, 0.596
1.2, 1.168, 0.976, 0.955
1.6, 1.477, 1.925, 1.548
2.0, 1.768, 2.059, 1.885
2.4, 2.068, 1.968, 2.001
2.8, 2.413, 2.589, 2.428
3.2, 2.799, 3.037, 2.911
3.6, 3.164, 3.026, 3.161
4.0, 3.531, 3.473, 3.563
4.4, 3.892, 3.552, 3.707
4.8, 4.266, 4.415, 4.111
5.2, 4.631, 4.839, 4.619
5.6, 5.002, 5.049, 4.990
6.0, 5.405, 5.312, 5.408
6.4, 5.885, 5.687, 5.809
6.8, 6.351, 6.237, 6.215
7.2, 6.818, 6.964, 6.671
7.6, 7.312, 7.316, 7.173
8.0, 7.762, 7.896, 7.778
8.4, 8.251, 8.221, 8.304
8.8, 8.746, 8.722, 8.738
9.2, 9.247, 8.813, 8.974
9.6, 9.784, 9.485, 9.395

カルマンフィルタのRMS誤差は0.145 mで、生の読み値の0.232 mを下回ります。移動平均は0.465 mで、何もしないよりも悪化しています。原因は前述の遅れです。ウィンドウが0.8 sにわたるため、1 m/sを超える速度で動くロボットに対して約0.4 s遅れます。

位置のゲインは0.306に落ち着きます。新しい読み値が入るたびに、推定値は予測から測定値に向かって約31%動きます。この値は手で調整したものではありません。フィルタが定常状態に達すると、QとRから自然に決まります。

実行全体のスケールでは3本の線がほぼ重なります。そこで2つ目のグラフでは誤差、つまり測定位置と推定位置から真値を引いた値を、同じ出力表から計算してプロットします。

最後の1秒は詳しく見る価値があります。真の速度は1.45 m/sまで上がっていますが、フィルタの速度推定値は1.19 m/sです。そのため予測が足りず、誤差は0.39 mまで増えます。モデルは、大きさqのランダムな揺さぶりを除けば速度は一定だと仮定しています。実際のロボットがそれより強く、あるいは長く加速すると、フィルタの追従は遅れます。qを大きくすると、フィルタはモデルをあまり信頼せず、センサをより信頼するようになります。変化への追従は速くなりますが、ノイズも多く通します。rを小さくしても同様の効果があります。カルマンフィルタを実際に導入する際の主なチューニング作業はQとRの選択です。適切な値は当て推量ではなく、センサのデータシートと実機のログから求めます。

1次元からロボットへ

実際の移動ロボットの状態には、少なくともx、y、方位が含まれます。運動モデルには方位のsinとcosが現れます。このモデルは非線形なので、通常のカルマンフィルタはそのままでは使えません。拡張カルマンフィルタは、各ステップで現在の推定値の周りでモデルを線形化し、それ以外は同じ予測と更新を実行します。不確かさが大きすぎる場合や、形が特殊で1つのガウス分布では表せない場合は、粒子フィルタがサンプルの集合で不確かさを表現します。

バイアスも状態に加えられます。ジャイロのバイアスを3番目の状態として加え、そのプロセスノイズをごく小さくすると、フィルタはジャイロと別の方位情報源との食い違いから、バイアスを継続的に推定します。多くのIMUフュージョンフィルタは、この方法でロボットの移動中も、最初のブロックで見たドリフトを抑えています。カルマンフィルタのチートシートにはこれらの派生形の式がまとめてあり、状態推定のハブからは当サイトの関連資料にリンクしています。

演習

1. 2つ目のブロックでq = 2.0に設定して再実行してください。新しいRMS誤差と最終的なカルマンゲインを報告し、モデルとセンサのどちらをどれだけ信頼するかという観点から、変化を説明してください。
2. シミュレーションとフィルタの両方でr = 0.05に設定してください。ゲインと、フィルタと生の測定値の差はどうなりますか。
3. すべての測定値に0.2 mの一定バイアスを加えてください。フィルタはそれを除去できますか。次に、状態を[position, velocity, bias]に拡張し、H = [1, 0, 1]としてください。別のセンサがなければ、フィルタがそれでもバイアスと位置を区別できない理由を説明してください。

まとめ

• センサ誤差には、ランダムノイズ、バイアス、量子化があります。平均化はランダムノイズをサンプル数の平方根分の1に減らしますが、バイアスには効きません。
• ジャイロを積分すると、角速度の一定バイアスが、時間に比例して増える角度誤差になります。短時間の静止キャリブレーションでその大部分を除去できます。
• 移動するロボットでは、移動平均はノイズを減らす代わりに遅れを生みます。フィルタは運動モデルで予測し、測定値が得られるたびにその予測を補正します。
• カルマンフィルタは、予測（状態をFに通し、共分散をQの分だけ増やす）と更新（PとRからゲインKを求め、共分散を縮小する）を交互に行います。速度のように、一度も測っていない量も推定します。
• QとRは、モデルとセンサのどちらを重視するかを決めます。Qが小さすぎると、フィルタは実際の変化に遅れます。

さらに読む

• Probabilistic Robotics, Thrun, Burgard and Fox, MIT Press, 2005：第3章でカルマンフィルタと拡張カルマンフィルタを導出し、第6章で距離センサのセンサモデルを扱っています。
• A New Approach to Linear Filtering and Prediction Problems, R. E. Kalman, Journal of Basic Engineering, 1960：原論文です。
• Modern Robotics, Lynch and Park, Cambridge University Press, 2017 (updated first edition, 2019 preprint)：第13章で車輪型移動ロボットとオドメトリを扱っています。フィルタを2次元に拡張する際に必要になる運動モデルです。