はじめに

ここではカルマンフィルタで使う白色ノイズプロセスの共分散行列について確認しました。
~~理論的な導出ができない~~数値計算で理論を確認するため、白色ノイズを仮定したモデルでの時間積分を実行しました。

もとは言えば、カルマンフィルタで時間更新するときに $P_{n+1} = F P_n F^T + Q$ と共分散行列を更新するのですが、ここで登場する$Q$がプロセスノイズと言われるものです。位置推定を行っているとき、状態変数である位置や速度が、観測がないと時間と共に位置の推定の精度が劣化していく、曖昧さが増大していくという部分に対応します。

ランダムウォークの共分散は、知られている通り時間に比例して分散が大きくなります。ではその時間積分、時間積分と積分前の相関、などがQに入ってきます。文献をみていると、時間のべき乗が異なっていたり、係数となる文数が違ったりします。自分でも実装するとき、どれを使えばよいのか迷ってしまいます。

そこで、今回はとりあえず直接シミュレーションしてみることにしました。コードは下記のprocess_noise.py process_noise2.py です。

シミュレーションによる確認

ランダムノイズ

確率過程$x(t)$が白色ノイズ過程であるとは、下記が成立することとします。

$$
E[x(t)] = 0 \\
E[x(t)x(t-\tau)] = \sigma^2 \delta(t-\tau)
$$

周波数領域に変換すると、$|S(f)|^2=\sigma^2$になります。さて、このとき$\sigma$の単位は何でしょうか？
$x(t)$の単位を[U]とすると、$\sigma$は[U /$\sqrt{T}$]
になります。

これを時間$T$の周期で離散化$x_n = x(t_n)$します。このとき、$x_n$はどういう分布に従うでしょうか。
今、$x_n$は $x(t)$と同じ単位$U$とすると、

$$
x_n \sim N(0, \sigma^2 T)
$$

になります。

次に

$$
\frac{dx(t)}{dt} = \nu(t)
$$

として、$\nu(t)$が白色ノイズ過程$E[\nu(t)]=0$、$E[\nu(t)\nu(t-\tau)]=\sigma^2 \delta(t-\tau)$　と仮定する場合の離散化を考えます。この場合は、ノイズ密度の大きさの単位は$U/T$となり、離散化すると

$$
x_n \sim N(0,\frac{\sigma^2}{T})
$$

になります。

ランダム加速度モデル

位置$p(t)$と速度$v(t)$があり、加速度については情報がない場合はこれを確率変数とします。

$$
\frac{d}{dt}p(t) = v(t) \\
\frac{d}{dt}v(t) = \nu(t)
$$

今、$\nu(t)$を白色ノイズ過程とします。いくつかサンプルを生成してみます。

import numpy as np

sig0 = 0.15
Fs = 200
t_end = 3.0

pos_init, vel_init = 0.0, 0.0
dt = 1.0 / Fs

def get_stochastic_proc():
    stoproc = {'time':[], 'acc':[], 'vel': [], 'pos': []}
    v, p = vel_init, pos_init
    t = 0
    while True:
        _acc = sig0 * np.random.normal() / np.sqrt(dt)
        if t > t_end:
            break
        v = v + _acc * dt
        p = p + v * dt
        stoproc['time'].append(t)
        stoproc['acc'].append(_acc)
        stoproc['vel'].append(v)
        stoproc['pos'].append(p)
        t = t + dt
    return stoproc

このとき、

$$
Var[v(t)] = \sigma^2 t　\\
Var[p(t)] = \frac{1}{3}\sigma^2 t^3 \\
Cov[v(t),p(t)] = \frac{1}{2}\sigma^2 t^3
$$

となるようです。下記、シミュレーションの結果です。

という訳で、位置速度を状態変数とする場合のプロセスノイズは

$$
\boldsymbol{Q}=
\left[
\begin{array}{cc}
\frac{1}{3} \sigma^2 T^3 & \frac{1}{2}\sigma^2T^2 \cr
\frac{1}{2}\sigma^2 T^2 & \sigma^2T
\end{array}
\right]
$$

とするのが良さそうです。

ランダム項をjerk にした場合のモデル

加速度の時間微分、すなわち、位置の時間の３階微分をjerk と呼ぶそうです。

$$
\frac{d}{dt}p(t) = v(t) \\
\frac{d}{dt}v(t) = a(t) \\
\frac{d}{dt}a(t) = \nu(t) \
$$

確率過程は下記です。

\begin{align}
E[\nu(t)] &=& 0\\
E[\nu(t)\nu(t-\tau)] &=& \sigma^2 \delta(t-\tau)
\end{align}

離散化して、いくつかサンプルを生成してみます。

です。この場合も離散化して数値積分を実行すると、下記が確認できました。

$$
Var[a(t)] = \sigma^2 t\\
Var[v(t)] = \frac{1}{3}\sigma^2 t^3\\
Var[p(t)] = \frac{1}{20}\sigma^2 t^5\\
Cov[a(t),v(t)] = \frac{1}{2}\sigma^2 t^2 \\
Cov[a(t),p(t)] = \frac{1}{6}\sigma^2 t^3 \\
Cov[v(t),p(t)] = \frac{1}{8}\sigma^2 t^4
$$

というわけで、

$$
\boldsymbol{Q}=
\left[
\begin{array}{ccc}
\frac{1}{20} \sigma^2 T^5 & \frac{1}{8}\sigma^2 T^4 & \frac{1}{6}\sigma^2T^3 \cr
\frac{1}{8} \sigma^2 T^4 & \frac{1}{3}\sigma^2T^3 & \frac{1}{2}\sigma^2 T^2\cr
\frac{1}{6}\sigma^2 T^3 & \frac{1}{2}\sigma^2 T^2 & \sigma^2T
\end{array}
\right]
$$

と試してみようと思います。

まとめ

というわけで、カルマンフィルタで使うプロセスノイズの共分散行列の式について理論を確認するために~~理論計算できないところを~~ 数値計算できるところをシミュレーションで確認しました。
さて、これでカルマンフィルタはうまく動くかな。。。
(2023/07/02)

読んでないけどメモ。

https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python