位置・速度を状態とし、加速度を白色ノイズ過程とするカルマンフィルタとカルマンスムーザ

はじめに

いよいよランダム加速度モデルでカルマンフィルタを実装してみます。
とりあえず、前回作ったプロセスノイズの共分散行列を使います。

コードはとりあえず下記のkd_pv.py に置きました。

内容

問題設定

位置と速度を状態変数とするシステムを考えます。速度の時間微分は白色ノイズ過程とします。
位置のみ、観測することができます。

まず、観測値を作るために

• 正解位置
• 正解位置に観測ノイズが加えられた観測値

を生成します。システム側で設定するのは

• 位置、速度の初期分布(初期値と共分散行列)
• ランダム加速度のノイズ密度

です。

カルマンフィルタ

いつもの式です。（今は省略）
下記のコードで動きました。

x = np.array([0.0, 0.0]).reshape(2, 1)
P = x_var_init
t_est = []
x_est = []
P_est = []
t_prev = 0.0
for _i, (t, y) in enumerate(zip(t_idx, pos_obs)):
    t_est.append(t)
    x_est.append(x)
    P_est.append(P)

    if _i == 0:
        continue

    # time update 
    dt = t - t_prev
    t_prev = t
    Q = np.array([[1.0 / 3.0 * np.power(sig0, 2) * np.power(dt, 3), \
         1.0 / 2.0 * np.power(sig0, 2) * np.power(dt, 2)], \
        [1.0 / 2.0 * np.power(sig0, 2) * np.power(dt, 2), \
        np.power(sig0, 2)* dt]])
    x = np.dot(F, x)
    P = np.dot(np.dot(F, P), F.T) + Q

    # measurement update
    S = np.dot(np.dot(H, P), H.T) + R  # inovation (pre-fit residual covariance)
    #K = np.linalg.solve(S, np.dot(P, H.T))
    K = np.dot(P, H.T) * (1.0 / S)
    x = x + np.dot(K, y - np.dot(H, x))
    P = np.dot((np.eye(2) - np.dot(K, H)), P)

観測値、正解の一緒のプロットしてみます。内部状態として持っている速度は非観測ですが、正解に近い値になっています。

面白いと思ったのは、ランダム加速度の大きさが小さいと追従性が悪くなります。
実際の加速度よりも小さい値をモデル化すると、加速度変化が小さい状態として速度に時間遅れが発生します。位置よりも速度に顕著に出ています。位置の方が観測からのFBが強くかかるからかな。（プロットしているのは、観測更新後の状態です）

カルマンスムーザ

カルマンフィルタで　$P(X_t|Y_1,\cdots,Y_t)$ を求め、$P(X_{t+1}|Y_1,\cdots,Y_t)$を予測しています。一方で、$P(X_{t+1}|Y_1,\cdots,Y_T)$が分かっている状態から、$P(X_t|Y_1,\cdots,Y_T)$ に更新します。

とりあえず下記で動きました。^^;) 先程のカルマンフィルタの推定で得た情報を使用します。

t_smooth = []
x_smooth = []
P_smooth = []
n = len(t_idx)
x_smooth.append(x_est[n-1])
P_smooth.append(P_est[n-1])
t_smooth.append(t_idx[n-1])
_x_back = x_est[n-1]
_P_back = P_est[n - 1]
for _i in range(n-2, -1, -1):
    _x_pred = np.dot(F, x_est[_i])
    _P_pred = np.dot(np.dot(F, P_est[_i]), F.T) + Q
    J = np.dot(np.dot(P_est[_i], F.T), np.linalg.inv(_P_pred))
    _x_back = x_est[_i] + np.dot(J, (_x_back - _x_pred))
    _P_back = P_est[_i] + np.dot(J, np.dot(_P_back - _P_pred, J.T))
    t_smooth.append(t_idx[_i])
    x_smooth.append(_x_back)
    P_smooth.append(_P_back)

下記の結果をえました。速度はSmoothing されていますね。

共分散の値をみてみると、Kalman filter よりも Kalman smoother の方が小さくなっています。

初期状態の情報を与えているので、smoother でも最初のほうは初期条件の影響を受けて共分散が大きくなり、値も引っ張られていますね。また、速度については、smoothing の方が共分散が小さく、より自信をもった値を推定できていることが分かります。

ところで、巷にカルマンスムーザで後ろ向きに計算をするときに、初期化をするということをすることがあるようです。試しに、ここでも実行してみました。戻り始めでは共分散が大きく、推定値がずれますが、収束してからは精度が良くなっています。局所解への収束を防ぐなどに役には立つのかもしれませんが、収束するまでの推定制度は劣化しますね。

まとめ

とりあえず動いた。と思います。計算内容の理論についてはどこかできちいんと説明を書きたいですが、何を計算したかはとりあえずコードをみると分かります。
(2023/07/03)