ここで、 $x \in R^n$ , $y \in R^m$ , $w \in R^{d_1}$ , $v \in R^{d_2}$ , $R(t)R^T(t) > 0$ とし、 $w(t)$ と $v(t)$ は互いに独立で、それぞれ共分散マトリクス $Q(t)$ と $I$ を持つものとします。このとき、クスナー方程式(6.26)は以下の式(6.37)のようになります。

dp = -p \: \text{tr}(A)dt - x^TA^T\frac{\partial p}{\partial x}dt + \frac{1}{2}\text{tr}\left\{ GQG^T\frac{\partial}{\partial x}\left(\frac{\partial p}{\partial x}\right)^T\right\}dt + p(x-\hat{x})^T H^T (RR^T)^{-1}\{dy_t-H\hat{x}dt\}

クスナー方程式と $\hat{h}$ は $\S6.2$ で以下のように定義されていました。

dp\{t,x|\mathcal{Y}_t\} = \mathcal{L}_x^* p\{t,x|\mathcal{Y}_t\} dt + p\{t,x|\mathcal{Y}_t\}[h(t,x)-\hat{h}(t,x)]^T (R_t R_t^T)^{-1} \{dy(t)-\hat{h}(t,x)dt\}

\hat{h}(t,x) = \int_{R^n} h(t,x) p\{t,x|\mathcal{Y}_t\} dx \tag{6.27}

さらに、 $\mathcal{L}_x^*$ は $\S4.6$ で以下のように定義されていました。

\mathcal{L}_x^*(\cdot) := -\sum_{i=1}^{n} \frac{\partial (\cdot f_i)}{\partial x_i} + \frac{1}{2}\sum_{i,j = 1}^{n}\frac{\partial^2 (\cdot [GQG^T]_{ij}}{\partial x_i \partial x_j} \tag{4.53}

クスナー方程式(6.26)から式(6.37)への変形はなんとなーく理解できるのですが、前向き拡散作用素 $\mathcal{L}_x^*$ の関係する項がなぜ二つに分裂するのかちょっとわからないです。

ここで、 $f=Ax, h=Hx$ であるので

\begin{aligned} \mathcal{E}\{x h^T\} - \hat{x}\hat{h}^T &= [\mathcal{E}\{xx^T|\mathcal{Y}_t\} - \hat{x}\hat{x}^T]H^T = P_tH^T \\ \mathcal{E}\{x f^T\} - \hat{x}\hat{f}^T &= P_t A^T \end{aligned}

ということがわかる。さらに、確率密度関数は正規性であるので、三次モーメント(歪度)は0となる。

\mathcal{E}\{(x_i - \hat{x}_i)(x_j - \hat{x}_j)(x_k - \hat{x}_k)|\mathcal{Y}_t\} = 0

以上を利用して式(6.32)を変形すると、以下の式(6.38)となる。

d\hat{x}(t|t) = A(t)\hat{x}(t|t)dt + P(t|t)H^T\{R(t)R^T(t)\}^{-1}\{dy(t)-H(t)\hat{x}(t|t)dt\}

さらに式(6.34)を変形すると以下の式(6.39)となる。

\dot{P}(t|t) = A(t)P(t|t) + P(t|t)A^T(t) + G(t)Q(t)G^T(t) - P(t|t)H^T(t)\{R(t)R^T(t)\}^{-1}H(t)P(t|t)

それぞれの初期値は以下のようになる。

\begin{aligned} \hat{x}(t_0|t_0) &= \mathcal{E}\{x(t_0) = \hat{x}_0\} \\ P(t_0|t_0) &= \mathcal{E}\{[x(t_0)-\hat{x}(t_0|t_0)][x(t_0)-\hat{x}(t_0|t_0)]^T\} = P_0\\ \end{aligned}

これをカルマンフィルターと呼びます。また、式(6.39)はマトリクスリッカチ微分方程式です。

やること

これまでのBotter自主ゼミノート

6.4 カルマンフィルタ

Discussion