前回 : カルマンフィルタの基礎第3回 - ma-hi-ro Hobby

カルマンフィルタの基礎第4回

第1回、第3回を受けて、ようやくカルマンフィルタの設計を行う
今回は、以下の1入力1出力(SISO)の線形時不変の状態空間モデルでの状態ベクトル $x(k)$ を推定することを考える

$\displaystyle \begin{aligned} x(k+1) &= A x(k) + b \nu (k) \\ y(k) &= c^T x(k) + \omega (k) \end{aligned}$

カルマンフィルタの基礎第4回

事前推定値と事後推定値

カルマンフィルタでは、いくつかステップを踏んで推定を行う
推定値については、以下の推定値を求めていく

事前推定値
- 前回( $k-1$ )までの推定結果から求める予測推定値
事後推定値
- $k$ における観測値 $y(k)$ を用いたフィルタリング推定値

カルマンゲインの決定法

線形予測器の構成と事後推定値

最終的に求めたい事後推定値 $\hat{x}(k)$ を、
事前推定値 $\hat{x}^-(k)$ と観測値 $y(k)$ から求めることを考える

$\displaystyle \hat{x}(k) = G(k) \hat{x}^-(k) + g(k) y(k)$

この $G(k)$ 、 $g(k)$ を求めることを考える

まずは、条件として、事後推定誤差 $\tilde{x}(k)$ ( $= x(k) - \hat{x}(k)$ ) が観測値 $y(i)$ と直交することから求める

$\displaystyle E[\tilde{x}(k) y(i)] = 0, \quad i = 1, 2, \ldots , k$

下記の条件を使用する

$\displaystyle \begin{cases} \begin{aligned} \hat{x}(k) &= G(k) \hat{x}^-(k) + g(k) y(k) \\ y(k) &= c^T x(k) + \omega (k) \\ E[\omega (k) y(i)] &= 0 \\ E[x(k) y(i)] &\neq 0 \end{aligned} \end{cases}$

結局、下記の結果を得る

$\displaystyle G(k) = I - g(k) c^T$

ここまでで、事後推定値 $\hat{x}(k)$ は下記になる

$\displaystyle \hat{x}(k) = \hat{x}^-(k) + g(k) (y(k) - c^T \hat{x}^-(k))$

$G(k)$ が消去されて、カルマンゲイン $g(k)$ だけで求めることができる

カルマンゲインの算出

カルマンゲイン $g(k)$ を求めるために、
今度は事後推定誤差 $\tilde{x}(k)$ と出力予測誤差 $\tilde{y}(k)$ ( $= y(k) - \hat{y}^-(k) = y(k) - c^T \hat{x}^-(k)$ ) が直交することから求める

$\displaystyle \begin{aligned} E[\tilde{x}(k) \tilde{y}(k)] &= 0 \end{aligned}$

下記の条件を使用する

$\displaystyle \begin{cases} \begin{aligned} \hat{x}(k) &= \hat{x}^-(k) + g(k) (y(k) - c^T \hat{x}^-(k)) \\ y(k) &= c^T x(k) + \omega (k) \\ \tilde{x}^-(k) &= x(k) - \hat{x}^-(k) \\ E[\tilde{x}^-(k) \omega (k)] &= 0 \\ c^T \tilde{x}^-(k) &= (\tilde{x}^-(k))^T c \\ \end{aligned} \end{cases}$

まずは、 $\tilde{x}(k)$ 、 $\tilde{y}(k)$ を変換すると

$\displaystyle \begin{aligned} \tilde{x}(k) &= (I - g(k) c^T) \tilde{x}^-(k) - g(k) \omega (k) \\ \tilde{y}(k) &= c^T \tilde{x}^-(k) + \omega (k) \\ \end{aligned}$

次に、条件を求めると

$\displaystyle \begin{aligned} E[\tilde{x}(k) \tilde{y}(k)] &= E[\{ (I - g(k) c^T) \tilde{x}^-(k) - g(k) \omega (k) \} \{ c^T \tilde{x}^-(k) + \omega (k) \}] \\ &= E[(I - g(k) c^T) \tilde{x}^-(k) (\tilde{x}^-(k))^T c - g(k) \omega^2 (k)] \\ &= (I - g(k) c^T) P^-(k) c - g(k) \sigma_\omega^2 \\ \end{aligned}$

ここで、事前共分散行列を下記のように定義した

$\displaystyle P^-(k) = E[\tilde{x}^-(k) (\tilde{x}^-(k))^T]$

さらに続けて $g(k)$ を求めると

$\displaystyle \begin{aligned} E[\tilde{x}(k) \tilde{y}(k)] &= 0 \\ (I - g(k) c^T) P^-(k) c - g(k) \sigma_\omega^2 &= 0 \\ (I - g(k) c^T) P^-(k) c &= g(k) \sigma_\omega^2 \\ g(k) &= \frac{P^-(k) c}{c^T P^-(k) c + \sigma_\omega^2} \\ \end{aligned}$

つまり、事前共分散行列 $P^-(k)$ を求めることができれば、カルマンゲイン $g(k)$ を設定できる

共分散行列の更新

事前共分散行列の算出

次に、先ほどの事前共分散行列 $P^-(k)$ を、
前回の事後共分散行列 $P(k-1)$ から求めることを考える

下記の条件を使用する

$\displaystyle \begin{cases} \begin{aligned} P^-(k) &= E[\tilde{x}^-(k)(\tilde{x}^-(k))^T] \\ P(k-1) &= E[\tilde{x}(k-1)(\tilde{x}(k-1))^T] \\ x(k) &= A x(k-1) + b \nu (k-1) \\ E[\tilde{x}(k-1) \nu (k-1)] &= 0\\ \end{aligned} \end{cases}$

また、事前推定値 $\hat{x}^-(k)$ は、前回の事後推定値 $\hat{x}(k-1)$ と状態モデルの $A$ から下記のように求める

$\displaystyle \hat{x}^-(k) = A \hat{x}(k-1)$

さらに、 $\tilde{x}^-(k)$ を変換すると、

$\displaystyle \begin{aligned} \tilde{x}^-(k) &= x(k) - \hat{x}^-(k) \\ &= A x(k-1) + b \nu (k-1) - A \hat{x}(k-1) \\ &= A \tilde{x}(k-1) + b \nu (k-1) \\ \end{aligned}$

結局、

$\displaystyle \begin{aligned} P^-(k) &= E[\{A \tilde{x}(k-1) + b \nu (k-1)\} \{A \tilde{x}(k-1) + b \nu (k-1)\}^T] \\ &= A E[\tilde{x}(k-1) (\tilde{x}(k-1))^T] A^T + b E[\nu (k-1) \nu (k-1)^T] b^T \\ &= A P(k-1) A^T + \sigma_\nu^2 b b^T \\ \end{aligned}$

を得ることができる

事後共分散行列の算出

最後に、事前共分散行列 $P^-(k)$ から事後共分散行列 $P(k) = E[{\tilde{x}(k)} {\tilde{x}(k)}^T]$ を求める

先に、 $\tilde{x}(k)$ を変換すると、

$\displaystyle \begin{aligned} \tilde{x}(k) &= x(k) - \hat{x}(k) \\ &= x(k) - \hat{x}^-(k) - g(k) (y(k) - c^T \hat{x}^-(k)) \\ &= x(k) - \hat{x}^-(k) - g(k) (c^T x(k) + \omega (k) - c^T \hat{x}^-(k)) \\ &= \tilde{x}^-(k) - g(k) (c^T \tilde{x}^-(k) + \omega (k)) \\ &= (I - g(k) c^T) \tilde{x}^-(k) - g(k) \omega (k) \\ \end{aligned}$

結局、

$\displaystyle \begin{aligned} P(k) &= E[\{\tilde{x}(k)\} \{\tilde{x}(k)\}^T] \\ &= E[\{(I - g(k) c^T) \tilde{x}^-(k) - g(k) \omega (k)\} \{(I - g(k) c^T) \tilde{x}^-(k) - g(k) \omega (k)\}^T] \\ &= E[\{(I - g(k) c^T) \tilde{x}^-(k)\} \{(I - g(k) c^T) \tilde{x}^-(k)\}^T] + E[\{g(k) \omega (k)\} \{g(k) \omega (k)\}^T] \\ &= E[\{(I - g(k) c^T) \tilde{x}^-(k)\} \{(\tilde{x}^-(k))^T - (\tilde{x}^-(k))^T c g^T(k) \}] + g(k) E[\omega (k) \omega^T(k)] g^T(k) \\ &= (I - g(k) c^T) E[\tilde{x}^-(k) (\tilde{x}^-(k))^T] - (I - g(k) c^T) E[\tilde{x}^-(k) (\tilde{x}^-(k))^T] c g^T(k) + \sigma_\omega^2 g(k) g^T(k) \\ &= (I - g(k) c^T) P^-(k) - (I - g(k) c^T) P^-(k) c g^T(k) + \sigma_\omega^2 g(k) g^T(k) \\ &= (I - g(k) c^T) P^-(k) - \sigma_\omega^2 g(k) g^T(k) + \sigma_\omega^2 g(k) g^T(k) \\ &= (I - g(k) c^T) P^-(k) \\ \end{aligned}$

となる

途中で、カルマンゲイン $g(k)$ の算出時に出現した $(I - g(k) c^T) P^-(k) c = \sigma_\omega^2 g(k)$ を使用した

カルマンフィルタのアルゴリズム

カルマンフィルタのアルゴリズムを整理する

予測ステップ

初期値 or 前回の事後推定値 $\hat{x}(k-1)$ と事後共分散行列 $P(k-1)$ から、
今回の事前推定値 $\hat{x}^-(k)$ と事前共分散行列 $P^-(k)$ を求める

$\displaystyle \begin{aligned} \hat{x}^-(k) &= A \hat{x}(k-1) \\ P^-(k) &= A P(k-1) A^T + \sigma_\nu^2 b b^T \\ \end{aligned}$

フィルタリングステップ

求めた事前推定値 $\hat{x}^-(k)$ と事前共分散行列 $P^-(k)$ から、
カルマンゲイン $g(k)$ と事後推定値 $\hat{x}(k)$ を求める
また、次回のために、事後共分散行列 $P(k-1)$ も算出する

$\displaystyle \begin{aligned} g(k) &= \frac{P^-(k) c}{c^T P^-(k) c + \sigma_\omega^2} \\ \hat{x}(k) &= \hat{x}^-(k) + g(k) (y(k) - c^T \hat{x}^-(k)) \\ P(k) &= (I - g(k) c^T) P^-(k) \\ \end{aligned}$