『MEMS センサを用いた低コスト INS/GPS 複合航法システム』

数式リスト

成岡優 2006 年 7 月 1 日

1 システムモデルに関する式

1.1 速度運動方程式

\begin{aligned}\frac{d}{dt}\{ \begin{matrix}0 \\ ṙ_{e}^{n} \end{matrix}\} & = q̃_{b}^{n}^{∗}\{ \begin{matrix}0 \\  & a^{b} \end{matrix}\} q̃_{b}^{n}+ \{ \begin{matrix}0 \\  & g^{n} \end{matrix}\} \\ & − \{ \begin{matrix}0 \\ ( 2ω⃗_{e/i}^{n} + ω⃗_{n/e}^{n}) ×ṙ_{e}^{n} \end{matrix} \} \end{aligned}  (1)

1.2 位置運動方程式

\frac{d}{dt}q̃_{e}^{n} = \frac{1}{2}q̃_{e}^{n}\{ 0 ; ω⃗_{n/e}^{n}\} , \frac{d}{dt}h = −(ṙ_{e}^{n})_{Z}  (2)

1.3 姿勢運動方程式

\begin{aligned}\frac{d}{dt}q̃_{n}^{b} & = \frac{1}{2} [q̃_{n}^{b}\{ 0 ; ω⃗_{b/i}^{b}\} \\ & −( \{ 0 ; ω⃗_{e/i}^{n} \} + \{ 0 ; ω⃗_{n/e}^{n}\} ) q̃_{n}^{b}] \end{aligned}  (3)

2 誤差モデルに関する式

2.1 線形化

2.1.1 速度運動方程式の線形化

\frac{d}{dt}Δṙ_{e}^{n} = Rot[q̃_{n}^{b}^{∗}]Δa^{b}

− 2[ \begin{matrix}0 & −R_{02}a_{0}^{b}− R_{12}a_{1}^{b}− R_{22}a_{2}^{b} & R_{01}a_{0}^{b}+ R_{11}a_{1}^{b}+ R_{21}a_{2}^{b} \\ R_{02}a_{0}^{b}+ R_{12}a_{1}^{b}+ R_{22}a_{2}^{b} & 0 & −R_{00}a_{0}^{b}− R_{10}a_{1}^{b}− R_{20}a_{2}^{b} \\ −R_{01}a_{0}^{b}− R_{11}a_{1}^{b}− R_{21}a_{2}^{b} & R_{00}a_{0}^{b}+ R_{10}a_{1}^{b}+ R_{20}a_{2}^{b} & 0 \end{matrix} ]_{q̃_{n}^{b}}Δu_{n}^{b}

+ Δg^{n}+ \frac{1}{r_{e}+ h} [ \begin{matrix}ṙ_{n}^{e}_{Z} & 0 & 0 \\ 0 & ṙ_{n}^{e}_{Z} & 0 \\ −ṙ_{n}^{e}_{X} & −ṙ_{n}^{e}_{Y} & 0 \end{matrix}] Δṙ_{e}^{n}− \frac{1}{(r_{e}+ h)^{2}} ( \begin{matrix}(ṙ_{e}^{n})_{X}(ṙ_{e}^{n})_{Z} \\ (ṙ_{e}^{n})_{Y}(ṙ_{e}^{n})_{Z} \\ −(ṙ_{e}^{n})_{X}^{2}− (ṙ_{e}^{n})_{Y}^{2} \end{matrix} ) Δh

+ 4Ω_{e/i} [ \begin{matrix}R_{21}(ṙ_{n}^{e})_{Y}− R_{11}(ṙ_{n}^{e})_{Z} & R_{10}(ṙ_{n}^{e})_{Z}− R_{20}(ṙ_{n}^{e})_{Y} & 0 \\ R_{01}(ṙ_{n}^{e})_{Z}− R_{21}(ṙ_{n}^{e})_{X} & R_{20}(ṙ_{n}^{e})_{X}− R_{00}(ṙ_{n}^{e})_{Z} & 0 \\ R_{11}(ṙ_{n}^{e})_{X}− R_{01}(ṙ_{n}^{e})_{Y} & R_{00}(ṙ_{n}^{e})_{Y}− R_{10}(ṙ_{n}^{e})_{X} & 0 \end{matrix}]_{q̃_{e}^{n}}Δu_{e}^{n}

− [ \begin{matrix}0 & −ω_{2} & ω_{1} \\ ω_{2} & 0 & −ω_{0} \\ −ω_{1} & ω_{0} & 0 \end{matrix} ]_{2ω⃗_{e/i}^{n} +ω⃗_{n/e}^{n}} Δṙ_{e}^{n}

(4)

2.1.2 位置運動方程式の線形化

\frac{d}{dt}Δu_{e}^{n}= \frac{1}{2(r_{e}+ h)} [ \begin{matrix}−R_{10} & R_{00} & 0 \\ −R_{11} & R_{01} & 0 \\ −R_{12} & R_{02} & 0 \end{matrix}]_{q̃_{e}^{n}}Δṙ_{e}^{n}+ \frac{1}{2(r_{e}+ h)^{2}} ( \begin{matrix}R_{10}(ṙ_{e}^{n})_{X}− R_{00}(ṙ_{e}^{n})_{Y} \\ R_{11}(ṙ_{e}^{n})_{X}− R_{01}(ṙ_{e}^{n})_{Y} \\ R_{12}(ṙ_{e}^{n})_{X}− R_{02}(ṙ_{e}^{n})_{Y} \end{matrix}) _{q̃_{e}^{n}}Δh  (5)
\frac{d}{dt}Δh = −Δ(ṙ_{e}^{n})_{Z}  (6)

2.1.3 姿勢運動方程式の線形化

\begin{aligned}\frac{d}{dt}Δu_{n}^{b} & = \frac{1}{2} [ \begin{matrix}R_{00} & R_{10} & R_{20} \\ R_{01} & R_{11} & R_{21} \\ R_{02} & R_{12} & R_{22} \end{matrix}]_{q̃_{n}^{b}}Δω⃗_{b/i}^{b} − Ω_{e/i} [ \begin{matrix}R_{01} & −R_{00} & 0 \\ R_{11} & −R_{10} & 0 \\ R_{21} & −R_{20} & 0 \end{matrix}]_{q̃_{e}^{n}}Δu_{e}^{n} \\ & − \frac{1}{2(r_{e}+ h)} [ \begin{matrix}0 & 1 & 0 \\ −1 & 0 & 0 \\ 0 & 0 & 0 \end{matrix}] Δṙ_{e}^{n}+ \frac{1}{2(r_{e}+ h)^{2}} ( \begin{matrix}(ṙ_{e}^{n})_{Y} \\ −(ṙ_{e}^{n})_{X} \\ 0 \end{matrix} ) Δh \\ & − [ \begin{matrix}0 & −ω_{2} & ω_{1} \\ ω_{2} & 0 & −ω_{0} \\ −ω_{1} & ω_{0} & 0 \end{matrix} ]_{ω⃗_{e/i}^{n} +ω⃗_{n/e}^{n}} Δu_{n}^{b} \end{aligned}  (7)

2.2 誤差モデルの運動方程式

式 (4), (5), (6), (7) をまとめると以下のようにかける。

x ≡ [ \begin{matrix}Δṙ_{n}^{e} \\ Δu_{e}^{n} \\ Δh \\ Δu_{n}^{b} \end{matrix}] , u ≡ [ \begin{matrix}Δa^{b} \\ Δω⃗_{b/i}^{b} \\ Δg⃗ \end{matrix} ]  (8)

と書けば、誤差モデルの運動方程式は

\frac{d}{dt}x = Ax + Bu  (9)

2.3 誤差モデルとシステムモデルの関係

z ≡ [ \begin{matrix}ṙ_{e}^{n} \\  & q̃_{e}^{n} \\ h \end{matrix}] − [ \begin{matrix}ṙ_{e}^{n} \\  & q̃_{e}^{n} \\ h \end{matrix}]_{GPS}  (10)

と書けば、 GPS の観測誤差が v とすると

z = Hx + v  (11)

ここで H は

H = [ \begin{matrix}I_{3×3} & 0_{3×4} & 0 \\ 0_{4×3} & [ \begin{matrix}−(q_{e}^{n})_{1} & −(q_{e}^{n})_{2} & −(q_{e}^{n})_{3} \\ (q_{e}^{n})_{0} & (q_{e}^{n})_{3} & −(q_{e}^{n})_{2} \\ −(q_{e}^{n})_{3} & (q_{e}^{n})_{0} & (q_{e}^{n})_{1} \\ (q_{e}^{n})_{2} & −(q_{e}^{n})_{1} & (q_{e}^{n})_{0} \end{matrix} ] & 0 \\ 0_{1×3} & 0_{1×3} & 1 \end{matrix}]  (12)

2.4 Kalman Filter Predict

P_{k+1} = (I + AΔt)P_{k}(I + AΔt)^{T}+ (BΔt)Q(BΔt)^{T}  (13)

2.5 Kalman Filter Correct

K_{k} = P_{k}H_{k}^{T}(H_{k}P_{k}H_{k}^{T}+ R_{k})^{−1}  (14)
P_{k} → (I − K_{k}H_{k})P_{k}  (15)

x_{k} ≡ [ \begin{matrix}Δṙ_{n}^{e} \\ Δu_{e}^{n} \\ Δh \\ Δu_{n}^{b} \end{matrix}]_{k} = K_{k}z_{k}  (16)

ṙ_{n}^{e} → ṙ_{n}^{e}− (Δṙ_{n}^{e})_{k}  (17)

\begin{aligned}q̃_{e}^{n} & → \{ \begin{matrix}1 \\ (Δu_{e}^{n})_{k} \end{matrix}\} ^{∗}q̃_{e}^{n}, \\ h & → h − (Δh)_{k} \end{aligned}  (18)

q̃_{n}^{b} → \{ \begin{matrix}1 \\ (Δu_{n}^{b})_{k} \end{matrix}\} ^{∗}q̃_{n}^{b}  (19)