カルマンフィルターは、観測できない確率変数(潜在変数)があるモデル(状態空間モデル)において、観測可能な確率変数に基づいて観測不可能な変数の状態を推定する手法です。カルマンフィルターは行列計算を用いて初等的に導出することも可能ですが、計算が結構複雑になる上に、逆行列の存在を仮定しなければならない部分もあったりと難点もあります。この記事では、ヒルベルト空間における直交射影の理論を用いてカルマンフィルターの漸化式を導出します。こちらのほうが理論がすっきりしており、数学の知識がある方には理解しやすいと思います。
問題設定
時刻$k=0,1,2,\cdots$に対し、$\bm x_k$を観測できない状態量、$\bm y_k$を観測可能な観測値、$\bm u_k$を既知の入力ベクトルとし、それらが以下の条件を満たしているとします。 \begin{align*} \begin{cases} \bm{x}_{k+1}=A_k\bm{x}_k+B_k\bm{u}_k+\bm{\xi}_k\\ \bm{y}_k=C_k \bm{x}_k+D_k \bm{u}_k+\bm{\eta}_k \end{cases}~~(k=1,2,\cdots) \end{align*}
ここで、
- 状態変数$\bm{x}_k$およびノイズ$\bm{\xi}_k$ は$\mathbb{R}^n$値確率変数、 観測ベクトル$\bm{y}_k$および ノイズ$\bm{\eta}_k$ は$\mathbb{R}^p$値確率変数 、入力ベクトル$\bm{u}_k \in \mathbb{R}^m$
- $A_k \in M_{n}(\mathbb{R}),B_k \in M_{m,n}(\mathbb{R}),C_k \in M_{n,p}(\mathbb{R}),D_k \in M_{m,p}(\mathbb{R})$
- $\bm{x_0},\bm{\xi}_1,\bm{\xi}_2,\cdots,\bm{\eta}_1,\bm{\eta}_2,\cdots$は独立な確率変数で、 \begin{align*} E[\bm \xi_k]=0,~V[\bm \xi_k]=Q_k,~E[\bm \eta_k]=0,~V[\bm \eta_k]=R_k \end{align*}
- さらに、 $R_k$は正定値であるとする。
この設定の下で、既知のパラメータ$A_k,B_k,C_k,D_k,Q_k,R_k$および観測値$\bm y_1,\bm y_2,\cdots,$から、$\bm x_k$たちの最適な推定量を構成する方法を考えます。さらに、ここでは初期値$\bm x_0$も既知であるとします。
まずは、入力$\bm u_k=0$の場合を考えます。この場合、$\bm x_k,\bm y_k$の満たす方程式は、\begin{align*} \begin{cases} \bm{x}_{k+1}=A_k\bm{x}_k+\bm{\xi}_k\\ \bm{y}_k=C_k \bm{x}_k+\bm{\eta}_k \end{cases}~~(k=1,2,\cdots) \end{align*}となります。
直交射影による推定量の定義
ここでは、2つの確率変数ベクトルの「内積」もどきを定義し、直交射影を構成します。 以下では、行列$A$に対しその転地行列を$A’$で表します。また、ほとんどいたるところ等しい確率変数は同一視することにします。
定義
$\mathbb{R}^{d_1}$値確率変数$\bm{x}$、$\mathbb{R}^{d_2}$値確率変数$\bm{y}$に対し、$(\bm{x},\bm{y})$および$\|x\|$を \begin{align*} (\bm{x},\bm{y})=E[\bm{x}\bm{y}’],~\|\bm x\|=(\bm{x},\bm{x})^{\frac{1}{2}} \end{align*} と定める。
ここで、$(\bm{x},\bm{y})$はスカラーではなく行列であることに注意します。 これに対しても、次に示すような内積に似た性質が成り立ちます。
命題1
$\mathbb{R}^{d_1}$値確率変数$\bm{x},\bm{x}_1,\bm{x}_2$、$\mathbb{R}^{d_2}$値確率変数$\bm{y},\bm{y}_1,\bm{y}_2$、$A \in M_{n,d_1}(\mathbb{R}),B \in M_{n,d_2}(\mathbb{R})$に対し、次が成り立つ。
(1) $(\bm y,\bm x)=(\bm x,\bm y)’$
(2) $(\bm x_1+\bm x_2,\bm y)=(\bm x_1,\bm y)+(\bm x_2,\bm y),~(\bm x,\bm y_1+\bm y_2)=(\bm x,\bm y_1)+(\bm x,\bm y_2)$
(3) $(A\bm x,\bm y)=A(\bm x,\bm y),~(\bm x,B\bm y)=(\bm x,\bm y)B’$
(4) $\mathrm{tr}\|x\|=0~\Longleftrightarrow~\bm x=0$
(証明) (1) $(\bm y,\bm x)=E[\bm y\bm x’]=E[\bm x\bm y’]’=(x,y)’$
(2) \begin{align*} (\bm x_1+\bm x_2,\bm y)&=E[(\bm x_1-\bm x_2)\bm y’]=E[\bm x_1\bm y’]+E[\bm x_2\bm y’]\\ &=(\bm x_1,\bm y)+(\bm x_2,\bm y) \end{align*} であり、第2式も同様である。
(3) \begin{align*} (A\bm x,\bm y)=E[(A\bm x\bm y’]=AE[\bm x\bm y’]=A(\bm x,y) \end{align*} であり、また(1)より、 \begin{align*} (\bm x,B\bm y)=(B\bm y,\bm x)’=\{B(\bm y,\bm x)\}’=(\bm x,\bm y)B’ \end{align*} である。
(4) $\mathrm{tr}\|x\|=\mathrm{tr}E[\bm x \bm x’]=\mathrm{tr}E[\bm x’ \bm x]=E[|\bm x|^2]$だから示すべきことを得る。 □
次に、\begin{align*} L_k=\left\{A_0\bm x_0+\sum_{j=1}^kA_j\bm{y}_k;A_0 \in M_{n,n}(\mathbb{R}),A_j \in M_{n,p}(\mathbb{R})\right\},~L_{-1}=\{0\} \end{align*} と置きます。
上の命題より、$\mathbb{R}^n$値確率変数全体は$\mathrm{tr}(\cdot,\cdot)$を内積とするHilbert空間で、$L_k$はその閉部分空間となるので、直交射影の存在と一意性から次の命題が得られます。
命題2
$\mathbb{R}^n$値確率変数$\bm{x}$に対し、$\bm{y}\in L_k$で \begin{align*} \mathrm{tr}\|\bm{y}-\bm{x}\|^2=E[|\bm{y}-\bm{x}|^2] \end{align*} を最小にするもの($\bm x$の$L_k$への直交射影)が一意的に存在する。
以下では、これを$P_k\bm x$と書く。特に$P_{-1}\bm x=0$である。
これを用いて、「観測値$\bm y_1,\cdots,\bm y_j$に基づく$\bm x_k$の推定量」$\bm x_{k|j}$を次のように定義します。
定義
$k,j=0,1,2,\cdots$に対し、$\hat{\bm x}_{k|j}$を \begin{align*} \hat{\bm x}_{k|j}=P_{j}\bm{x}_k \end{align*} によって定める。
これは、$\bm x_0$および$\bm{y}_1,\cdots,\bm{y}_j$の線形結合で表される$\bm{x}_k$の推定量のうち、平均2乗誤差を最小にするものということです。
入力がない場合のカルマンフィルタ
次に、上で構成した推定量$\bm x_{k|j}$を具体的に計算するための式を導きます。以下では、時刻が進むたびに$\bm y_1,\bm y_2,\cdots$たちが順次観測されてゆき、$\bm y_k$が観測された時点で、その時の状態量$\bm x_k$を推定したい状況を考えます。つまり、$\bm x_{k|k}$を知りたいということです。
この時、時刻$k$において新たに観測された$\bm y_k$と、 時刻$k-1$で求めた$\bm x_{k-1|k-1}$の値を使って、新たな推定量$\bm x_{k|k}$を計算するような漸化式が得られれば、計算のコストが省けてうれしいわけです。そのような漸化式を導くために、いくつかの補題を証明します。いずれも補題も、直交射影の性質から類推されるものです。
以下記述を簡単にするため、$\bm y_0=\bm x_0$と定めます。
補題1
$\hat y \in L_k$および$\mathbb{R}^n$値確率変数$\bm{x}$に対し、次は同値である。
(1) $\hat{\bm y}=P_k\bm x$
(2) $j=0,1,\cdots,k$に対し、$(\hat{\bm{y}}-\bm{x},\bm{y}_j)=O$
(証明) $(1)\Longrightarrow (2):$背理法で示す。(1)を満たす$\hat{\bm y}$に対し、ある$0\leq j\leq k$が存在して$(\hat{\bm{y}}-\bm{x},\bm{y}_j)\neq O$であるとする。この時、$\bm{y}_j \neq 0$であるから、$\mathrm{tr}\|\bm y_j\|^2=E[|\bm y_j|^2]>0$である。そこで、 \begin{align*} \bm{y}=\hat{\bm y}-\frac{(\hat{\bm{y}}-\bm{x},\bm{y}_j)}{\mathrm{tr}\|\bm y_j\|^2}\bm{y}_j \end{align*} と置くと、 \begin{align*} \mathrm{tr}\|\bm{y}-\bm{x}\|^2=&\mathrm{tr}\left\|\hat{\bm y}-\bm{x}-\frac{(\hat{\bm{y}}-\bm{x},\bm{y}_j)}{\mathrm{tr}\|\bm y_j\|^2}\bm{y}_j\right\|^2\\ =&\mathrm{tr}\|\hat{\bm y}-\bm{x}\|^2-\mathrm{tr}\left(\frac{(\hat{\bm{y}}-\bm{x},\bm{y}_j)}{\mathrm{tr}\|\bm y_j\|^2}\bm{y}_j,\hat{\bm y}-\bm{x}\right)\\ &-\mathrm{tr}\left(\hat{\bm y}-\bm{x},\frac{(\hat{\bm{y}}-\bm{x},\bm{y}_j)}{\mathrm{tr}\|\bm y_j\|^2}\bm{y}_j\right) +\mathrm{tr}\left\|\frac{(\hat{\bm{y}}-\bm{x},\bm{y}_j)}{\mathrm{tr}\|\bm y_j\|^2}\bm{y}_j\right\|^2\\ =&\mathrm{tr}\|\hat{\bm y}-\bm{x}\|^2-\frac{\mathrm{tr}(\hat{\bm{y}}-\bm{x},\bm{y}_j)(\bm{y}_j,\hat{\bm y}-\bm{x})}{\mathrm{tr}\|\bm y_j\|^2} -\frac{\mathrm{tr}(\hat{\bm{y}}-\bm{x},\bm{y}_j)(\hat{\bm y}-\bm{x},\bm{y}_j)’}{\mathrm{tr}\|\bm y_j\|^2}\\ &+\frac{\mathrm{tr}(\hat{\bm{y}}-\bm{x},\bm{y}_j)(\hat{\bm y}-\bm{x},\bm{y}_j)’}{\mathrm{tr}\|\bm y_j\|^2}\\ =&\mathrm{tr}\|\hat{\bm y}-\bm{x}\|^2-\frac{\mathrm{tr}(\hat{\bm{y}}-\bm{x},\bm{y}_j)(\hat{\bm y}-\bm{x},\bm{y}_j)’}{\mathrm{tr}\|\bm y_j\|^2} \end{align*} であるが、$(\hat{\bm{y}}-\bm{x},\bm{y}_j)\neq O$から上と同様に$\mathrm{tr}(\hat{\bm{y}}-\bm{x},\bm{y}_j)(\hat{\bm y}-\bm{x},\bm{y}_j)’>0$となり、(1)の仮定に反する。よって、(2)が成り立つ。
$(2)\Longrightarrow (1):$$\bm y\in L_k$とすると、仮定より$(\hat{\bm y}-\bm x,\bm y-\hat{\bm y})=O$であるから、 \begin{align*} \mathrm{tr}\|\bm y-\bm x\|^2=\mathrm{tr}\|\hat{\bm y}-\bm x+\bm y-\hat{\bm y}\|^2 =\mathrm{tr}\|\hat{\bm y}-\bm x\|^2+\mathrm{tr}\|\bm y-\hat{\bm y}\|^2 \geq \mathrm{tr}\|\hat{\bm y}-\bm x\|^2 \end{align*} となり(1)が成り立つ。 □
上の補題により、次のような結果が得られます。
補題2
(1) 任意の$A \in M_{n,p}(\mathbb{R})$および$k=0,1,2,\cdots$に対し、 \begin{align*} P_kA\bm x=AP_k\bm x \end{align*} が成り立つ。
(2) 任意の$0\leq j \leq k$に対し、$P_j\bm \xi_k=0$が成り立つ。
(証明) (1) 任意の$j=0,1,\cdots,k$に対し、 \begin{align*} (AP_k\bm x,\bm y_j)=A(P_k\bm x,\bm y_j)=O \end{align*} だから上の補題により示すべきことを得る。
(2) (2) 任意の$i=0,1,\cdots,j$に対し、$\bm y_i$は$\bm \xi_i-1$までの式で表されるから、$\bm \xi_k$とは独立である。よって、$(\bm \xi_k,\bm y_i)=0$となるから、上の補題により結論が従う。 □
次に、$\{\bm y_k\}$の「直交化」であるイノベーション過程を定義します。
定義
\begin{align*} \bm z_k=\bm y_k-C_kP_{k-1}\bm{x}_k=\bm y_k-C_k\hat{\bm x}_{k|k-1} \end{align*} で定まる$\{\bm z_k\}$をイノベーション過程と呼ぶ。特に、$P_{-1}=O$により、$\bm z_0=\bm y_0=\bm x_0$である。
次の補題により、これが$\{\bm y_k\}$の「直交化」になっていることがわかります。
補題3
イノベーション過程$\{\bm z_k\}$に対し、 \begin{align*} &(\bm z_i,\bm z_j)=\begin{cases} C_i\|x_i-P_{i-1}\bm{x}_i\|^2C_i’+R_i&(i=j\geq 1)\\ O&(i\neq j) \end{cases}\\ &H_k=\left\{\sum_{j=1}^kA_j\bm{z}_k;A_0 \in M_{n,n}(\mathbb{R}),A_j \in M_{n,p}(\mathbb{R})\right\} \end{align*} である。
(証明) \begin{align*} \bm z_k=C_k\bm x_k+\bm \eta_k-C_kP_{k-1}\bm{x}_k=C_k(\bm x_k-P_{k-1}\bm{x}_k)+\bm \eta_k \end{align*} であるから、$i=j$の時$\bm x_i-P_{i-1}\bm{x}_i$と$\eta_i$の独立性により、 \begin{align*} (\bm z_i,\bm z_i)=\|C_i(\bm x_i-P_{i-1}\bm{x}_i)\|^2+\|\bm \eta_i\|^2=C_i\|x_i-P_{i-1}\bm{x}_i\|^2C_i’+R_i \end{align*} となる。一方$i<j$とすると、$\bm \eta_j$は$\bm \eta_i$および$\bm x_i-P_{i-1}\bm{x}_i$と独立であるから、 \begin{align*} (\bm z_i,\bm z_j)&=(C_i(\bm x_i-P_{i-1}\bm{x}_i)+\bm \eta_i,C_j(\bm x_j-P_{j-1}\bm{x}_j))\\ &=(\bm z_i,C_j(\bm x_j-P_{j-1}\bm{x}_j) =(\bm y_i-C_{i}P_{i-1}\bm x_i,C_j(\bm x_j-P_{j-1}\bm{x}_j))=O \end{align*} となる。最後の等号は、補題1より$(\bm y_i-C_{i}P_{i-1}\bm x_i,\bm x_j-P_{j-1}\bm{x}_j)=O$となることを用いた。よって前半の主張が示された。後半の主張は帰納的に明らかである。 □
上の補題により、$k \geq 1$の時$\|z_{k}\|^2$は半正定値＋正定値の形なので、正定値となることがわかります。そこで、$\bm{e}_k=\|\bm z_{k}\|^{-1}\bm z_k$と定めると、$\{\bm e_k\}_{k\geq 1}$は$\{\bm y_k\}_{k \geq 1}$の「正規直交化」となり、次の補題が成り立ちます。
補題4
$\bm{e}_k=\|\bm z_{k}\|^{-1}\bm z_k~(k \geq 1)$とすると、$\mathbb{R}^n$値確率変数$\bm{x}$に対し、 \begin{align*} P_{j}\bm x=P_0\bm x+\sum_{i=1}^j (\bm x,\bm e_i)\bm e_i \end{align*} が成り立つ。
(証明) $\displaystyle \bm y=P_0\bm x+\sum_{i=1}^j (\bm x_i,\bm e_i)\bm e_i$と置くと、前の補題より$\displaystyle H_j$であr任意の$\bm e_i~(1\leq i\leq j)$に対し、 \begin{align*} &(\bm x-\bm y,\bm e_i)=(\bm x,\bm e_j)-(\bm y,\bm e_j) =(\bm x,\bm e_i)-\{(P_0\bm x,\bm e_j)+(\bm x,\bm e_i)(\bm e_i,\bm e_i)\}=0\\ &(\bm x-\bm y,\bm x_0)=(P_0\bm x-\bm y,\bm x_0)=0 \end{align*} となる。これと前の補題により、任意の$\bm y_i~(0\leq i\leq j)$に対し、$(\bm x-{\bm y},\bm y_i)=0$となるから、補題1により$\bm y=P_{j}\bm x$である。 □
以上の準備の下で、$\bm x_{k|k}$を計算するための漸化式が証明できます。
定理5
$k\geq 1$に対し、$G_k=(\bm x_k,\bm e_k)\|\bm z_k\|^{-1}$と置くと、 \begin{align*} &\hat{\bm x}_{k|k-1}=A_{k-1}\hat{\bm x}_{k-1|k-1}\\ &\hat{\bm x}_{k|k}=\hat{\bm x}_{k|k-1}+G_k(\bm y_k-C_k \hat{\bm x}_{k|k-1}) \end{align*} が成り立つ。
(証明) 補題2により、 \begin{align*} \hat{\bm x}_{k|k-1}=P_{k-1}\bm x_k=P_{k-1}(A_{k-1}\bm x_{k-1}+\xi_{k-1})=A_{k-1}P_{k-1}\bm x_{k-1}=A_{k-1}\hat{\bm x}_{k-1|k-1} \end{align*} となるから最初の式が従う。
また、補題4により \begin{align*} \hat{\bm x}_{k|k}&=P_k\bm x_k=P_{k-1}\bm x_k+(\bm x_k,\bm e_k)\bm e_k\\ &=\bm{x}_{k|k-1}+(\bm x_k,\bm e_k)\|\bm z_k\|^{-1}\bm z_k =\bm{x}_{k|k-1}+(\bm x_k,\bm e_k)\|\bm z_k\|^{-1}(\bm y_k-C_k \hat{\bm x}_{k|k-1}) \end{align*} となり2式目も従う。
上で登場した$G_k$はカルマンゲインと呼ばれます。これを計算するための漸化式を求めます。
定理6
$P_{k,j}=\|\bm x_k-\hat{\bm x}_{k|j}\|^2$とすると、 \begin{align*} &P_{k,k-1}=A_{k-1}P_{k-1,k-1}A_{k-1}’+Q_{k-1}\\ &P_{k,k}=(I-G_kC_k)P_{k,k-1}\\ &G_k=P_{k,k-1}C_k'(C_kP_{k,k-1}C_k’+R_k)^{-1} \end{align*} が成り立つ。
(証明) 前の定理を用いると、$(\bm x_{k-1}-\hat{\bm x}_{k-1|k-1})$と$\bm \xi_{k-1}$の独立性により、 \begin{align*} P_{k,k-1}&=\|\bm x_{k}-\hat{\bm x}_{k|k-1}\|^2 =\|A(\bm x_{k-1}-\hat{\bm x}_{k-1|k-1})+\bm \xi_{k-1}\|^2\\ &=\|A(\bm x_{k-1}-\hat{\bm x}_{k-1|k-1})\|^2+\|\bm \xi_{k-1}\|^2 =A_{k-1}P_{k-1,k-1}A_{k-1}’+Q_{k-1} \end{align*} となり最初の式が得られる。
また、$\bm \eta_k$と$\bm x_k$の独立性および補題7から$(\bm x_k,\bm\eta_k)=0,(\bm x_{k|k-1},\bm x_k-\bm x_{k|k-1})=0$となることに注意すると、 \begin{align*} G_k&=(\bm x_k,\bm e_k)\|\bm z_k\|^{-1}=(\bm x_k,\bm z_k)\|\bm z_k\|^{-2} =(\bm x_k,\bm C_k(\bm x_k-\bm x_{k|k-1})+\bm \eta_k)\|\bm z_k\|^{-2}\\ &=(\bm x_k,\bm C_k(\bm x_k-\bm x_{k|k-1}))\|\bm z_k\|^{-2} =(\bm x_k-\bm x_{k|k-1},\bm C_k(\bm x_k-\bm x_{k|k-1}))\|\bm z_k\|^{-2}\\ &=\|\bm x_k-\bm x_{k|k-1}\|^2C_k'(\bm z_k,\bm z_k)^{-1}=P_{k,k-1}C_k'(C_kP_{k,k-1}C_k’+R_k)^{-1} \end{align*} となり、3番目の式を得る。
さらにこれより、 \begin{align*} P_{k,k} =&\|\bm x_k-\hat{\bm x}_{k|k}\|^2 =\|\bm x_k-\hat{\bm x}_{k|k-1}-G_k(\bm y_k-C_k \hat{\bm x}_{k|k-1})\|^2\\ =&\|\bm x_k-\hat{\bm x}_{k|k-1}-G_k(C_k\bm x_k-C_k \hat{\bm x}_{k|k-1}+\bm \eta_k)\|^2\\ =&\|(I-G_kC_k)(\bm x_k-\hat{\bm x}_{k|k-1})\|^2+\|G_k\bm\eta_k\|^2\\ =&(I-G_kC_k)P_{k,k-1}(I-G_kC_k)’+G_kR_kG_k’\\ =&(I-G_kC_k)P_{k,k-1}-P_{k,k-1}C_k’G_k’+G_kC_kP_{k,k-1}C_k’G_k’+G_kR_kG_k’\\ =&(I-G_kC_k)P_{k,k-1}-G_k(C_kP_{k,k-1}C_k’+R_k)G_k’\\ &+G_kC_kP_{k,k-1}C_k’G_k’+G_kR_kG_k’\\ =&(I-G_kC_k)P_{k,k-1} \end{align*} となって、2番目の式を得る。 □
以上により、$\bm x_{k|k}$を計算するための漸化式が得られました。定義により初期推定量$\bm{x}_{0|0}$は$\bm{x}_{0|0}=P_0\bm{x}_{0|0}=\bm x_0$となることも合わせると、次のような結果になります。
カルマンフィルターによる状態推定(入力$\bm u_k=0$の場合)
\begin{align*} &P_{k,k-1}=A_{k-1}P_{k-1,k-1}A_{k-1}’+Q_{k-1}\\ &P_{k,k}=(I-G_kC_k)P_{k,k-1}\\ &G_k=P_{k,k-1}C_k'(C_kP_{k,k-1}C_k’+R_k)^{-1}\\ &\hat{\bm x}_{0|0}=\bm x_0\\ &\hat{\bm{x}}_{k|k}=\hat{\bm{x}}_{k|k-1}+G_k(\bm{y}_k-C_k\hat{\bm{x}}_{k|k-1})\\ &\hat{\bm{x}}_{k|k-1}=A_{k-1}\hat{\bm{x}}_{k-1|k-1}\end{align*}
入力がある場合
次に$\bm u_k$が一般の場合を考えます。この時は\begin{align*} \begin{cases} \bm x_0^{(1)}=\bm x_0,&\bm x_{k+1}^{(1)}=A_k\bm x_{k}^{(1)}+\bm \xi_k\\ \bm x_1^{(1)}=0,&\bm x_{k+1}^{(2)}=A_k\bm x_{k}^{(2)}+C_k\bm u_k \end{cases},~ \begin{cases} \bm y_{k}^{(1)}=B_k\bm x_{k}^{(1)}+\bm \eta_k\\ \bm y_{k}^{(2)}=B_k\bm x_{k}^{(2)}+D_k\bm u_k \end{cases}\end{align*}と定めると、\begin{align*} \bm x_k=\bm x_k^{(1)}+\bm x_k^{(2)},~\bm y_k=\bm y_k^{(1)}+\bm y_k^{(2)}\end{align*}となります。今$\bm x_k^{(2)}$については直接計算することができますので、$\bm x_k^{(1)}$の値を推定できれば十分です。そこで、前節で$\bm x_k \to \bm x_k^{(1)},\bm y_k \to \bm y_k^{(1)}$として得られる推定量を$\hat{\bm{x}}_{k|j}^{(1)}$とし、\begin{align*} \hat{\bm{x}}_{k|j}=\hat{\bm{x}}_{k|j}^{(1)}+\bm{x}_k^{(2)}\end{align*}と定めると、定理5の$G_k$に対し、\begin{align*} \hat{\bm{x}}_{k|k}&=\hat{\bm{x}}_{k|k}^{(1)}+\bm{x}_k^{(2)} =\hat{\bm{x}}_{k|k-1}^{(1)}+G_k(\bm{y}_k^{(1)}-C_k\hat{\bm{x}}_{k|k-1}^{(1)})+\bm{x}_k^{(2)}\\ &=\hat{\bm{x}}_{k|k-1}+G_k\{(\bm{y}_k-\bm{y}_k^{(2)})-C_k(\hat{\bm{x}}_{k|k-1}-\hat{\bm{x}}_{k|k-1}^{(2)})\}\\ &=\hat{\bm{x}}_{k|k-1}+G_k(\bm{y}_k-C_k\hat{\bm{x}}_{k|k-1}-D_k\bm u_k)\\ \hat{\bm{x}}_{k|k-1}&=\hat{\bm{x}}_{k-1|k}^{(1)}+\bm{x}_k^{(2)} =A_{k-1}\hat{\bm{x}}_{k-1|k-1}^{(1)}+\bm{x}_k^{(2)}\\ &=A_{k-1}(\hat{\bm{x}}_{k-1|k-1}-\bm{x}_{k-1}^{(2)})+A_{k-1}\bm{x}_{k-1}^{(2)}+C_{k-1}\bm u_{k-1}\\ &=A_{k-1}\hat{\bm{x}}_{k-1|k-1}+C_{k-1}\bm u_{k-1}\end{align*}となります。以上をまとめると、次の結果になります。
カルマンフィルターによる状態推定(入力$\bm u_k$がある場合)
\begin{align*} &P_{k,k-1}=A_{k-1}P_{k-1,k-1}A_{k-1}’+Q_{k-1}\\ &P_{k,k}=(I-G_kC_k)P_{k,k-1}\\ &G_k=P_{k,k-1}C_k'(C_kP_{k,k-1}C_k’+R_k)^{-1}\\ &\hat{\bm x}_{0|0}=\bm x_0\\ &\hat{\bm{x}}_{k|k}=\hat{\bm{x}}_{k|k-1}+G_k(\bm{y}_k-C_k\hat{\bm{x}}_{k|k-1}-D_k\bm u_k)\\ &\hat{\bm{x}}_{k|k-1}=A_{k-1}\hat{\bm{x}}_{k-1|k-1}+C_{k-1}\bm u_{k-1}\end{align*}
$P_{k,j}$や$G_k$は入力があってもなくても同じ値になります。さらに、これらは観測データ$\bm y_k$がなくても事前に計算することができます。そのため、$G_k$をあらかじめ計算しておき、$\bm y_k$が観測された時点で$\bm x_k$を計算することにより、計算リソースを節約することができます。
