1 ゼロ点除去問題のメモ 1

x⃗_{k+1} = A_{k}x⃗_{k}+ B_{k}w⃗  (1)
z⃗_{k}= H_{k}x⃗_{k}+v⃗  (2)

E[w⃗] = μ⃗ ≠ 0  (3)
V [w⃗] ≡ E[(w⃗− μ⃗)(w⃗− μ⃗)^{T}] = Q  (4)
E[v⃗] = 0  (5)
V [v⃗] ≡ E[(v⃗− 0)(v⃗− 0)^{T}] = R  (6)

E[w⃗_{modified}] ≡ 0  (7)
V [w⃗_{modified}] ≡ V [w] = Q  (8)

x⃗_{k+1} = A_{k}x⃗_{k}+ B_{k}w⃗≡ A_{k}x⃗_{k}+ B_{k}μ⃗_{k} + B_{k}w⃗_{modified}  (9)

[ x⃗ ; μ⃗]_{k+1} = [ \begin{matrix}A_{k} & B_{k} \\ 0 & I \end{matrix}] [ x⃗ ; μ⃗]_{k}+ [ \begin{matrix}B \\ 0 \end{matrix}] w⃗_{modified}  (10)

P_{extended} ≡ V [[ x⃗ ; μ⃗]] = [ \begin{matrix}P & 0 \\ 0 & S \end{matrix}]  (11)

P_{extendedk+1} = [ \begin{matrix}A_{k} & B_{k} \\ 0 & I \end{matrix}] P_{extendedk}[ \begin{matrix}A_{k} & B_{k} \\ 0 & I \end{matrix}]^{T}+ [ \begin{matrix}B_{k} \\ 0 \end{matrix}] Q[ \begin{matrix}B_{k} \\ 0 \end{matrix}]^{T}

= [ \begin{matrix}A_{k} & B_{k} \\ 0 & I \end{matrix}] [ \begin{matrix}P_{k} & 0 \\ 0 & S \end{matrix}] [ \begin{matrix}A_{k}^{T} & 0 \\ B_{k}^{T} & I \end{matrix}] + [ \begin{matrix}B_{k} \\ 0 \end{matrix}] Q[B_{k}^{T} 0]

= [ \begin{matrix}AP_{k}A^{T}+ B_{k}SB_{k}^{T} & B_{k}S \\ SB_{k}^{T} & S \end{matrix}] + [ \begin{matrix}B_{k}QB_{k}^{T} & 0 \\ 0 & 0 \end{matrix}]

= [ \begin{matrix}P_{k+1} + B_{k}SB_{k}^{T} & B_{k}S \\ SB_{k}^{T} & S \end{matrix}]

(12)

z⃗_{k}= [ \begin{matrix}H & 0 \\ 0 & 0 \end{matrix}] [ x⃗ ; μ⃗]_{k}+ [ \begin{matrix}I \\ 0 \end{matrix}] v⃗  (13)

2 ゼロ点除去問題のメモ 2

Kalman Filter においてモデル化されていないバイアス成分\underline{μ} が混入していた場合、すなわち

¯\underline{x}_{k+1} = Φ_{k}ˆ\underline{x}_{k}+ Γ_{k}( \underline{μ}_{k}+ \underline{u}_{ideal k})  (14)

における影響を調べる。

定義は以下のとおり。

E [\underline{μ}_{k}] ≈ \underline{μ}_{k}  (15)
V [\underline{μ}_{k}] ≈ \underline{0}  (16)
E [\underline{μ}_{k}\underline{u}_{ideal k}^{T}] = \underline{0}  (17)
V [\underline{μ}_{k}+ \underline{u}_{ideal k}] = V [\underline{u}_{ideal k}]= Q  (18)
¯\underline{x}_{nobias k+1} ≡ Φ_{k}ˆ\underline{x}_{k}+ Γ_{k}\underline{u}_{ideal k}  (19)
ˆ\underline{x}_{ideal 0} ≡ ˆ \underline{x}_{0}  (20)
ˆ\underline{x}_{ideal k+1} ≡ (Φ_{k}ˆ\underline{x}_{ideal k}+ Γ_{k}\underline{u}_{ideal k}) + K_{k+1} (\underline{z}_{k+1} − H_{k+1} (Φ_{k}ˆ\underline{x}_{ideal k}+ Γ_{k}\underline{u}_{ideal k}) )  (21)
¯\underline{x}_{ideal k+1} ≡ Φ_{k}ˆ\underline{x}_{ideal k}+ Γ_{k}\underline{u}_{ideal k}  (22)
E [\underline{z}_{k+1} − H_{k+1} ¯ \underline{x}_{ideal k+1}] = \underline{0}  (23)
V [\underline{z}_{k+1} − H_{k+1} ¯ \underline{x}_{ideal k+1}] = R  (24)

これらの定義から

ˆ\underline{x}_{k+1} = ¯ \underline{x}_{k+1} + K_{k+1} (\underline{z}_{k+1} − H_{k+1} ¯ \underline{x}_{k+1})

= ( Φ_{k}ˆ\underline{x}_{k}+ Γ_{k}( \underline{μ}_{k}+ \underline{u}_{ideal k}) ) + K_{k+1} ( \underline{z}_{k+1} − H_{k+1} ( Φ_{k}ˆ\underline{x}_{k}+ Γ_{k}( \underline{μ}_{k}+ \underline{u}_{ideal k}) ) ) = (Φˆ\underline{x}_{k}+ Γ_{k}\underline{u}_{ideal k}) + K_{k+1} (\underline{z}_{k+1} − H_{k+1} (Φ_{k}ˆ\underline{x}_{k}+ Γ_{k}\underline{u}_{ideal k}) ) + (I − K_{k+1}H_{k+1})Γ_{k}\underline{μ}_{k} = ¯ \underline{x}_{nobias k+1} + K_{k+1} (\underline{z}_{k+1} − H_{k+1} ¯ \underline{x}_{nobias k+1}) + (I − K_{k+1}H_{k+1})Γ_{k}\underline{μ}_{k}

(25)

ここで以下の AdditionalTerm.を求める。

¯\underline{x}_{nobias k+1} = ¯ \underline{x}_{ideal k+1} + Additional Term.
= Φ_{k}ˆ\underline{x}_{ideal k}+ Γ_{k}\underline{u}_{ideal k}+ Additional Term.

(26)

順々に計算をしてみる。

¯\underline{x}_{nobias 1} = Φ_{0}ˆ\underline{x}_{ideal 0}+ Γ_{0}\underline{u}_{ideal 0}  (27)
ˆ\underline{x}_{1} = ¯ \underline{x}_{nobias 1}+ K_{1}(\underline{z}_{1}− H_{1}¯\underline{x}_{nobias 1}) + (I − K_{1}H_{1})Γ_{0}\underline{μ}_{0}

= ˆ \underline{x}_{ideal 1}+ (I − K_{1}H_{1})Γ_{0}\underline{μ}_{0} (28)

¯\underline{x}_{nobias 2} = Φ_{1}ˆ\underline{x}_{1}+ Γ_{1}\underline{u}_{ideal 1}

= Φ_{1}ˆ\underline{x}_{ideal 1}+ Γ_{1}\underline{u}_{ideal 1}+ Φ_{1}(I − K_{1}H_{1})Γ_{0}\underline{μ}_{0} = ¯ \underline{x}_{ideal 2}+ Φ_{1}(I − K_{1}H_{1})Γ_{0}\underline{μ}_{0}

(29)

ˆ\underline{x}_{2} = ¯ \underline{x}_{nobias 2}+ K_{2}(\underline{z}_{2}− H_{2}¯\underline{x}_{nobias 2}) + (I − K_{2}H_{2})Γ_{1}\underline{μ}_{1}

= ˆ \underline{x}_{ideal 2}+ (I − K_{2}H_{2})( Φ_{1}(I − K_{1}H_{1})Γ_{0}\underline{μ}_{0}+ Γ_{1}\underline{μ}_{1})

(30)

¯\underline{x}_{nobias 3} = Φ_{2}ˆ\underline{x}_{2}+ Γ_{2}\underline{u}_{ideal 2}

= Φ_{2}ˆ\underline{x}_{ideal 2}+ Γ_{2}\underline{u}_{ideal 2}

+ Φ_{2}(I − K_{2}H_{2})( Φ_{1}(I − K_{1}H_{1})Γ_{0}\underline{μ}_{0}+ Γ_{1}\underline{μ}_{1})

= ¯ \underline{x}_{ideal 3}+ ∑_{i=0}^{1} ( ∏_{j=2,j−−}^{i+1} Φ_{j} (I − K_{j}H_{j})) Γ_{i}\underline{μ}_{i}

(31)

以上の計算結果から

¯\underline{x}_{nobias k+1} = ¯ \underline{x}_{ideal k+1} + \underbrace{∑_{i=0}^{k−1} ( ∏_{j=k,j−−}^{i+1} Φ_{j} (I − K_{j}H_{j})) Γ_{i}\underline{μ}_{i}}_{AdditionalTerm.}  (32)

これより以下の式が導かれる。

¯\underline{x}_{k+1} = ¯ \underline{x}_{nobias k+1} + Γ_{k}\underline{μ}_{k}

= ¯ \underline{x}_{ideal k+1} + ∑_{i=0}^{k−1} ( ∏_{j=k,j−−}^{i+1} Φ_{j} (I − K_{j}H_{j})) Γ_{i}\underline{μ}_{i}+ Γ_{k}\underline{μ}_{k}

= ¯ \underline{x}_{ideal k+1} + ∑_{i=0}^{k} ( ∏_{j=k,j−−}^{i+1} Φ_{j} (I − K_{j}H_{j})) Γ_{i}\underline{μ}_{i} ( ∵ ∏_{j=k,j−−}^{k+1} = I)

(33)

ˆ\underline{x}_{k+1} = ¯ \underline{x}_{k+1} + K_{k+1} (\underline{z}_{k+1} − H_{k+1} ¯ \underline{x}_{k+1})

= ¯ \underline{x}_{ideal k+1} + K_{k+1} (\underline{z}_{k+1} − H_{k+1} ¯ \underline{x}_{ideal k+1})

+ (I − K_{k+1}H_{k+1})∑_{i=0}^{k} ( ∏_{j=k,j−−}^{i+1} Φ_{j} (I − K_{j}H_{j})) Γ_{i}\underline{μ}_{i}

= ˆ \underline{x}_{ideal k+1} + (I − K_{k+1}H_{k+1})∑_{i=0}^{k} ( ∏_{j=k,j−−}^{i+1} Φ_{j} (I − K_{j}H_{j})) Γ_{i}\underline{μ}_{i}

(34)

ˆ\underline{x}_{ideal k+1} = ˆ \underline{x}_{k+1} − (I − K_{k+1}H_{k+1})∑_{i=0}^{k} ( ∏_{j=k,j−−}^{i+1} Φ_{j} (I − K_{j}H_{j})) Γ_{i}\underline{μ}_{i}  (35)

また

E [\underline{z}_{k+1} − H_{k+1} ¯ \underline{x}_{k+1}] = E [ \underline{z}_{k+1} − H_{k+1} ( ¯ \underline{x}_{ideal k+1} + ∑_{i=0}^{k} ( ∏_{j=k,j−−}^{i+1} Φ_{j} (I − K_{j}H_{j})) Γ_{i}\underline{μ}_{i}) ]

= E [\underline{z}_{k+1} − H_{k+1} ¯ \underline{x}_{ideal k+1}]

− E [H_{k+1} ( ∑_{i=0}^{k} ( ∏_{j=k,j−−}^{i+1} Φ_{j} (I − K_{j}H_{j})) Γ_{i}\underline{μ}_{i}) ]

= −H_{k+1} ∑_{i=0}^{k} ( ∏_{j=k,j−−}^{i+1} Φ_{j} (I − K_{j}H_{j})) Γ_{i} E [\underline{μ}_{i}]

≈ −H_{k+1} ∑_{i=0}^{k} ( ∏_{j=k,j−−}^{i+1} Φ_{j} (I − K_{j}H_{j})) Γ_{i}\underline{μ}_{i}

(36)

ここで

\underline{μ}_{i} = \underline{μ} = const. (0 ≦ i ≦ k)  (37)

とすれば

E [\underline{z}_{k+1} − H_{k+1} ¯ \underline{x}_{k+1}] = −\{ H_{k+1} ∑_{i=0}^{k} ( ∏_{j=k,j−−}^{i+1} Φ_{j} (I − K_{j}H_{j})) Γ_{i}\} \underline{μ}  (38)

さらに

E [\underline{z}_{k+1} − H_{k+1} ¯ \underline{x}_{k+1}] ≈ \frac{1}{k} ∑_{i=0}^{k} (\underline{z}_{i+1} − H_{i+1} ¯ \underline{x}_{i+1} )  (39)

従ってバイアス成分の推定は最小二乗法を用いて

\begin{aligned}\underline{μ} & ≈ −(\{\}^{T}\{\})^{−1} \{\}^{T}E [\underline{z}_{k+1} − H_{k+1} ¯ \underline{x}_{k+1}] \\ & ≈ −(\{\}^{T}\{\})^{−1} \{\}^{T}\frac{1}{k} ∑_{i=0}^{k} (\underline{z}_{i+1} − H_{i+1} ¯ \underline{x}_{i+1} ) \end{aligned}  (40)

で¯\underline{x}_{i}(1 ≦ i ≦ k + 1) の計算の際に含まれていたバイアス成分\underline{μ} を推定し、それによって影響をうけた分を式 ( 35) で補正をおこなえばよい。以後 ( k + 2 ≦ i) は\underline{μ} を引いた形、例えば

¯\underline{x}_{k+2} = Φ_{k+1} ˆ \underline{x}_{k+1} + Γ_{k+1} ( \underline{μ}_{k+1} + \underline{u}_{ideal k+1} − \underline{μ})  (41)

で計算を行い、 i = n(k + 1) で推定、補正を繰り返せばよい。

k は、バイアス成分の性質 (非常にゆっくりとした変化である) を考慮して大きめに取るとよいだろう。 (高周波カットの性質を持つようになる、このあたりのことは平均化フィルタを z 変換とかで考察してみるとわかるはず)