GPS への Kalman Filter の適用

1 Kalman Filter の式

1. Update 時、時間更新する

x̂_{k}^{−} = Ax̂_{k−1} + Bu_{k−1}  (1)
P_{k}^{−} = AP_{k−1}A^{T}+ BQB^{T}  (2)

ただしここで Q は u の誤差共分散行列。

Q = E[uu^{T}]  (3)

2. Correct 時、別の観測量が得られたとき

K_{k} = P_{k}^{−} H^{T}(HP_{k}^{−} H^{T}+ R)^{−1}  (4)
x̂^{k}= x̂_{k}^{−} + K_{k}(z_{k}− Hx̂_{k}^{−})  (5)
P_{k} = (I − K_{k}H)P_{k}^{−}  (6)

ただしここで観測方程式は (z_{k} は得られる観測量)

z_{k} = Hx̂_{k}^{−} + ϵ  (7)

また R は ϵ の誤差共分散行列。

R = E[ϵϵ^{T}]  (8)

2 GPS の式

GPS の観測方程式

ρρρ = G[ \begin{matrix}δx x x \\ δb \end{matrix}] + ϵ̃ϵϵ_{ρ}  (9)

観測方程式から最小二乗法を用いて更新する場合は

[ \begin{matrix}δx̂ x x \\ δb̂ \end{matrix}] = (G^{T}G)^{−1} G^{T}δρρρ  (10)

3 GPS の式に対する Kalman Filter の適用

通常 Kalman Filter は早い更新周期で低い精度のものを運動方程式でモデル化 (=Update) し、遅い更新周期で高い精度のものを観測方程式でモデル化 (=Correct) することによって用いるが、今回は観測方程式のみがあるので Correct 時のみの式を使う。つまり、 Kalman Filter よりは逐次的重み付き最小二乗法に近い。

式 (10) で用いた方法では更新データに対して最小二乗法を連続的に適用したため、ある更新に対してはそれ以前の更新で得られたデータをまったく活用していないことになる。それに対して今回用いる方法では、適切な P と R のもとでは、 Kalman Filter 内で効果的に以前に得られたデータが利用されることとなり、よりよい推定を行うことができるはずである。

観測方程式であるが、特に線形化を行う必要はない。かわりに観測方程式 (9) を式 (5) に適用することに

よって得られる x̂^{k}= [ \begin{matrix}δx̂ x x \\ δb̂ \end{matrix}] について、更新があるたびに

[ \begin{matrix}x x x & 0 \\ b_{0} \end{matrix}]_{k} = [ \begin{matrix}x x x & 0 \\ b_{0} \end{matrix}]_{k−1} + [ \begin{matrix}δx̂ x x \\ δb̂ \end{matrix}]  (11)

と推定位置をずらしていけばよいと思われる。これは更新前の推定位置から真値までのずれが

x̂_{k}^{−} = [ \begin{matrix}δx x x \\ δb \end{matrix}] = 000  (12)

つまり、更新がなければ現在の推定位置が正しいものだと思うしかないということと同義である。

以上すべて式を書き出すと

K_{k} = P_{k−1}G^{T}(GP_{k−1}G^{T}+ R)^{−1}  (13)

[ \begin{matrix}δx̂ x x \\ δb̂ \end{matrix}]_{k} = K_{k}ρρρ_{k}  (14)

\begin{aligned}[ \begin{matrix}x x x & 0 \\ b_{0} \end{matrix}]_{k} & = [ \begin{matrix}x x x & 0 \\ b_{0} \end{matrix}]_{k−1} + [ \begin{matrix}δx̂ x x \\ δb̂ \end{matrix}]_{k} \\ & = [ \begin{matrix}x x x & 0 \\ b_{0} \end{matrix}]_{k−1} + K_{k}ρρρ_{k} \end{aligned}  (15)

P_{k} = (I − K_{k}G)P_{k−1}  (16)

G は x x x^{(i)}(i 番目衛星位置)、 x x x_{0k}(観測者位置) から作られる。ニュートン・ラフソン法によると

G = [ \begin{matrix}(−\frac{x x x^{(1)}−x x x_{0k}}{∥x x x^{(1)}−x x x_{0k}∥} )^{T} & 1 \\ (−\frac{x x x^{(2)}−x x x_{0k}}{∥x x x^{(2)}−x x x_{0k}∥} )^{T} & 1 \\ (−\frac{x x x^{(3)}−x x x_{0k}}{∥x x x^{(3)}−x x x_{0k}∥} )^{T} & 1 \\ ⋮ & ⋮ \end{matrix}]  (17)

P の初期値は適当に大きな対角行列でよいと思われる。

4 INS/GPS 2D

4.1 update 時

4.1.1 運動方程式

[ \begin{matrix}a_{x} \\ a_{y} \end{matrix}]_{k} = [ \begin{matrix}cosϕ_{k} & −sinϕ_{k} \\ sinϕ_{k} & cosϕ_{k} \end{matrix} ] [ \begin{matrix}a_{f} \\ a_{s} \end{matrix}]  (18)
ϕ_{k+1} = ϕ_{k}+ ∫_{t}^{t+Δt} ωdt ≈ ϕ_{k}+ ωΔt  (19)
\begin{aligned}[ \begin{matrix}v_{x} \\ v_{y} \end{matrix}]_{k+1} & = [ \begin{matrix}v_{x} \\ v_{y} \end{matrix}]_{k}+ ∫_{t}^{t+Δt} [ \begin{matrix}a_{x} \\ a_{y} \end{matrix}]_{k}dt \\ & ≈ [ \begin{matrix}v_{x} \\ v_{y} \end{matrix}]_{k}+ [ \begin{matrix}a_{x} \\ a_{y} \end{matrix}]_{k}Δt \end{aligned}  (20)

\begin{aligned}[ \begin{matrix}x \\ y \end{matrix}]_{k+1} & = [ \begin{matrix}x \\ y \end{matrix}]_{k}+ ∫_{t}^{t+Δt} [ \begin{matrix}v_{x} \\ v_{y} \end{matrix}]_{k}dt \\ & ≈ [ \begin{matrix}x \\ y \end{matrix}]_{k}+ [ \begin{matrix}v_{x} \\ v_{y} \end{matrix}]_{k}Δt \end{aligned}  (21)

4.1.2 P の更新運動方程式を真値との誤差で線形化する。状態量としては Δϕ, Δ[ \begin{matrix}v_{x} \\ v_{y} \end{matrix}], Δ[ \begin{matrix}x \\ y \end{matrix}] の計 5 個。順に線形化していくと

[ \begin{matrix}Δa_{x} \\ Δa_{y} \end{matrix}]_{k} = [ \begin{matrix}cos(ϕ + Δϕ)_{k} & −sin(ϕ + Δϕ)_{k} \\ sin(ϕ + Δϕ)_{k} & cos(ϕ + Δϕ)_{k} \end{matrix} ] [ \begin{matrix}a_{f} + Δa_{f} \\ a_{s}+ Δa_{s} \end{matrix} ] − [ \begin{matrix}cosϕ_{k} & −sinϕ_{k} \\ sinϕ_{k} & cosϕ_{k} \end{matrix} ] [ \begin{matrix}a_{f} \\ a_{s} \end{matrix}]

= [ \begin{matrix}−(a_{f} cosϕ_{k}+ a_{s}cosϕ_{k}) \\ (a_{f} cosϕ_{k}− a_{s}sinϕ_{k}) \end{matrix} ] [Δϕ_{k}]+ [ \begin{matrix}−sinϕ_{k} & cosϕ_{k} \\ cosϕ_{k} & sinϕ_{k} \end{matrix}] [ \begin{matrix}Δa_{f} \\ Δa_{s} \end{matrix} ]

(22)

[Δϕ ]_{k+1} = (ϕ + Δϕ)_{k+1} − ϕ_{k+1}

≈ ((ϕ + Δϕ)_{k}+ (ω + Δω)Δt) − (ϕ_{k}+ ωΔt) = [1][Δϕ ]_{k}+ [Δt][Δω ]

(23)

[ \begin{matrix}Δv_{x} \\ Δv_{y} \end{matrix}]_{k+1} = [ \begin{matrix}v_{x}+ Δv_{x} \\ v_{y}+ Δv_{y} \end{matrix} ]_{k+1} − [ \begin{matrix}v_{x} \\ v_{y} \end{matrix}]_{k+1}

≈ ([ \begin{matrix}v_{x}+ Δv_{x} \\ v_{y}+ Δv_{y} \end{matrix} ]_{k}+ [ \begin{matrix}a_{x}+ Δa_{x} \\ a_{y}+ Δa_{y} \end{matrix} ]_{k}Δt) − ([ \begin{matrix}v_{x} \\ v_{y} \end{matrix}]_{k}+ [ \begin{matrix}a_{x} \\ a_{y} \end{matrix}]_{k}Δt)

= [ \begin{matrix}Δv_{x} \\ Δv_{y} \end{matrix}]_{k}+ [ \begin{matrix}Δa_{x} \\ Δa_{y} \end{matrix}]_{k}Δt

(24)

[ \begin{matrix}Δx \\ Δy \end{matrix}]_{k+1} = [ \begin{matrix}x + Δx \\ y + Δy \end{matrix} ]_{k+1} − [ \begin{matrix}x \\ y \end{matrix}]_{k+1}

≈ ([ \begin{matrix}x + Δx \\ y + Δy \end{matrix} ]_{k}+ [ \begin{matrix}v_{x}+ Δv_{x} \\ v_{y}+ Δv_{y} \end{matrix} ]_{k}Δt) − ([ \begin{matrix}x \\ y \end{matrix}]_{k}+ [ \begin{matrix}v_{x} \\ v_{y} \end{matrix}]_{k}Δt)

= [ \begin{matrix}Δx \\ Δy \end{matrix}]_{k}+ [ \begin{matrix}Δv_{x} \\ Δv_{y} \end{matrix}]_{k}Δt

(25)

以上まとめて

[ \begin{matrix}Δϕ \\ Δv_{x} \\ Δv_{y} \\ Δx \\ Δy \end{matrix}]_{k+1} = [ \begin{matrix}1 & 0 & 0 & 0 & 0 \\ −(a_{f} cosϕ_{k}+ a_{s}cosϕ_{k})Δt & 1 & 0 & 0 & 0 \\ (a_{f} cosϕ_{k}− a_{s}sinϕ_{k})Δt & 0 & 1 & 0 & 0 \\ 0 & Δt & 0 & 1 & 0 \\ 0 & 0 & Δt & 0 & 1 \end{matrix}] [ \begin{matrix}Δϕ \\ Δv_{x} \\ Δv_{y} \\ Δx \\ Δy \end{matrix}]_{k}

+ [ \begin{matrix}Δt & 0 & 0 \\ 0 & −sinϕ_{k}Δt & cosϕ_{k}Δt \\ 0 & cosϕ_{k}Δt & sinϕ_{k}Δt \\ 0 & 0 & 0 \\ 0 & 0 & 0 \end{matrix}] [ \begin{matrix}Δω \\ Δa_{s} \\ Δa_{f} \end{matrix} ]

(26)

よって P 更新は

P_{k+1} = A_{k}P_{k}A_{k}^{T}+ B_{k}QB_{k}^{T}  (27)
A_{k} = [ \begin{matrix}1 & 0 & 0 & 0 & 0 \\ −(a_{f} cosϕ_{k}+ a_{s}cosϕ_{k})Δt & 1 & 0 & 0 & 0 \\ (a_{f} cosϕ_{k}− a_{s}sinϕ_{k})Δt & 0 & 1 & 0 & 0 \\ 0 & Δt & 0 & 1 & 0 \\ 0 & 0 & Δt & 0 & 1 \end{matrix}]  (28)
B_{k} = [ \begin{matrix}Δt & 0 & 0 \\ 0 & −sinϕ_{k}Δt & cosϕ_{k}Δt \\ 0 & cosϕ_{k}Δt & sinϕ_{k}Δt \\ 0 & 0 & 0 \\ 0 & 0 & 0 \end{matrix}]  (29)

4.2 correct 時

4.2.1 P の修正

z ≡ [ \begin{matrix}ϕ_{INS}− ϕ_{GPS} \\ v_{x}_{INS}− v_{x}_{GPS} \\ v_{y}_{INS}− v_{y}_{GPS} \\ x_{INS}− x_{GPS} \\ y_{INS}− y_{GPS} \end{matrix}] = [ \begin{matrix}1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 1 \end{matrix}] [ \begin{matrix}Δϕ \\ Δv_{x} \\ Δv_{y} \\ Δx \\ Δy \end{matrix}] + v v v  (30)

よって

H = [ \begin{matrix}1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 1 \end{matrix}]  (31)

K = PH^{T}(HPH^{T}+ R)^{−1}  (32)

修正後の P (P̂) は

P̂= (I − KH)P  (33)

4.2.2 INS の修正観測によって真値との差がわかる。これを Δϕ̂ のように表現すると Kalman Filter から

[ \begin{matrix}Δϕ̂ \\ Δv̂_{x} \\ Δv̂_{y} \\ Δx̂ \\ Δŷ \end{matrix}] = Kz = K [ \begin{matrix}ϕ_{INS}− ϕ_{GPS} \\ v_{x}_{INS}− v_{x}_{GPS} \\ v_{y}_{INS}− v_{y}_{GPS} \\ x_{INS}− x_{GPS} \\ y_{INS}− y_{GPS} \end{matrix}]  (34)

よって修正された ϕ, [ \begin{matrix}v_{x} \\ v_{y} \end{matrix}], [ \begin{matrix}x \\ y \end{matrix}] は

[ \begin{matrix}ϕ \\ v_{x} \\ v_{y} \\ x \\ y \end{matrix}]_{after correct}= [ \begin{matrix}ϕ \\ v_{x} \\ v_{y} \\ x \\ y \end{matrix}]_{before correct}− [ \begin{matrix}Δϕ̂ \\ Δv̂_{x} \\ Δv̂_{y} \\ Δx̂ \\ Δŷ \end{matrix}]  (35)