1 線形化に関して

• Quarternion 演算の線形化

Δq̃_{1}^{∗}q̃_{2}q̃_{1} = \{ \begin{matrix}Δq_{10} \\ −Δq q q_{1} \end{matrix} \} \{ \begin{matrix}q_{20} \\ q q q & 2 \end{matrix}\} \{ \begin{matrix}q_{10} \\ q q q & 1 \end{matrix}\}

= \{ \begin{matrix}q_{20}Δq_{10}+ q q q_{2}· Δq q q_{1} \\ q q q_{2}Δq_{10}− q_{20}Δq q q_{1}+ q q q_{2}× Δq q q_{1} \end{matrix} \} \{ \begin{matrix}q_{10} \\ q q q & 1 \end{matrix}\}

= \{ \begin{matrix}(q_{10}q_{20}Δq_{10}+ q_{10}q q q_{2}· Δq q q_{1}) \\ −((q q q_{1}· q q q_{2})Δq_{10}− q_{20}q q q_{1}· Δq q q_{1}− q q q_{2}× q q q_{1}· Δq q q_{1}) \\ (q_{20}q q q_{1}Δq_{10}+ (q q q_{2}· Δq q q_{1})q q q_{1}) + (q_{10}q q q_{2}Δq_{10}− q_{10}q_{20}Δq q q_{1}+ q_{10}q q q_{2}× Δq q q_{1}) \\ +(q q q_{2}× q q q_{1}Δq_{10}+ q_{20}q q q_{1}× Δq q q_{1}+ (q q q_{2}× Δq q q_{1}) × q q q_{1}) \end{matrix} \}

= \{ \begin{matrix}(q_{10}q_{20}− q q q_{1}· q q q_{2})Δq_{10}+ (q_{10}q q q_{2}+ q_{20}q q q_{1}+ q q q_{2}× q q q_{1}) · Δq q q_{1} \\ (q_{10}q q q_{2}+ q_{20}q q q_{2}+ q q q_{2}× q q q_{1})Δq_{10}− (q_{10}q_{20}− q q q_{1}· q q q_{2})Δq q q_{1}+ (q_{10}q q q_{2}+ q_{20}q q q_{2}) × Δq q q_{1}− (q q q_{1}· Δq q q_{1})q q q_{2}+ (q q q_{2}· Δq q q_{1})q q q_{1} \end{matrix} \}

= \{ \begin{matrix}(q_{10}q_{20}− q q q_{1}· q q q_{2})Δq_{10}+ (q_{10}q q q_{2}+ q_{20}q q q_{1}+ q q q_{2}× q q q_{1}) · Δq q q_{1} \\ (q_{10}q q q_{2}+ q_{20}q q q_{2}+ q q q_{2}× q q q_{1})Δq_{10}− (q_{10}q_{20}− q q q_{1}· q q q_{2})Δq q q_{1}+ (q_{10}q q q_{2}+ q_{20}q q q_{2}+ q q q_{2}× q q q_{1}) × Δq q q_{1} \end{matrix} \}

q̃_{1}^{∗}q̃_{2}Δq̃_{1} = \{ \begin{matrix}q_{10} \\ −q q q_{1} \end{matrix} \} \{ \begin{matrix}q_{20} \\ q q q & 2 \end{matrix}\} \{ \begin{matrix}Δq_{10} \\ Δq q q_{1} \end{matrix}\}

= \{ \begin{matrix}q_{10}q_{20}+ q q q_{1}· q q q_{2} \\ q_{10}q q q_{2}− q_{20}q q q_{1}+ q q q_{2}× q q q_{1} \end{matrix} \} \{ \begin{matrix}Δq_{10} \\ Δq q q_{1} \end{matrix}\}

= \{ \begin{matrix}(q_{10}q_{20}+ q q q_{1}· q q q_{2})Δq_{10}− (q_{10}q q q_{2}− q_{20}q q q_{1}+ q q q_{2}× q q q_{1}) · Δq q q_{1} \\ (q_{10}q_{20}+ q q q_{1}· q q q_{2})Δq q q_{1}+ (q_{10}q q q_{2}− q_{20}q q q_{1}+ q q q_{2}× q q q_{1})Δq_{10}+ (q_{10}q q q_{2}− q_{20}q q q_{1}+ q q q_{2}× q q q_{1}) × Δq q q_{1} \end{matrix} \}

= \{ \begin{matrix}(q_{10}q_{20}+ q q q_{1}· q q q_{2})Δq_{10}− (q_{10}q q q_{2}− q_{20}q q q_{1}+ q q q_{2}× q q q_{1}) · Δq q q_{1} \\ (q_{10}q q q_{2}− q_{20}q q q_{2}+ q q q_{2}× q q q_{1})Δq_{10}+ (q_{10}q_{20}+ q q q_{1}· q q q_{2})Δq q q_{1}+ (q_{10}q q q_{2}− q_{20}q q q_{2}+ q q q_{2}× q q q_{1}) × Δq q q_{1} \end{matrix} \}

Δq̃_{1}^{∗}\{ \begin{matrix}0 \\ v & v & v \end{matrix}\} q̃_{1} = \{ \begin{matrix}(−q q q_{1}· v v v)Δq_{10}+ (q_{10}v v v+ v v v× q q q_{1}) · Δq q q_{1} \\ (q_{10}v v v+ v v v× q q q_{1})Δq_{10}+ (q q q_{1}· v v v)Δq q q_{1}+ (q_{10}v v v+ v v v× q q q_{1}) × Δq q q_{1} \end{matrix} \}

q̃_{1}^{∗}\{ \begin{matrix}0 \\ v & v & v \end{matrix}\} Δq̃_{1} = \{ \begin{matrix}(q q q_{1}· v v v)Δq_{10}− (q_{10}v v v+ v v v× q q q_{1}) · Δq q q_{1} \\ (q_{10}v v v+ v v v× q q q_{1})Δq_{10}+ (q q q_{1}· v v v)Δq q q_{1}+ (q_{10}v v v+ v v v× q q q_{1}) × Δq q q_{1} \end{matrix} \}

\begin{aligned}Δq̃_{1}^{∗}\{ \begin{matrix}0 \\ v & v & v \end{matrix}\} q̃_{1}+ q̃_{1}^{∗}\{ \begin{matrix}0 \\ v & v & v \end{matrix}\} Δq̃_{1} & = 2\{ \begin{matrix}0 \\ (q_{10}v v v+ v v v× q q q_{1})Δq_{10}+ (q q q_{1}· v v v)Δq q q_{1}+ (q_{10}v v v+ v v v× q q q_{1}) × Δq q q_{1} \end{matrix} \} \\ & = 2\{ \begin{matrix}0 \\ [ \begin{matrix}ω_{1} & λ & −ω_{3} & ω_{2} \\ ω_{2} & ω_{3} & λ & −ω_{1} \\ ω_{3} & −ω_{2} & ω_{1} & λ \end{matrix} ] \begin{matrix}λ = q q q_{1}· v v v \\ ω = q_{10}v v v+ v v v× q q q_{1} \end{matrix} Δ[ \begin{matrix}q_{10} \\ q q q & 1 \end{matrix}] \end{matrix} \} \end{aligned}

q_{10}v v v+ v v v× q q q_{1} = [ \begin{matrix}v_{1} & 0 & −v_{3} & v_{2} \\ v_{2} & v_{3} & 0 & −v_{1} \\ v_{3} & −v_{2} & v_{1} & 0 \end{matrix} ] [ \begin{matrix}q_{10} \\ q q q & 1 \end{matrix}]

\begin{aligned}Δq̃_{1}\{ \begin{matrix}0 \\ v & v & v \end{matrix}\} q̃_{1}^{∗}+ q̃_{1}\{ \begin{matrix}0 \\ v & v & v \end{matrix}\} Δq̃_{1}^{∗} & = 2\{ \begin{matrix}0 \\ (q_{10}v v v− v v v× q q q_{1})Δq_{10}+ (q q q_{1}· v v v)Δq q q_{1}− (q_{10}v v v− v v v× q q q_{1}) × Δq q q_{1} \end{matrix} \} \\ & = 2\{ \begin{matrix}0 \\ [ \begin{matrix}ω_{1} & λ & ω_{3} & −ω_{2} \\ ω_{2} & −ω_{3} & λ & ω_{1} \\ ω_{3} & ω_{2} & −ω_{1} & λ \end{matrix} ] \begin{matrix}λ = q q q_{1}· v v v \\ ω = q_{10}v v v− v v v× q q q_{1} \end{matrix} Δ[ \begin{matrix}q_{10} \\ q q q & 1 \end{matrix}] \end{matrix} \} \end{aligned}

q_{10}v v v− v v v× q q q_{1} = [ \begin{matrix}v_{1} & 0 & v_{3} & −v_{2} \\ v_{2} & −v_{3} & 0 & v_{1} \\ v_{3} & v_{2} & −v_{1} & 0 \end{matrix} ] [ \begin{matrix}q_{10} \\ q q q & 1 \end{matrix}]

q̃^{∗}\{ \begin{matrix}0 \\ Δv v v \end{matrix}\} q̃= \{ \begin{matrix}q_{0} \\ −q q q \end{matrix} \} \{ \begin{matrix}0 \\ Δv v v \end{matrix}\} \{ \begin{matrix}q_{0} \\ q & q & q \end{matrix}\}

= \{ \begin{matrix}q q q· Δv v v \\ q_{0}Δv v v− q q q× Δv v v \end{matrix} \} \{ \begin{matrix}q_{0} \\ q & q & q \end{matrix}\}

= \{ \begin{matrix}q_{0}q q q· Δv v v− (q_{0}Δv v v· q q q− (q q q× Δv v v) · q q q) \\ (q q q· Δv v v)q q q+ (q_{0}^{2}Δv v v− q_{0}q q q× Δv v v) + (q_{0}Δv v v× q q q− (q q q× Δv v v) × q q q) \end{matrix} \}

= \{ \begin{matrix}(q q q× Δv v v) · q q q \\ (q q q· Δv v v)q q q+ q_{0}^{2}Δv v v− 2q_{0}q q q× Δv v v− (q q q× Δv v v) × q q q \end{matrix} \}

= \{ \begin{matrix}0 \\ (q_{0}^{2}− q q q· q q q)Δv v v− 2q_{0}q q q× Δv v v+ 2(q q q· Δv v v)q q q \end{matrix} \}

q̃\{ \begin{matrix}0 \\ Δv v v \end{matrix}\} q̃^{∗} = \{ \begin{matrix}0 \\ (q_{0}^{2}− q q q· q q q)Δv v v+ 2q_{0}q q q× Δv v v+ 2(q q q· Δv v v)q q q \end{matrix} \}

\{ \begin{matrix}0 \\ Δv v v_{1} \end{matrix}\} \{ \begin{matrix}0 \\ v v v & 2 \end{matrix}\} = \{ \begin{matrix}−v v v_{2}· Δv v v_{1} \\ −v v v_{2}× Δv v v_{1} \end{matrix} \}

\{ \begin{matrix}0 \\ v v v & 1 \end{matrix}\} \{ \begin{matrix}0 \\ Δv v v_{2} \end{matrix}\} = \{ \begin{matrix}−v v v_{1}· Δv v v_{2} \\ v v v_{1}× Δv v v_{2} \end{matrix} \}

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

\frac{d}{dt}\{ \begin{matrix}0 \\ Δṙ_{e}^{n} \end{matrix}\} = \frac{d}{dt}( \{ \begin{matrix}0 \\ ṙ_{e}^{n}+ Δṙ_{e}^{n} \end{matrix} \} − \{ \begin{matrix}0 \\ ṙ_{e}^{n} \end{matrix}\} )

= [( q̃_{n}^{b}+ Δq̃_{n}^{b}) \{ \begin{matrix}0 \\ a^{b}+ Δa^{b} \end{matrix} \} ( q̃_{n}^{b}+ Δq̃_{n}^{b}) ^{∗}+ \{ \begin{matrix}0 \\ g^{n}+ Δg^{n} \end{matrix} \}

− \{ \begin{matrix}0 \\ ( 2( ω⃗_{e/i}^{n} + Δω⃗_{e/i}^{n} ) + ( ω⃗_{n/e}^{n} + Δω⃗_{n/e}^{n}) ) × (ṙ_{e}^{n}+ Δṙ_{e}^{n}) \end{matrix} \}

− (q̃_{e}^{n}+ Δq̃_{e}^{n}) ^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} × (r⃗_{e}+ Δr⃗_{e}) ) \end{matrix} \} (q̃_{e}^{n}+ Δq̃_{e}^{n}) ]

− [q̃_{n}^{b}\{ \begin{matrix}0 \\ & a^{b} \end{matrix}\} q̃_{n}^{b}^{∗}+ \{ \begin{matrix}0 \\ & g^{n} \end{matrix}\}

− \{ \begin{matrix}0 \\ ( 2ω⃗_{e/i}^{n} + ω⃗_{n/e}^{n}) ×ṙ_{e}^{n} \end{matrix} \}

− q̃_{e}^{n}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) \end{matrix} \} q̃_{e}^{n}]

= Δq̃_{n}^{b}\{ \begin{matrix}0 \\ & a^{b} \end{matrix}\} q̃_{n}^{b}^{∗}+q̃_{n}^{b}\{ \begin{matrix}0 \\ & a^{b} \end{matrix}\} Δq̃_{n}^{b}^{∗}+q̃_{n}^{b}\{ \begin{matrix}0 \\ Δa^{b} \end{matrix}\} q̃_{n}^{b}^{∗}+ \{ \begin{matrix}0 \\ Δg^{n} \end{matrix}\}

− \{ \begin{matrix}0 \\ ( 2Δω⃗_{e/i}^{n} + Δω⃗_{n/e}^{n}) ×ṙ_{e}^{n}+ ( 2ω⃗_{e/i}^{n} + ω⃗_{n/e}^{n}) × Δṙ_{e}^{n} \end{matrix} \}

−q̃_{e}^{n}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} × Δr⃗_{e}) \end{matrix} \} q̃_{e}^{n}

− Δq̃_{e}^{n}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) \end{matrix} \} q̃_{e}^{n}−q̃_{e}^{n}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) \end{matrix} \} Δq̃_{e}^{n}

* Δω⃗_{e/i}^{n} は

\{ \begin{matrix}0 \\ Δω⃗_{e/i}^{n} \end{matrix} \} = (q̃_{e}^{n}+ Δq̃_{e}^{n}) ^{∗}\{ 0 ; ω⃗_{e/i}^{i} \} (q̃_{e}^{n}+ Δq̃_{e}^{n}) −q̃_{e}^{n}^{∗}\{ 0 ; ω⃗_{e/i}^{i} \} q̃_{e}^{n}

= Δq̃_{e}^{n}^{∗}{ 0 ; ω⃗_{e/i}^{i} } q̃_{e}^{n}+q̃_{e}^{n}^{∗}{ 0 ; ω⃗_{e/i}^{i} } Δq̃_{e}^{n}

= Δq̃_{e}^{n}^{∗} \{ \begin{matrix}& 0 \\ ( & \begin{matrix}0 \\ 0 \\ Ω_{e/i} \end{matrix} & ) \end{matrix} \} q̃_{e}^{n}+q̃_{e}^{n}^{∗} \{ \begin{matrix}& 0 \\ ( & \begin{matrix}0 \\ 0 \\ Ω_{e/i} \end{matrix} & ) \end{matrix} \} Δq̃_{e}^{n}

= 2\{ \begin{matrix}0 \\ ( q_{0} ( \begin{matrix}0 \\ 0 \\ Ω_{e/i} \end{matrix} ) + ( \begin{matrix}0 \\ 0 \\ Ω_{e/i} \end{matrix} ) q q q) Δq_{0}+ ( q q q· ( \begin{matrix}0 \\ 0 \\ Ω_{e/i} \end{matrix} ) ) Δq q q+ ( q_{0} ( \begin{matrix}0 \\ 0 \\ Ω_{e/i} \end{matrix} ) + ( \begin{matrix}0 \\ 0 \\ Ω_{e/i} \end{matrix} ) × q q q) × Δq q q \end{matrix} \}

= 2Ω_{e/i} \{ \begin{matrix}0 \\ −q_{2}Δq_{0}+ q_{3}Δq_{1}− q_{0}Δq_{2}+ q_{1}Δq_{3} \\ q_{1}Δq_{0}+ q_{0}Δq_{1}+ q_{3}Δq_{2}+ q_{2}Δq_{3} \\ q_{0}Δq_{0}− q_{1}Δq_{1}− q_{2}Δq_{2}+ q_{3}Δq_{3} \end{matrix} \}

* Δω⃗_{n/e}^{n} は

\begin{aligned}Δω⃗_{n/e}^{n} & = \frac{1}{r_{e}+ (h + Δh)} ( \begin{matrix}(ṙ_{e}^{n})_{Y}+ Δ(ṙ_{e}^{n})_{Y} \\ −(ṙ_{e}^{n})_{X}− Δ(ṙ_{e}^{n})_{X} \\ 0 \end{matrix} ) − \frac{1}{r_{e}+ h} ( \begin{matrix}(ṙ_{e}^{n})_{Y} \\ −(ṙ_{e}^{n})_{X} \\ 0 \end{matrix} ) \\ & = \frac{1}{r_{e}+ h} ( \begin{matrix}Δ(ṙ_{e}^{n})_{Y} \\ −Δ(ṙ_{e}^{n})_{X} \\ 0 \end{matrix} ) − \frac{1}{(r_{e}+ h)^{2}} ( \begin{matrix}(ṙ_{e}^{n})_{Y} \\ −(ṙ_{e}^{n})_{X} \\ 0 \end{matrix} ) Δh \end{aligned}

* ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) に関する部分は

\begin{aligned}Δ[q̃_{e}^{n}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) \end{matrix} \} q̃_{e}^{n}] & = q̃_{e}^{n}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} × Δr⃗_{e}) \end{matrix} \} q̃_{e}^{n} \\ & + Δq̃_{e}^{n}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) \end{matrix} \} q̃_{e}^{n}+q̃_{e}^{n}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) \end{matrix} \} Δq̃_{e}^{n} \end{aligned}

ここで

q̃_{e}^{n}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} × Δr⃗_{e}) \end{matrix} \} q̃_{e}^{n}

= 2Ω_{e/i}^{2}Rot[q̃_{e}^{n}]\{ ( \begin{matrix}q_{0}q_{2}+ q_{1}q_{3} \\ q_{3}q_{2}− q_{1}q_{0} \\ 0 \end{matrix} ) _{q̃_{e}^{n}}Δh + (R_{normal}+ h) [ \begin{matrix}q_{2} & q_{3} & q_{0} & q_{1} \\ −q_{1} & −q_{0} & q_{3} & q_{2} \\ 0 & 0 & 0 & 0 \end{matrix}]_{q̃_{e}^{n}}Δq̃_{e}^{n}\}

\begin{matrix}Δq̃_{e}^{n}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) \end{matrix} \} q̃_{e}^{n}+q̃_{e}^{n}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) \end{matrix} \} Δq̃_{e}^{n} \\ = 2\{ \begin{matrix}0 \\ [ \begin{matrix}ω_{1} & λ & −ω_{3} & ω_{2} \\ ω_{2} & ω_{3} & λ & −ω_{1} \\ ω_{3} & −ω_{2} & ω_{1} & λ \end{matrix} ] \begin{matrix}λ = q q q_{e}^{n}· v v v \\ ω = q_{e}^{n}_{0}v v v+ v v v× q q q_{e}^{n} \\ v v v= ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) \end{matrix} Δq̃_{e}^{n} \end{matrix}\} \end{matrix}

λ = 2Ω_{e/i}^{2}(R_{normal}+ h) ( \begin{matrix}q_{1} \\ q_{2} \\ q_{3} \end{matrix}) _{_{q q q}_{n}_{e}} · ( \begin{matrix}q_{0}q_{2}+ q_{1}q_{3} \\ q_{3}q_{2}− q_{1}q_{0} \\ 0 \end{matrix} ) _{q̃_{e}^{n}}
= 2Ω_{e/i}^{2}(R_{normal}+ h) q_{3}(q_{1}^{2}+ q_{2}^{2})
ω = 2Ω_{e/i}^{2}(R_{normal}+ h) ( q_{0} ( \begin{matrix}q_{0}q_{2}+ q_{1}q_{3} \\ q_{3}q_{2}− q_{1}q_{0} \\ 0 \end{matrix} ) _{q̃_{e}^{n}}+ ( \begin{matrix}q_{0}q_{2}+ q_{1}q_{3} \\ q_{3}q_{2}− q_{1}q_{0} \\ 0 \end{matrix} ) _{q̃_{e}^{n}}× ( \begin{matrix}q_{1} \\ q_{2} \\ q_{3} \end{matrix}) _{q̃_{e}^{n}})

= 2Ω_{e/i}^{2}(R_{normal}+ h) ( \begin{matrix}q_{2}(q_{0}^{2}+ q_{3}^{2}) \\ −q_{1}(q_{0}^{2}+ q_{3}^{2}) \\ q_{0}(q_{1}^{2}+ q_{2}^{2}) \end{matrix} ) _{q̃_{e}^{n}}

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

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

\begin{aligned}\frac{d}{dt}Δh & = −((ṙ_{e}^{n})_{Z}+ Δ(ṙ_{e}^{n})_{Z}) − (−(ṙ_{e}^{n})_{Z}) \\ & = −Δ(ṙ_{e}^{n})_{Z} \end{aligned}

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

\frac{d}{dt}q̃_{n}^{b} = \frac{1}{2} [( q̃_{n}^{b}+ Δq̃_{n}^{b}) \{ \begin{matrix}0 \\ ω⃗_{b/i}^{b} + Δω⃗_{b/i}^{b} \end{matrix} \}

−( \{ \begin{matrix}0 \\ ω⃗_{e/i}^{n} + Δω⃗_{e/i}^{n} \end{matrix} \} + \{ \begin{matrix}0 \\ ω⃗_{n/e}^{n} + Δω⃗_{n/e}^{n} \end{matrix} \} ) ( q̃_{n}^{b}+ Δq̃_{n}^{b}) ]

− \frac{1}{2} [q̃_{n}^{b}\{ 0 ; ω⃗_{b/i}^{b}\} − ( \{ 0 ; ω⃗_{e/i}^{n} \} + \{ 0 ; ω⃗_{n/e}^{n}\} ) q̃_{n}^{b}]

= \frac{1}{2} [Δq̃_{n}^{b}\{ 0 ; ω⃗_{b/i}^{b}\} +q̃_{n}^{b}\{ \begin{matrix}0 \\ Δω⃗_{b/i}^{b} \end{matrix} \}

−( \{ 0 ; ω⃗_{e/i}^{n} \} + \{ 0 ; ω⃗_{n/e}^{n}\} ) Δq̃_{n}^{b}− ( \{ \begin{matrix}0 \\ Δω⃗_{e/i}^{n} \end{matrix} \} + \{ \begin{matrix}0 \\ Δω⃗_{n/e}^{n} \end{matrix} \} ) q̃_{n}^{b}]

2 Quarternion Error Model

特殊な演算を考えることにする

exp(rrr) = \{ \begin{matrix}\{ \begin{matrix}cos(|rrr|) \\ sin(|rrr|)\frac{rrr}{|rrr|} \end{matrix}\} & (rrr ≠ 000) \\ \{ \begin{matrix}1 \\ 0 & 0 & 0 \end{matrix}\} & (rrr = 000) \end{matrix}  (1)
q̃+ Δq̃≡ exp(u u u) ∗ q̃  (2)
(q̃+ Δq̃)^{∗} ≡ (exp(u u u) ∗ q̃)^{∗}

= q̃^{∗}∗ exp(u u u)^{∗} = q̃^{∗}∗ exp(−u u u)

(3)

(q̃+ Δq̃)^{∗}\{ \begin{matrix}0 \\ v & v & v \end{matrix}\} (q̃+ Δq̃) ≡ (exp(u u u) ∗ q̃)^{∗}\{ \begin{matrix}0 \\ v & v & v \end{matrix}\} (exp(u u u) ∗ q̃)

= q̃^{∗}exp(−u u u)\{ \begin{matrix}0 \\ v & v & v \end{matrix}\} exp(u u u)q̃

(4)

\begin{aligned}q̃^{∗}\{ \begin{matrix}0 \\ v & v & v \end{matrix}\} q̃ & = \{ \begin{matrix}q_{0} \\ −q q q \end{matrix} \} \{ \begin{matrix}0 \\ v & v & v \end{matrix}\} \{ \begin{matrix}q_{0} \\ q & q & q \end{matrix}\} \\ & = \{ \begin{matrix}0 \\ (q_{0}^{2}− q q q· q q q)v v v− 2q_{0}q q q× v v v+ 2(q q q· v v v)q q q \end{matrix} \} \end{aligned}

より

exp(−u u u)\{ \begin{matrix}0 \\ v & v & v \end{matrix}\} exp(u u u) = \{ \begin{matrix}cos(| − u u u|) \\ sin(| − u u u|)\frac{−u u u}{|−u u u|} \end{matrix} \} \{ \begin{matrix}0 \\ v & v & v \end{matrix}\} \{ \begin{matrix}cos(|u u u|) \\ sin(|u u u|)\frac{u u u}{|u u u|} \end{matrix}\}

= \{ \begin{matrix}cos(|u u u|) \\ sin(|u u u|)\frac{−u u u}{|u u u|} \end{matrix}\} \{ \begin{matrix}0 \\ v & v & v \end{matrix}\} \{ \begin{matrix}cos(|u u u|) \\ sin(|u u u|)\frac{u u u}{|u u u|} \end{matrix}\}

= \{ \begin{matrix}0 \\ (cos(|u u u|)^{2}− sin(|u u u|)^{2} \frac{−u u u·u u u}{|u u u|^{2}})v v v− 2cos(|u u u|)sin(|u u u|)\frac{u u u}{|u u u|} × v v v+ 2(sin(|u u u|)\frac{u u u}{|u u u|} · v v v)sin(|u u u|)\frac{u u u}{|u u u|} \end{matrix} \}

= \{ \begin{matrix}0 \\ v v v− 2cos(|u u u|)sin(|u u u|)\frac{u u u}{|u u u|} × v v v+ 2(sin(|u u u|)\frac{u u u}{|u u u|} · v v v)sin(|u u u|)\frac{u u u}{|u u u|} \end{matrix} \}

(5)

ここで |u u u| ≪ 1 の仮定を用いると

\frac{sin(|u u u|)}{|u u u|} ≈ 1  (6)

\frac{sin(|u u u|)cos(|u u u|)}{|u u u|} ≈ 1  (7)
(u u u· v v v)u u u≈ 000  (8)

であるから、

exp(−u u u)\{ \begin{matrix}0 \\ v & v & v \end{matrix}\} exp(u u u) = \{ \begin{matrix}0 \\ v v v− 2cos(|u u u|)sin(|u u u|)\frac{u u u}{|u u u|} × v v v+ 2(sin(|u u u|)\frac{u u u}{|u u u|} · v v v)sin(|u u u|)\frac{u u u}{|u u u|} \end{matrix} \}

≈ \{ \begin{matrix}0 \\ v v v+ 2v v v× u u u \end{matrix} \}

= \{ \begin{matrix}0 \\ v v v+ 2[ \begin{matrix}0 & −v_{2} & v_{1} \\ v_{2} & 0 & −v_{0} \\ −v_{1} & v_{0} & 0 \end{matrix} ]_{v v v}u u u \end{matrix} \}

(9)

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

\frac{d}{dt}\{ \begin{matrix}0 \\ Δṙ_{e}^{n} \end{matrix}\} = \frac{d}{dt}( \{ \begin{matrix}0 \\ ṙ_{e}^{n}+ Δṙ_{e}^{n} \end{matrix} \} − \{ \begin{matrix}0 \\ ṙ_{e}^{n} \end{matrix}\} )

= [(q̃_{b}^{n}+ Δq̃_{b}^{n}) ^{∗}\{ \begin{matrix}0 \\ a^{b}+ Δa^{b} \end{matrix} \} (q̃_{b}^{n}+ Δq̃_{b}^{n}) + \{ \begin{matrix}0 \\ g^{n}+ Δg^{n} \end{matrix} \}

− \{ \begin{matrix}0 \\ ( 2( ω⃗_{e/i}^{n} + Δω⃗_{e/i}^{n} ) + ( ω⃗_{n/e}^{n} + Δω⃗_{n/e}^{n}) ) × (ṙ_{e}^{n}+ Δṙ_{e}^{n}) \end{matrix} \}

− (q̃_{e}^{n}+ Δq̃_{e}^{n}) ^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} × (r⃗_{e}+ Δr⃗_{e}) ) \end{matrix} \} (q̃_{e}^{n}+ Δq̃_{e}^{n}) ]

− [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} \} −q̃_{e}^{n}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) \end{matrix} \} q̃_{e}^{n}]

= ( exp(u u u_{n}^{b}) ∗q̃_{n}^{b}) \{ \begin{matrix}0 \\ a^{b}+ Δa^{b} \end{matrix} \} ( exp(u u u_{n}^{b}) ∗q̃_{n}^{b}) ^{∗}−q̃_{n}^{b}\{ \begin{matrix}0 \\ & a^{b} \end{matrix}\} q̃_{n}^{b}^{∗}+ \{ \begin{matrix}0 \\ Δg^{n} \end{matrix}\}

− \{ \begin{matrix}0 \\ ( 2Δω⃗_{e/i}^{n} + Δω⃗_{n/e}^{n}) ×ṙ_{e}^{n}+ ( 2ω⃗_{e/i}^{n} + ω⃗_{n/e}^{n}) × Δṙ_{e}^{n} \end{matrix} \}

− (exp(u u u_{e}^{n}) ∗q̃_{e}^{n})^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) \end{matrix} \} (exp(u u u_{e}^{n}) ∗q̃_{e}^{n})+q̃_{e}^{n}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) \end{matrix} \} q̃_{e}^{n}

q̃_{e}^{n}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} × Δr⃗_{e}) \end{matrix} \} q̃_{e}^{n}

= exp(u u u_{n}^{b})\{ \begin{matrix}& 0 \\ Rot & [q̃_{n}^{b}^{∗}] & (a^{b}+ Δa^{b}) \end{matrix} \} exp(−u u u_{n}^{b}) − \{ \begin{matrix}0 \\ Rot & [q̃_{n}^{b}^{∗}] & a^{b} \end{matrix}\} + \{ \begin{matrix}0 \\ Δg^{n} \end{matrix}\}

− \{ \begin{matrix}0 \\ ( 2Δω⃗_{e/i}^{n} + Δω⃗_{n/e}^{n}) ×ṙ_{e}^{n}+ ( 2ω⃗_{e/i}^{n} + ω⃗_{n/e}^{n}) × Δṙ_{e}^{n} \end{matrix} \}

−q̃_{e}^{n}^{∗}\{ −exp(u u u_{e}^{n})\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) \end{matrix} \} exp(u u u_{e}^{n})− \{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) \end{matrix} \} \} q̃_{e}^{n}

− \{ \begin{matrix}0 \\ Rot & [q̃_{e}^{n}] & ( ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} × Δr⃗_{e}) ) \end{matrix} \}

≈ \{ \begin{matrix}0 \\ Rot[q̃_{n}^{b}^{∗}]Δa^{b}− 2Rot[q̃_{n}^{b}^{∗}]a^{b}× u u u_{n}^{b} \end{matrix} \} + \{ \begin{matrix}0 \\ Δg^{n} \end{matrix}\}

− \{ \begin{matrix}0 \\ ( 2Δω⃗_{e/i}^{n} + Δω⃗_{n/e}^{n}) ×ṙ_{e}^{n}+ ( 2ω⃗_{e/i}^{n} + ω⃗_{n/e}^{n}) × Δṙ_{e}^{n} \end{matrix} \}

− \{ \begin{matrix}0 \\ 2Rot[q̃_{e}^{n}]( ( ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) ) × u u u_{e}^{n}) \end{matrix} \} − \{ \begin{matrix}0 \\ Rot & [q̃_{e}^{n}] & ( ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} × Δr⃗_{e}) ) \end{matrix} \}

(10)

\{ \begin{matrix}0 \\ Δω⃗_{e/i}^{n} \end{matrix} \} = (q̃_{e}^{n}+ Δq̃_{e}^{n}) ^{∗}\{ 0 ; ω⃗_{e/i}^{i} \} (q̃_{e}^{n}+ Δq̃_{e}^{n}) −q̃_{e}^{n}^{∗}\{ 0 ; ω⃗_{e/i}^{i} \} q̃_{e}^{n}

≡ (exp(u u u_{e}^{n}) ∗q̃_{e}^{n})^{∗}{ 0 ; ω⃗_{e/i}^{i} } (exp(u u u_{e}^{n}) ∗q̃_{e}^{n})−q̃_{e}^{n}^{∗}{ 0 ; ω⃗_{e/i}^{i} } q̃_{e}^{n}

= q̃_{e}^{n}^{∗}exp(−u u u_{e}^{n}){ 0 ; ω⃗_{e/i}^{i} } exp(u u u_{e}^{n})q̃_{e}^{n}−q̃_{e}^{n}^{∗}{ 0 ; ω⃗_{e/i}^{i} } q̃_{e}^{n}

(11)

\begin{aligned}\{ \begin{matrix}0 \\ Δω⃗_{e/i}^{n} \end{matrix} \} & ≈ q̃_{e}^{n}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{i} + 2ω⃗_{e/i}^{i} × u u u_{e}^{n} \end{matrix} \} q̃_{e}^{n}−q̃_{e}^{n}^{∗}\{ 0 ; ω⃗_{e/i}^{i} \} q̃_{e}^{n} \\ & = q̃_{e}^{n}^{∗}\{ \begin{matrix}0 \\ 2ω⃗_{e/i}^{i} × u u u_{e}^{n} \end{matrix} \} q̃_{e}^{n} \end{aligned}  (12)
\begin{aligned}Δω⃗_{e/i}^{n} & = Rot[q̃_{e}^{n}](2ω⃗_{e/i}^{i} × u u u_{e}^{n}) \\ & = [ \begin{matrix}R_{00} & R_{01} & R_{02} \\ R_{10} & R_{11} & R_{12} \\ R_{20} & R_{21} & R_{22} \end{matrix}]_{q̃_{e}^{n}}2Ω_{e/i} [ \begin{matrix}0 & −1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 0 \end{matrix}] u u u_{e}^{n} \\ & = 2Ω_{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 u u_{e}^{n} \end{aligned}  (13)
Δω⃗_{n/e}^{n} = ( \frac{Δ(ṙ_{e}^{n})_{Y}}{R_{normal}+h} −\frac{Δ(ṙ_{e}^{n})_{X}}{R_{meridian}+h} 0 ) − ( \frac{(ṙ_{e}^{n})_{Y}}{(R_{normal}+h)^{2}} −\frac{(ṙ_{e}^{n})_{X}}{(R_{meridian}+h)^{2}} 0 ) Δh

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

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

(14)

ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) = 2Ω_{e/i}^{2}(R_{normal}+ h) ( \begin{matrix}q_{0}q_{2}+ q_{1}q_{3} \\ q_{3}q_{2}− q_{1}q_{0} \\ 0 \end{matrix} )

= Ω_{e/i}^{2}(R_{normal}+ h) [ \begin{matrix}R_{20} \\ R_{21} \\ 0 \end{matrix}]_{Rot_{[_{q̃_{e}^{n}}]}}

(15)

2Rot[q̃_{e}^{n}]( ( ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) ) × u u u_{e}^{n})

= 2Rot[q̃_{e}^{n}][ \begin{matrix}0 & −v_{2} & v_{1} \\ v_{2} & 0 & −v_{0} \\ −v_{1} & v_{0} & 0 \end{matrix} ]_{ω⃗_{e/i}^{e} ×( ω⃗_{e/i}^{e} ×r⃗_{e})} u u u_{e}^{n}

= 2Ω_{e/i}^{2}(R_{normal}+ h) Rot[q̃_{e}^{n}][ \begin{matrix}0 & 0 & R_{21} \\ 0 & 0 & −R_{20} \\ −R_{21} & R_{20} & 0 \end{matrix} ]_{Rot_{[_{q̃_{e}^{n}}]}}u u u_{e}^{n}

= 2Ω_{e/i}^{2}(R_{normal}+ h) [ \begin{matrix}−R_{02}R_{21} & R_{02}R_{20} & R_{00}R_{21}− R_{01}R_{20} \\ −R_{12}R_{21} & R_{12}R_{20} & R_{10}R_{21}− R_{11}R_{20} \\ −R_{22}R_{21} & R_{22}R_{20} & R_{20}R_{21}− R_{21}R_{20} \end{matrix}]_{Rot_{[_{q̃_{e}^{n}}]}}u u u_{e}^{n}

(16)

\begin{aligned}Δq̃ & = \{ \begin{matrix}1 \\ u & u & u \end{matrix}\} q̃− q̃ \\ & = \{ \begin{matrix}q_{0}− u u u· q q q \\ q q q+ q_{0}u u u+ u u u× q q q \end{matrix} \} − \{ \begin{matrix}q_{0} \\ q & q & q \end{matrix}\} \\ & = \{ \begin{matrix}−u u u· q q q \\ q_{0}u u u+ u u u× q q q \end{matrix} \} \\ & = [ \begin{matrix}−q_{1} & −q_{2} & −q_{3} \\ q_{0} & −q_{3} & q_{2} \\ q_{3} & q_{0} & −q_{1} \\ −q_{2} & q_{1} & q_{0} \end{matrix} ] u u u \end{aligned}  (17)

Rot[q̃_{e}^{n}]( ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} × Δr⃗_{e}) )

= 2Ω_{e/i}^{2}Rot[q̃_{e}^{n}]\{ ( \begin{matrix}q_{0}q_{2}+ q_{1}q_{3} \\ q_{3}q_{2}− q_{1}q_{0} \\ 0 \end{matrix} ) _{q̃_{e}^{n}}Δh + (R_{normal}+ h) [ \begin{matrix}q_{2} & q_{3} & q_{0} & q_{1} \\ −q_{1} & −q_{0} & q_{3} & q_{2} \\ 0 & 0 & 0 & 0 \end{matrix}]_{q̃_{e}^{n}}Δq̃_{e}^{n}\}

= 2Ω_{e/i}^{2}Rot[q̃_{e}^{n}]\{ ( \begin{matrix}q_{0}q_{2}+ q_{1}q_{3} \\ q_{3}q_{2}− q_{1}q_{0} \\ 0 \end{matrix} ) _{q̃_{e}^{n}}Δh

+(R_{normal}+ h) [ \begin{matrix}q_{2} & q_{3} & q_{0} & q_{1} \\ −q_{1} & −q_{0} & q_{3} & q_{2} \\ 0 & 0 & 0 & 0 \end{matrix}]_{q̃_{e}^{n}} [ \begin{matrix}−q_{1} & −q_{2} & −q_{3} \\ q_{0} & −q_{3} & q_{2} \\ q_{3} & q_{0} & −q_{1} \\ −q_{2} & q_{1} & q_{0} \end{matrix} ]_{q̃_{e}^{n}}u u u\}

= 2Ω_{e/i}^{2}Rot[q̃_{e}^{n}]\{ ( \begin{matrix}q_{0}q_{2}+ q_{1}q_{3} \\ q_{3}q_{2}− q_{1}q_{0} \\ 0 \end{matrix} ) _{q̃_{e}^{n}}Δh

+ (R_{normal}+ h) [ \begin{matrix}2(q_{0}q_{3}− q_{1}q_{2}) & 1 − 2(q_{2}^{2}+ q_{3}^{2}) & 0 \\ 1 − 2(q_{0}^{2}+ q_{2}^{2}) & 2(q_{0}q_{3}+ q_{1}q_{2}) & 0 \\ 0 & 0 & 0 \end{matrix}]_{q̃_{e}^{n}}u u u\}

= 2Ω_{e/i}^{2}Rot[q̃_{e}^{n}]\{ \frac{1}{2} [ \begin{matrix}R_{20} \\ R_{21} \\ 0 \end{matrix}]_{Rot_{[_{q̃_{e}^{n}}]}}Δh + (R_{normal}+ h) [ \begin{matrix}−R_{10} & R_{00} & 0 \\ −R_{11} & R_{01} & 0 \\ 0 & 0 & 0 \end{matrix}]_{Rot_{[_{q̃_{e}^{n}}]}}u u u\}

= Ω_{e/i}^{2} \{ [ \begin{matrix}R_{00}R_{20}− R_{01}R_{21} \\ R_{10}R_{20}− R_{11}R_{21} \\ R_{20}R_{20}− R_{21}R_{21} \end{matrix}]_{Rot_{[_{q̃_{e}^{n}}]}}Δh

+2(R_{normal}+ h) [ \begin{matrix}−R_{00}R_{10}− R_{01}R_{11} & R_{00}^{2}+ R_{01}^{2} & 0 \\ −R_{10}^{2}− R_{11}^{2} & R_{10}R_{00}+ R_{11}R_{01} & 0 \\ −R_{20}R_{10}− R_{21}R_{11} & R_{20}R_{00}+ R_{21}R_{01} & 0 \end{matrix}]_{Rot_{[_{q̃_{e}^{n}}]}}u u u_{e}^{n}\}

(18)

\frac{d}{dt}Δṙ_{e}^{n} = Rot[q̃_{n}^{b}^{∗}]Δa^{b}− 2Rot[q̃_{n}^{b}^{∗}]a^{b}× u u u_{n}^{b}+ Δg^{n}+ṙ_{e}^{n}× ( 2Δω⃗_{e/i}^{n} + Δω⃗_{n/e}^{n}) − ( 2ω⃗_{e/i}^{n} + ω⃗_{n/e}^{n}) × Δṙ_{e}^{n}

− 2Rot[q̃_{e}^{n}]( ( ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) ) × u u u_{e}^{n}) − Rot[q̃_{e}^{n}]( ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} × Δr⃗_{e}) )

= Rot[q̃_{n}^{b}^{∗}]Δa^{b}− 2( \begin{matrix}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_{02}a_{0}^{b}+ R_{12}a_{1}^{b}+ R_{22}a_{2}^{b} \end{matrix} ) _{q̃_{n}^{b}}× u u u_{n}^{b}+ Δg^{n}

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

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

+ 4Ω_{e/i} [ \begin{matrix}0 & −ṙ_{n}^{e}_{Z} & ṙ_{n}^{e}_{Y} \\ ṙ_{n}^{e}_{Z} & 0 & −ṙ_{n}^{e}_{X} \\ −ṙ_{n}^{e}_{Y} & ṙ_{n}^{e}_{X} & 0 \end{matrix} ] [ \begin{matrix}R_{01} & −R_{00} & 0 \\ R_{11} & −R_{10} & 0 \\ R_{21} & −R_{20} & 0 \end{matrix}]_{q̃_{e}^{n}}u u 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}

− 2Ω_{e/i}^{2}(R_{normal}+ h) [ \begin{matrix}−R_{02}R_{21} & R_{02}R_{20} & R_{00}R_{21}− R_{01}R_{20} \\ −R_{12}R_{21} & R_{12}R_{20} & R_{10}R_{21}− R_{11}R_{20} \\ −R_{22}R_{21} & R_{22}R_{20} & R_{20}R_{21}− R_{21}R_{20} \end{matrix}]_{Rot_{[_{q̃_{e}^{n}}]}}u u u_{e}^{n}

− Ω_{e/i}^{2} \{ [ \begin{matrix}R_{00}R_{20}− R_{01}R_{21} \\ R_{10}R_{20}− R_{11}R_{21} \\ R_{20}R_{20}− R_{21}R_{21} \end{matrix}]_{Rot_{[_{q̃_{e}^{n}}]}}Δh

+2(R_{normal}+ h) [ \begin{matrix}−R_{00}R_{10}− R_{01}R_{11} & R_{00}^{2}+ R_{01}^{2} & 0 \\ −R_{10}^{2}− R_{11}^{2} & R_{10}R_{00}+ R_{11}R_{01} & 0 \\ −R_{20}R_{10}− R_{21}R_{11} & R_{20}R_{00}+ R_{21}R_{01} & 0 \end{matrix}]_{Rot_{[_{q̃_{e}^{n}}]}}u u u_{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 u 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 u 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}− Ω_{e/i}^{2} [ \begin{matrix}R_{00}R_{20}− R_{01}R_{21} \\ R_{10}R_{20}− R_{11}R_{21} \\ R_{20}R_{20}− R_{21}R_{21} \end{matrix}]_{Rot_{[_{q̃_{e}^{n}}]}}Δh

+ 2Ω_{e/i}^{2}(R_{normal}+ h)

[ \begin{matrix}R_{00}R_{10}+ R_{01}R_{11}+ R_{02}R_{21} & −R_{02}R_{20}− R_{00}^{2}− R_{01}^{2} & −R_{00}R_{21}+ R_{01}R_{20} \\ R_{12}R_{21}+ R_{10}^{2}+ R_{11}^{2} & −R_{12}R_{20}− R_{10}R_{00}− R_{11}R_{01} & −R_{10}R_{21}+ R_{11}R_{20} \\ R_{22}R_{21}+ R_{20}R_{10}+ R_{21}R_{11} & −R_{22}R_{20}− R_{20}R_{00}− R_{21}R_{01} & −R_{20}R_{21}+ R_{21}R_{20} \end{matrix} ]_{Rot_{[_{q̃_{e}^{n}}]}}u u u_{e}^{n}

(19)

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

\begin{aligned}\frac{d}{dt}(q̃_{e}^{n}+ Δq̃_{e}^{n}) & = \frac{1}{2}(q̃_{e}^{n}+ Δq̃_{e}^{n})\{ \begin{matrix}0 \\ ω⃗_{n/e}^{n} + Δω⃗_{n/e}^{n} \end{matrix} \} \\ & ≡ \frac{1}{2} exp(u u u_{e}^{n}) ∗q̃_{e}^{n}\{ \begin{matrix}0 \\ ω⃗_{n/e}^{n} + Δω⃗_{n/e}^{n} \end{matrix} \} \end{aligned}  (20)
\frac{d}{dt}(q̃_{e}^{n}+ Δq̃_{e}^{n}) ≡ \frac{d}{dt}(exp(u u u_{e}^{n}) ∗q̃_{e}^{n})

= \frac{d}{dt}exp(u u u_{e}^{n}) ∗q̃_{e}^{n}+ exp(u u u_{e}^{n}) ∗ \frac{d}{dt}q̃_{e}^{n}

(21)

\frac{d}{dt}exp(u u u_{e}^{n}) = ( \frac{d}{dt}(q̃_{e}^{n}+ Δq̃_{e}^{n}) − exp(u u u_{e}^{n}) ∗ \frac{d}{dt}q̃_{e}^{n}) q̃_{e}^{n}^{∗}

= ( \frac{1}{2} exp(u u u_{e}^{n}) ∗q̃_{e}^{n}\{ \begin{matrix}0 \\ ω⃗_{n/e}^{n} + Δω⃗_{n/e}^{n} \end{matrix} \} − exp(u u u_{e}^{n}) ∗ \frac{1}{2}q̃_{e}^{n}\{ 0 ; ω⃗_{n/e}^{n}\} ) q̃_{e}^{n}^{∗}

= \frac{1}{2} exp(u u u_{e}^{n}) ∗q̃_{e}^{n}\{ \begin{matrix}0 \\ Δω⃗_{n/e}^{n} \end{matrix} \} q̃_{e}^{n}^{∗}

(22)

ここで |u u u| ≪ 1 の仮定を用いると

exp(u u u_{e}^{n}) ≈ \{ \begin{matrix}1 \\ u u u & n e \end{matrix}\}  (23)

より

\frac{d}{dt}exp(u u u_{e}^{n}) ≈ \frac{d}{dt}\{ \begin{matrix}1 \\ u u u & n e \end{matrix}\} = \{ \begin{matrix}0 \\ \frac{d}{dt} & u u u & n e \end{matrix}\}  (24)

従って

\frac{d}{dt}\{ \begin{matrix}0 \\ u u u & n e \end{matrix}\} = \frac{1}{2} \{ \begin{matrix}1 \\ u u u & n e \end{matrix}\} q̃_{e}^{n}\{ \begin{matrix}0 \\ Δω⃗_{n/e}^{n} \end{matrix} \} q̃_{e}^{n}^{∗}

= \frac{1}{2} \{ \begin{matrix}1 \\ u u u & n e \end{matrix}\} \{ \begin{matrix}0 \\ Rot[q̃_{e}^{n}^{∗}]Δω⃗_{n/e}^{n} \end{matrix} \}

= \frac{1}{2} \{ \begin{matrix}0 \\ Rot[q̃_{e}^{n}^{∗}]Δω⃗_{n/e}^{n} \end{matrix} \}

(25)

\frac{d}{dt}u u u_{e}^{n} = \frac{1}{2}Rot[q̃_{e}^{n}^{∗}]Δω⃗_{n/e}^{n}

= \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̃_{e}^{n}} \{ \frac{1}{r_{e}+ h} [ \begin{matrix}0 & 1 & 0 \\ −1 & 0 & 0 \\ 0 & 0 & 0 \end{matrix}] Δṙ_{e}^{n}− \frac{1}{(r_{e}+ h)^{2}} ( \begin{matrix}(ṙ_{e}^{n})_{Y} \\ −(ṙ_{e}^{n})_{X} \\ 0 \end{matrix} ) Δh\}

= \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

(26)

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

\frac{d}{dt}(q̃_{n}^{b}+ Δq̃_{n}^{b}) = \frac{1}{2} \{ (q̃_{n}^{b}+ Δq̃_{n}^{b})\{ \begin{matrix}0 \\ ω⃗_{b/i}^{b} + Δω⃗_{b/i}^{b} \end{matrix} \}

−( \{ \begin{matrix}0 \\ ω⃗_{e/i}^{n} + Δω⃗_{e/i}^{n} \end{matrix} \} + \{ \begin{matrix}0 \\ ω⃗_{n/e}^{n} + Δω⃗_{n/e}^{n} \end{matrix} \} ) (q̃_{n}^{b}+ Δq̃_{n}^{b})\}

≡ \frac{1}{2} \{ (exp(u u u_{n}^{b}) ∗q̃_{n}^{b})\{ \begin{matrix}0 \\ ω⃗_{b/i}^{b} + Δω⃗_{b/i}^{b} \end{matrix} \}

−( \{ \begin{matrix}0 \\ ω⃗_{e/i}^{n} + Δω⃗_{e/i}^{n} \end{matrix} \} + \{ \begin{matrix}0 \\ ω⃗_{n/e}^{n} + Δω⃗_{n/e}^{n} \end{matrix} \} ) (exp(u u u_{n}^{b}) ∗q̃_{n}^{b})\}

(27)

\frac{d}{dt}(q̃_{n}^{b}+ Δq̃_{n}^{b}) ≡ \frac{d}{dt}(exp(u u u_{n}^{b}) ∗q̃_{n}^{b})

= \frac{d}{dt}exp(u u u_{n}^{b}) ∗q̃_{n}^{b}+ exp(u u u_{n}^{b}) ∗ \frac{d}{dt}q̃_{n}^{b}

(28)

\frac{d}{dt}exp(u u u_{n}^{b}) = ( \frac{d}{dt}(q̃_{n}^{b}+ Δq̃_{n}^{b}) − exp(u u u_{n}^{b}) ∗ \frac{d}{dt}q̃_{n}^{b}) q̃_{n}^{b}^{∗}

= [ \frac{1}{2} \{ (exp(u u u_{n}^{b}) ∗q̃_{n}^{b})\{ \begin{matrix}0 \\ ω⃗_{b/i}^{b} + Δω⃗_{b/i}^{b} \end{matrix} \}

−( \{ \begin{matrix}0 \\ ω⃗_{e/i}^{n} + Δω⃗_{e/i}^{n} \end{matrix} \} + \{ \begin{matrix}0 \\ ω⃗_{n/e}^{n} + Δω⃗_{n/e}^{n} \end{matrix} \} ) (exp(u u u_{n}^{b}) ∗q̃_{n}^{b})\}

−exp(u u u_{n}^{b})\frac{1}{2} \{ q̃_{n}^{b}\{ 0 ; ω⃗_{b/i}^{b}\} − ( \{ 0 ; ω⃗_{e/i}^{n} \} + \{ 0 ; ω⃗_{n/e}^{n}\} ) q̃_{n}^{b}\} ] q̃_{n}^{b}^{∗}

= \frac{1}{2} [(exp(u u u_{n}^{b}) ∗q̃_{n}^{b})\{ \begin{matrix}0 \\ Δω⃗_{b/i}^{b} \end{matrix} \}

− ( \{ \begin{matrix}0 \\ ω⃗_{e/i}^{n} + Δω⃗_{e/i}^{n} \end{matrix} \} + \{ \begin{matrix}0 \\ ω⃗_{n/e}^{n} + Δω⃗_{n/e}^{n} \end{matrix} \} ) (exp(u u u_{n}^{b}) ∗q̃_{n}^{b})

+exp(u u u_{n}^{b})( { 0 ; ω⃗_{e/i}^{n} } + { 0 ; ω⃗_{n/e}^{n}} ) q̃_{n}^{b}] q̃_{n}^{b}^{∗}

= \frac{1}{2} [exp(u u u_{n}^{b})q̃_{n}^{b}\{ \begin{matrix}0 \\ Δω⃗_{b/i}^{b} \end{matrix} \} q̃_{n}^{b}^{∗}

− ( \{ \begin{matrix}0 \\ ω⃗_{e/i}^{n} + Δω⃗_{e/i}^{n} \end{matrix} \} + \{ \begin{matrix}0 \\ ω⃗_{n/e}^{n} + Δω⃗_{n/e}^{n} \end{matrix} \} ) exp(u u u_{n}^{b})

+exp(u u u_{n}^{b})( { 0 ; ω⃗_{e/i}^{n} } + { 0 ; ω⃗_{n/e}^{n}} ) ]

(29)

ここで |u u u_{n}^{b}| ≪ 1 であるから exp(u u u_{e}^{n}) ≈ \{ \begin{matrix}1 \\ u u u & n e \end{matrix}\} より

\frac{d}{dt}exp(u u u_{n}^{b}) ≈ \frac{1}{2} [\{ \begin{matrix}1 \\ u u u & b n \end{matrix}\} q̃_{n}^{b}\{ \begin{matrix}0 \\ Δω⃗_{b/i}^{b} \end{matrix} \} q̃_{n}^{b}^{∗}

− ( \{ \begin{matrix}0 \\ ω⃗_{e/i}^{n} + Δω⃗_{e/i}^{n} \end{matrix} \} + \{ \begin{matrix}0 \\ ω⃗_{n/e}^{n} + Δω⃗_{n/e}^{n} \end{matrix} \} ) \{ \begin{matrix}1 \\ u u u & b n \end{matrix}\}

+\{ \begin{matrix}1 \\ u u u & b n \end{matrix}\} ( \{ 0 ; ω⃗_{e/i}^{n} \} + \{ 0 ; ω⃗_{n/e}^{n}\} ) ]

= \frac{1}{2} [\{ \begin{matrix}1 \\ u u u & b n \end{matrix}\} q̃_{n}^{b}\{ \begin{matrix}0 \\ Δω⃗_{b/i}^{b} \end{matrix} \} q̃_{n}^{b}^{∗}

−\{ \begin{matrix}0 \\ Δω⃗_{e/i}^{n} + Δω⃗_{n/e}^{n} + 2(ω⃗_{e/i}^{n} + ω⃗_{n/e}^{n}) × u u u_{n}^{b} \end{matrix} \} ]

(30)

\frac{d}{dt}u u u_{n}^{b} = \frac{1}{2} \{Rot[q̃_{n}^{b}^{∗}]Δω⃗_{b/i}^{b} − Δω⃗_{e/i}^{n} − Δω⃗_{n/e}^{n} − 2(ω⃗_{e/i}^{n} + ω⃗_{n/e}^{n}) × u u 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 u 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 u u_{n}^{b}

(31)

3 Initialize

地球上で静止している状態では

• 速度の運動方程式

\{ \begin{matrix}0 \\ 0 \end{matrix}\} = q̃_{n}^{b}\{ \begin{matrix}0 \\  & a^{b} \end{matrix}\} q̃_{n}^{b}^{∗}+ \{ \begin{matrix}0 \\  & g^{n} \end{matrix}\}  (32)

• 位置の運動方程式

\{ \begin{matrix}0 \\ 0 \end{matrix}\} = \frac{1}{2}q̃_{e}^{n}\{ 0 ; ω⃗_{n/e}^{e}\}  (33)
∵ ω⃗_{n/e}^{n} = 0  (34)

• 姿勢の運動方程式

\{ \begin{matrix}0 \\ 0 \end{matrix}\} = \frac{1}{2} [q̃_{n}^{b}\{ 0 ; ω⃗_{b/i}^{b}\} − \{ 0 ; ω⃗_{e/i}^{n} \} q̃_{n}^{b}]  (35)

以上より

q̃_{n}^{b}\{ \begin{matrix}0 \\  & a^{b} \end{matrix}\} q̃_{n}^{b}^{∗}+ \{ \begin{matrix}0 \\  & g^{n} \end{matrix}\} = 0  (36)
q̃_{n}^{b}{ 0 ; ω⃗_{b/i}^{b}} = { 0 ; ω⃗_{e/i}^{n} } q̃_{n}^{b}  (37)

3.1 速度の運動方程式からの Initialize

初期姿勢を推定する。

\begin{aligned}0 & = q̃_{n}^{b}\{ \begin{matrix}0 \\  & a^{b} \end{matrix}\} + \{ \begin{matrix}0 \\  & g^{n} \end{matrix}\} q̃_{n}^{b} \\ & = \{ \begin{matrix}−q⃗· a⃗ \\ q_{0}a⃗+ q⃗× a⃗ \end{matrix} \} + \{ \begin{matrix}−g⃗· q⃗ \\ q_{0}g⃗+ g⃗× q⃗ \end{matrix} \} \\ & = \{ \begin{matrix}−q⃗· (a⃗+ g⃗) \\ q_{0}(a⃗+ g⃗) + q⃗× (a⃗− g⃗) \end{matrix} \} \\ & = [ \begin{matrix}0 & −(a⃗+ g⃗)_{1} & −(a⃗+ g⃗)_{2} & −(a⃗+ g⃗)_{3} \\ (a⃗+ g⃗)_{1} & 0 & (a⃗− g⃗)_{3} & −(a⃗− g⃗)_{2} \\ (a⃗+ g⃗)_{2} & −(a⃗− g⃗)_{3} & 0 & (a⃗− g⃗)_{1} \\ (a⃗+ g⃗)_{3} & (a⃗− g⃗)_{2} & −(a⃗− g⃗)_{1} & 0 \end{matrix} ] [ q_{0} ; q⃗] \end{aligned}  (38)

直接的に [ q_{0} ; q⃗] を求めるわけにはいかないので、工夫して iteration する。 • 方法 1

q̃_{k+1} ≡ \{ \begin{matrix}1 \\ −u u u \end{matrix} \} q̃_{k}= [ \begin{matrix}1 & u_{1} & u_{2} & u_{3} \\ −u_{1} & 1 & u_{3} & −u_{2} \\ −u_{2} & −u_{3} & 1 & u_{1} \\ −u_{3} & u_{2} & −u_{1} & 1 \end{matrix} ] [ q_{0} ; q⃗]_{k}  (39)

代入して

0 = [ \begin{matrix}0 & −(a⃗+ g⃗)_{1} & −(a⃗+ g⃗)_{2} & −(a⃗+ g⃗)_{3} \\ (a⃗+ g⃗)_{1} & 0 & (a⃗− g⃗)_{3} & −(a⃗− g⃗)_{2} \\ (a⃗+ g⃗)_{2} & −(a⃗− g⃗)_{3} & 0 & (a⃗− g⃗)_{1} \\ (a⃗+ g⃗)_{3} & (a⃗− g⃗)_{2} & −(a⃗− g⃗)_{1} & 0 \end{matrix} ] [ \begin{matrix}1 & u_{1} & u_{2} & u_{3} \\ −u_{1} & 1 & u_{3} & −u_{2} \\ −u_{2} & −u_{3} & 1 & u_{1} \\ −u_{3} & u_{2} & −u_{1} & 1 \end{matrix} ] [ q_{0} ; q⃗]_{k}  (40)

整理すると

A = B[ \begin{matrix}u_{1} \\ u_{2} \\ u_{3} \end{matrix}]  (41)
A ≡ −[ \begin{matrix}−q_{3}(a + g)_{3}− q_{2}(a + g)_{2}− q_{1}(a + g)_{1} \\ q_{2}(a − g)_{3}− q_{3}(a − g)_{2}+ q_{0}(a + g)_{1} \\ −q_{1}(a − g)_{3}+ q_{0}(a + g)_{2}+ q_{3}(a − g)_{1} \\ q_{0}(a + g)_{3}+ q_{1}(a − g)_{2}− q_{2}(a − g)_{1} \end{matrix}  ]  (42)

B ≡ [ \begin{matrix}−q_{3}(a + g)_{2}+ q_{2}(a + g)_{3}+ q_{0}(a + g)_{1} & q_{3}(a + g)_{1}− q_{1}(a + g)_{3}+ q_{0}(a + g)_{2} & −q_{2}(a + g)_{1}+ q_{1}(−a + g)_{2}+ q_{0}(a + g)_{3} \\ q_{2}(a − g)_{2}+ q_{3}(a − g)_{3}+ q_{1}(a + g)_{1} & q_{2}(a + g)_{1}− q_{0}(a − g)_{3}− q_{1}(a − g)_{2} & q_{3}(a + g)_{1}+ q_{0}(a − g)_{2}− q_{1}(a − g)_{3} \\ q_{1}(a + g)_{2}− q_{0}(a − g)_{3}− q_{2}(a − g)_{1} & q_{1}(a − g)_{1}+ q_{3}(a − g)_{3}+ q_{2}(a + g)_{2} & −q_{0}(a − g)_{1}+ q_{3}(a + g)_{2}− q_{2}(a − g)_{3} \\ −q_{0}(a − g)_{2}+ q_{1}(a + g)_{3}− q_{3}(a − g)_{1} & q_{0}(a − g)_{1}+ q_{2}(a + g)_{3}− q_{3}(a − g)_{2} & q_{1}(a − g)_{1}+ q_{2}(a − g)_{2}+ q_{3}(a + g)_{3} \end{matrix}  ] (43)

最小二乗法を適用して

B^{T}B[ \begin{matrix}u_{1} \\ u_{2} \\ u_{3} \end{matrix}] = B^{T}A  (44)

[ \begin{matrix}u_{1} \\ u_{2} \\ u_{3} \end{matrix}] = (B^{T}B)^{−1} B^{T}A  (45)

補足

A_{4×1} = B_{4×3} u u u_{3×1} + δδδ_{4×1}  (46)
δδδ = A − Bu u u  (47)
minimize. δδδ^{T}δδδ = (A − Bu u u)^{T}(A − Bu u u)

= A^{T}A + u u u^{T}B^{T}Bu u u− A^{T}Bu u u− u u u^{T}B^{T}A

(48)

then. 0 = \frac{δδδ^{T}δδδ}{∂u u u^{T}}

= B^{T}Bu u u− B^{T}A

(49)

∴ u u u= (B^{T}B)^{−1} B^{T}A  (50)

• 方法 2 Yawing が 0 固定の場合

q̃_{k}= \{ \begin{matrix}cos & \frac{θ}{2} & cos & \frac{ϕ}{2} \\ cos & \frac{θ}{2} & sin & \frac{ϕ}{2} \\ sin & \frac{θ}{2} & cos & \frac{ϕ}{2} \\ −sin \frac{θ}{2} sin \frac{ϕ}{2} \end{matrix} \}  (51)

q̃_{k+1} = \{ \begin{matrix}cos & \frac{θ+Δθ}{2} & cos & \frac{ϕ+Δϕ}{2} \\ cos & \frac{θ+Δθ}{2} & sin & \frac{ϕ+Δϕ}{2} \\ sin & \frac{θ+Δθ}{2} & cos & \frac{ϕ+Δϕ}{2} \\ −sin \frac{θ+Δθ}{2} sin \frac{ϕ+Δϕ}{2} \end{matrix} \}

≈ \{ \begin{matrix}cos \frac{θ}{2} cos \frac{ϕ}{2} − sin \frac{θ}{2} cos \frac{ϕ}{2} \frac{Δθ}{2} − cos \frac{θ}{2} sin \frac{ϕ}{2} \frac{Δϕ}{2} \\ cos \frac{θ}{2} sin \frac{ϕ}{2} − sin \frac{θ}{2} sin \frac{ϕ}{2} \frac{Δθ}{2} + cos \frac{θ}{2} cos \frac{ϕ}{2} \frac{Δϕ}{2} \\ sin \frac{θ}{2} cos \frac{ϕ}{2} + cos \frac{θ}{2} cos \frac{ϕ}{2} \frac{Δθ}{2} − sin \frac{θ}{2} sin \frac{ϕ}{2} \frac{Δϕ}{2} \\ −sin \frac{θ}{2} sin \frac{ϕ}{2} − cos \frac{θ}{2} sin \frac{ϕ}{2} \frac{Δθ}{2} − sin \frac{θ}{2} cos \frac{ϕ}{2} \frac{Δϕ}{2} \end{matrix} \}

= \{ \begin{matrix}q_{0}− q_{2}v_{1}− q_{1}v_{2} \\ q_{1}+ q_{3}v_{1}+ q_{0}v_{2} \\ q_{2}+ q_{0}v_{1}+ q_{3}v_{2} \\ q_{3}− q_{1}v_{1}− q_{2}v_{2} \end{matrix} \}

(52)

−\frac{π}{2} ≦ θ ≦ \frac{π}{2}  (53)
−\frac{π}{2} ≦ ϕ ≦ \frac{π}{2}  (54)

sin \frac{θ}{2} = sign.(q_{2}) \sqrt{q_{2}^{2}+ q_{3}^{2}}  (55)
cos \frac{θ}{2} = \sqrt{q_{0}^{2}+ q_{1}^{2}}  (56)
sin \frac{ϕ}{2} = sign.(q_{1}) \sqrt{q_{1}^{2}+ q_{3}^{2}}  (57)
cos \frac{ϕ}{2} = \sqrt{q_{0}^{2}+ q_{2}^{2}}  (58)

0 = [ \begin{matrix}0 & −(a⃗+ g⃗)_{1} & −(a⃗+ g⃗)_{2} & −(a⃗+ g⃗)_{3} \\ (a⃗+ g⃗)_{1} & 0 & (a⃗− g⃗)_{3} & −(a⃗− g⃗)_{2} \\ (a⃗+ g⃗)_{2} & −(a⃗− g⃗)_{3} & 0 & (a⃗− g⃗)_{1} \\ (a⃗+ g⃗)_{3} & (a⃗− g⃗)_{2} & −(a⃗− g⃗)_{1} & 0 \end{matrix} ] [ \begin{matrix}q_{0}− q_{2}v_{1}− q_{1}v_{2} \\ q_{1}+ q_{3}v_{1}+ q_{0}v_{2} \\ q_{2}+ q_{0}v_{1}+ q_{3}v_{2} \\ q_{3}− q_{1}v_{1}− q_{2}v_{2} \end{matrix} ]_{k}  (59)
A = B[ \begin{matrix}v_{1} \\ v_{2} \end{matrix}]  (60)
A ≡ −[ \begin{matrix}−(a + g)_{1}q_{1}− (a + g)_{2}q_{2}− (a + g)_{3}q_{3} \\ (a + g)_{1}q_{0}− (a − g)_{2}q_{3}+ (a − g)_{3}q_{2} \\ (a − g)_{1}q_{3}+ (a + g)_{2}q_{0}− (a − g)_{3}q_{1} \\ −(a − g)_{1}q_{2}+ (a − g)_{2}q_{1}+ (a + g)_{3}q_{0} \end{matrix}  ]  (61)
B ≡ [ \begin{matrix}−(a + g)_{1}q_{3}− (a + g)_{2}q_{0}+ (a + g)_{3}q_{1} & −(a + g)_{1}q_{0}− (a + g)_{2}q_{3}+ (a + g)_{3}q_{2} \\ −(a + g)_{1}q_{2}+ (a − g)_{2}q_{1}+ (a − g)_{3}q_{0} & −(a + g)_{1}q_{1}+ (a − g)_{2}q_{2}+ (a − g)_{3}q_{3} \\ −(a − g)_{1}q_{1}− (a + g)_{2}q_{2}− (a − g)_{3}q_{3} & −(a − g)_{1}q_{2}− (a + g)_{2}q_{1}− (a − g)_{3}q_{0} \\ −(a − g)_{1}q_{0}+ (a − g)_{2}q_{3}− (a + g)_{3}q_{2} & −(a − g)_{1}q_{3}+ (a − g)_{2}q_{0}− (a + g)_{3}q_{1} \end{matrix}  ]  (62)
[ \begin{matrix}v_{1} \\ v_{2} \end{matrix}] = (B^{T}B)^{−1} B^{T}A  (63)

3.2 姿勢の運動方程式からの Initialize

4 キャリブレーションについて

4.1 センサの Alignment

\{ \begin{matrix}a_{11} = K_{1}u⃗_{1}· u⃗_{01} \\ a_{12} = K_{2}u⃗_{2}· u⃗_{01} \\ a_{13} = K_{3}u⃗_{3}· u⃗_{01} \end{matrix}  (64)
\{ \begin{matrix}a_{21} = K_{1}u⃗_{1}· u⃗_{02} \\ a_{22} = K_{2}u⃗_{2}· u⃗_{02} \\ a_{23} = K_{3}u⃗_{3}· u⃗_{02} \end{matrix}  (65)
\{ \begin{matrix}a_{31} = K_{1}u⃗_{1}· u⃗_{03} \\ a_{32} = K_{2}u⃗_{2}· u⃗_{03} \\ a_{33} = K_{3}u⃗_{3}· u⃗_{03} \end{matrix}  (66)
|u⃗_{1}| = |u⃗_{2}| = |u⃗_{3}| = 1  (67)
[ \begin{matrix}\frac{a_{11}}{K_{1}} & \frac{a_{12}}{K_{2}} & \frac{a_{13}}{K_{3}} \\ \frac{a_{21}}{K_{1}} & \frac{a_{22}}{K_{2}} & \frac{a_{23}}{K_{3}} \\ \frac{a_{31}}{K_{1}} & \frac{a_{32}}{K_{2}} & \frac{a_{33}}{K_{3}} \end{matrix} ] = [ \begin{matrix} & u_{01}^{T} \\  & u_{02}^{T} \\  & u_{03}^{T} \end{matrix}] [u⃗_{1} u⃗_{2} u⃗_{3}]  (68)
[u⃗_{1} u⃗_{2} u⃗_{3}] = [ \begin{matrix} & u_{01}^{T} \\  & u_{02}^{T} \\  & u_{03}^{T} \end{matrix}]^{−1} [ \begin{matrix}\frac{a_{11}}{K_{1}} & \frac{a_{12}}{K_{2}} & \frac{a_{13}}{K_{3}} \\ \frac{a_{21}}{K_{1}} & \frac{a_{22}}{K_{2}} & \frac{a_{23}}{K_{3}} \\ \frac{a_{31}}{K_{1}} & \frac{a_{32}}{K_{2}} & \frac{a_{33}}{K_{3}} \end{matrix} ]  (69)

4.2 センサのアライメント補正

Mode1 で使用する (u⃗_{01} = ( \begin{matrix}1 \\ 0 \\ 0 \end{matrix}) ) ことを考える

\{ \begin{matrix}v_{output1} = K_{1}v⃗· u⃗_{1} \\ v_{output2} = K_{2}v⃗· u⃗_{2} \\ v_{output3} = K_{3}v⃗· u⃗_{3} \end{matrix}  (70)

ただし、

v_{output}≡ v_{raw}− v_{zero}  (71)

整理して、

[ \frac{v_{output1}}{K_{1}} ; \frac{v_{output2}}{K_{2}} ; \frac{v_{output3}}{K_{3}} ] = [ \begin{matrix} & u_{1}^{T} \\  & u_{2}^{T} \\  & u_{3}^{T} \end{matrix}] v⃗  (72)

よって、

v⃗= [ \begin{matrix} & u_{1}^{T} \\  & u_{2}^{T} \\  & u_{3}^{T} \end{matrix}]^{−1} [ \frac{v_{output1}}{K_{1}} ; \frac{v_{output2}}{K_{2}} ; \frac{v_{output3}}{K_{3}} ]  (73)

5 分散について

5.1 Quarternion Error Model と Quarternion Covariance の関係

q̃+ Δq̃= \{ 1 ; u⃗\} \{ q_{0} ; q⃗\} = \{ \begin{matrix}q_{0}− u⃗· q⃗ \\ q⃗+ q_{0}u⃗+ u⃗× q⃗ \end{matrix} \}  (74)
∴ Δq̃ = \{ \begin{matrix}−u⃗· q⃗ \\ q_{0}u⃗+ u⃗× q⃗ \end{matrix} \} = [ \begin{matrix}−q_{1} & −q_{2} & −q_{3} \\ q_{0} & q_{3} & −q_{2} \\ −q_{3} & q_{0} & q_{1} \\ q_{2} & −q_{1} & q_{0} \end{matrix} ] [ \begin{matrix}u_{1} \\ u_{2} \\ u_{3} \end{matrix}]  (75)
σ^{2}(Δq̃) ≡ E [Δq̃(Δq̃)^{T}]

= E [[ \begin{matrix}−u⃗· q⃗ \\ q_{0}u⃗+ u⃗× q⃗ \end{matrix} ] [ \begin{matrix}−u⃗· q⃗ \\ q_{0}u⃗+ u⃗× q⃗ \end{matrix} ]^{T}]

= E [[ \begin{matrix}−u⃗· q⃗ \\ q_{0}u⃗+ u⃗× q⃗ \end{matrix} ] [−u⃗· q⃗ q_{0}u^{T}+ (u⃗× q⃗)^{T} ]]

= E [[ \begin{matrix}(u⃗· q⃗)^{2} & −(u⃗· q⃗)(q_{0}u^{T}+ (u⃗× q⃗)^{T}) \\ −(q_{0}u⃗+ u⃗× q⃗)(u⃗· q⃗) & q_{0}^{2}u⃗u^{T}+ q_{0}u⃗(u⃗× q⃗)^{T}+ (u⃗× q⃗)q_{0}u^{T}+ (u⃗× q⃗)(u⃗× q⃗)^{T} \end{matrix} ]]

= [ \begin{matrix}q^{T}E [u⃗u^{T}] q⃗ & −q_{0}q^{T}E [u⃗u^{T}]+q^{T}E [u⃗u^{T}]ΩT ; q⃗ \\ −q_{0}E [u⃗u^{T}] q⃗+ Ωq⃗E [u⃗u^{T}] q⃗ & q_{0}^{2}E [u⃗u^{T}]+ q_{0}E [u⃗u^{T}]ΩT ; q⃗+ q_{0}Ωq⃗E [u⃗u^{T}]+ Ωq⃗E [u⃗u^{T}]ΩT ; q⃗ \end{matrix} ]

(76)

ただし、

Ωq⃗ ≡ [ \begin{matrix}0 & q⃗_{3} & −q⃗_{2} \\ −q⃗_{3} & 0 & q⃗_{1} \\ q⃗_{2} & −q⃗_{1} & 0 \end{matrix} ]  (77)

対角要素だけ欲しい場合は、

\{ \begin{matrix}σ^{2}(Δq̃_{0}) ≡ E [(Δq_{0})^{2}] = q^{T}E [u⃗u^{T}] q⃗ \\ σ^{2}(Δq̃_{i}) ≡ E [(Δq_{i})^{2}] = \{(q_{0}I + Ωq⃗)E [u⃗u^{T}](q_{0}I + ΩT ; q⃗)\}_{i,i} (i = 1 ∼ 3) \end{matrix}  (78)

5.2 オイラー角との関係

5.2.1 位置

ϕ = arcsin{1 − 2((q_{e}^{n})_{0}^{2}+ (q_{e}^{n})_{3}^{2}) }  (79)
Δϕ = \frac{∂ϕ}{∂(q_{e}^{n})_{0}}Δ(q_{e}^{n})_{0}+ \frac{∂ϕ}{∂(q_{e}^{n})_{1}}Δ(q_{e}^{n})_{1}+ \frac{∂ϕ}{∂(q_{e}^{n})_{2}}Δ(q_{e}^{n})_{2}+ \frac{∂ϕ}{∂(q_{e}^{n})_{3}}Δ(q_{e}^{n})_{3}

= \frac{−4(q_{e}^{n})_{0}}{cosϕ} Δ(q_{e}^{n})_{0}+ \frac{−4(q_{e}^{n})_{3}}{cosϕ} Δ(q_{e}^{n})_{3}

(80)

x = siny  (81)
\frac{d}{dx}arcsinx = \frac{d}{dx}y = \frac{dy}{dx}\frac{d}{dy}y = ( \frac{dx}{dy}) ^{−1} = \frac{1}{cosy}  (82)
λ = arctan\frac{(q_{e}^{n})_{3}}{(q_{e}^{n})_{0}} − arctan\frac{(q_{e}^{n})_{1}}{(q_{e}^{n})_{2}}  (83)
Δλ = \frac{∂λ}{∂(q_{e}^{n})_{0}}Δ(q_{e}^{n})_{0}+ \frac{∂λ}{∂(q_{e}^{n})_{1}}Δ(q_{e}^{n})_{1}+ \frac{∂λ}{∂(q_{e}^{n})_{2}}Δ(q_{e}^{n})_{2}+ \frac{∂λ}{∂(q_{e}^{n})_{3}}Δ(q_{e}^{n})_{3}

= \frac{−(q_{e}^{n})_{3}}{(q_{e}^{n})_{0}^{2}+ (q_{e}^{n})_{3}^{2}} Δ(q_{e}^{n})_{0}− \frac{(q_{e}^{n})_{2}}{(q_{e}^{n})_{1}^{2}+ (q_{e}^{n})_{2}^{2}} Δ(q_{e}^{n})_{1}

− \frac{−(q_{e}^{n})_{1}}{(q_{e}^{n})_{1}^{2}+ (q_{e}^{n})_{2}^{2}} Δ(q_{e}^{n})_{2}+ \frac{(q_{e}^{n})_{0}}{(q_{e}^{n})_{0}^{2}+ (q_{e}^{n})_{3}^{2}} Δ(q_{e}^{n})_{3}

(84)

α = arctan\frac{(q_{e}^{n})_{3}}{(q_{e}^{n})_{0}} + arctan\frac{(q_{e}^{n})_{1}}{(q_{e}^{n})_{2}}  (85)
Δα ≡ 0

= \frac{∂α}{∂(q_{e}^{n})_{0}}Δ(q_{e}^{n})_{0}+ \frac{∂α}{∂(q_{e}^{n})_{1}}Δ(q_{e}^{n})_{1}+ \frac{∂α}{∂(q_{e}^{n})_{2}}Δ(q_{e}^{n})_{2}+ \frac{∂α}{∂(q_{e}^{n})_{3}}Δ(q_{e}^{n})_{3}

= \frac{−(q_{e}^{n})_{3}}{(q_{e}^{n})_{0}^{2}+ (q_{e}^{n})_{3}^{2}} Δ(q_{e}^{n})_{0}+ \frac{(q_{e}^{n})_{2}}{(q_{e}^{n})_{1}^{2}+ (q_{e}^{n})_{2}^{2}} Δ(q_{e}^{n})_{1}

+ \frac{−(q_{e}^{n})_{1}}{(q_{e}^{n})_{1}^{2}+ (q_{e}^{n})_{2}^{2}} Δ(q_{e}^{n})_{2}+ \frac{(q_{e}^{n})_{0}}{(q_{e}^{n})_{0}^{2}+ (q_{e}^{n})_{3}^{2}} Δ(q_{e}^{n})_{3}

(86)

また

1 ≡ ((q_{e}^{n})_{0}+ Δ(q_{e}^{n})_{0}) ^{2}+ ((q_{e}^{n})_{1}+ Δ(q_{e}^{n})_{1}) ^{2}+ ((q_{e}^{n})_{2}+ Δ(q_{e}^{n})_{2}) ^{2}+ ((q_{e}^{n})_{3}+ Δ(q_{e}^{n})_{3}) ^{2}  (87)

ゆえに

0 ≡ (q_{e}^{n})_{0}Δ(q_{e}^{n})_{0}+ (q_{e}^{n})_{1}Δ(q_{e}^{n})_{1}+ (q_{e}^{n})_{2}Δ(q_{e}^{n})_{2}+ (q_{e}^{n})_{3}Δ(q_{e}^{n})_{3}  (88)

以上まとめると

[ \begin{matrix}cosϕΔϕ \\ Δλ \\ 0 \\ 0 \end{matrix} ]

= [ \begin{matrix}−4(q_{e}^{n})_{0} & 0 & 0 & −4(q_{e}^{n})_{3} \\ −\frac{(q_{e}^{n})_{3}}{(q_{e}^{n})_{0}^{2}+(q_{e}^{n})_{3}^{2}} & −\frac{(q_{e}^{n})_{2}}{(q_{e}^{n})_{1}^{2}+(q_{e}^{n})_{2}^{2}} & \frac{(q_{e}^{n})_{1}}{(q_{e}^{n})_{1}^{2}+(q_{e}^{n})_{2}^{2}} & \frac{(q_{e}^{n})_{0}}{(q_{e}^{n})_{0}^{2}+(q_{e}^{n})_{3}^{2}} \\ −\frac{(q_{e}^{n})_{3}}{(q_{e}^{n})_{0}^{2}+(q_{e}^{n})_{3}^{2}} & \frac{(q_{e}^{n})_{2}}{(q_{e}^{n})_{1}^{2}+(q_{e}^{n})_{2}^{2}} & −\frac{(q_{e}^{n})_{1}}{(q_{e}^{n})_{1}^{2}+(q_{e}^{n})_{2}^{2}} & \frac{(q_{e}^{n})_{0}}{(q_{e}^{n})_{0}^{2}+(q_{e}^{n})_{3}^{2}} \\ (q_{e}^{n})_{0} & (q_{e}^{n})_{1} & (q_{e}^{n})_{2} & (q_{e}^{n})_{3} \end{matrix} ] [ \begin{matrix}Δ(q_{e}^{n})_{0} \\ Δ(q_{e}^{n})_{1} \\ Δ(q_{e}^{n})_{2} \\ Δ(q_{e}^{n})_{3} \end{matrix} ]

(89)

Δλ を求めるだけなら

λ + α = 2arctan\frac{(q_{e}^{n})_{3}}{(q_{e}^{n})_{0}}  (90)

であるから

Δλ = Δ(λ + α) (∵ Δα ≡ 0)

= Δ( 2arctan\frac{(q_{e}^{n})_{3}}{(q_{e}^{n})_{0}})

= 2( \frac{−(q_{e}^{n})_{3}}{(q_{e}^{n})_{0}^{2}+ (q_{e}^{n})_{3}^{2}} Δ(q_{e}^{n})_{0}+ \frac{(q_{e}^{n})_{0}}{(q_{e}^{n})_{0}^{2}+ (q_{e}^{n})_{3}^{2}} Δ(q_{e}^{n})_{3})

(91)

まとめると

[ \begin{matrix}cosϕΔϕ \\ ((q_{e}^{n})_{0}^{2}+ (q_{e}^{n})_{3}^{2}) Δλ \end{matrix} ]

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

(92)

整理して

\begin{aligned}[ & \begin{matrix}\frac{cosϕ}{−4} Δϕ \\ \frac{((q_{e}^{n})_{0}^{2}+(q_{e}^{n})_{3}^{2})}{−2} Δλ \end{matrix} & ] \\ & = [ \begin{matrix}(q_{e}^{n})_{0} & (q_{e}^{n})_{3} \\ (q_{e}^{n})_{3} & −(q_{e}^{n})_{0} \end{matrix} ] [ \begin{matrix}Δ(q_{e}^{n})_{0} \\ Δ(q_{e}^{n})_{3} \end{matrix}] \end{aligned}  (93)
[ \begin{matrix}Δ(q_{e}^{n})_{0} \\ Δ(q_{e}^{n})_{3} \end{matrix}] = \frac{1}{−(q_{e}^{n})_{0}^{2}− (q_{e}^{n})_{3}^{2}} [ \begin{matrix}(q_{e}^{n})_{0} & −(q_{e}^{n})_{3} \\ −(q_{e}^{n})_{3} & −(q_{e}^{n})_{0} \end{matrix} ] [ \begin{matrix}\frac{cosϕ}{−4} Δϕ \\ \frac{((q_{e}^{n})_{0}^{2}+(q_{e}^{n})_{3}^{2})}{−2} Δλ \end{matrix} ]  (94)

さらに

[ \begin{matrix}cosϕΔϕ \\ ((q_{e}^{n})_{0}^{2}+ (q_{e}^{n})_{3}^{2}) Δλ \end{matrix} ]

= [ \begin{matrix}−4(q_{e}^{n})_{0} & 0 & 0 & −4(q_{e}^{n})_{3} \\ −2(q_{e}^{n})_{3} & 0 & 0 & 2(q_{e}^{n})_{0} \end{matrix} ] [ \begin{matrix}−q_{1} & −q_{2} & −q_{3} \\ q_{0} & q_{3} & −q_{2} \\ −q_{3} & q_{0} & q_{1} \\ q_{2} & −q_{1} & q_{0} \end{matrix} ]_{q̃_{e}^{n}} [ \begin{matrix}Δu_{1} \\ Δu_{2} \\ Δu_{3} \end{matrix} ]

(95)

5.2.2 姿勢

Ψ = arctan\frac{2((q_{n}^{b})_{1}(q_{n}^{b})_{2}+ (q_{n}^{b})_{0}(q_{n}^{b})_{3})}{(q_{n}^{b})_{0}^{2}+ (q_{n}^{b})_{1}^{2}− (q_{n}^{b})_{2}^{2}− (q_{n}^{b})_{3}^{2}}

= arctan\frac{2((q_{n}^{b})_{1}(q_{n}^{b})_{2}+ (q_{n}^{b})_{0}(q_{n}^{b})_{3})}{1 − 2((q_{n}^{b})_{2}^{2}+ (q_{n}^{b})_{3}^{2})}

= arctan\frac{2((q_{n}^{b})_{1}(q_{n}^{b})_{2}+ (q_{n}^{b})_{0}(q_{n}^{b})_{3})}{2((q_{n}^{b})_{0}^{2}+ (q_{n}^{b})_{1}^{2}) − 1}

(96)

ΔΨ = \frac{∂Ψ}{∂(q_{n}^{b})_{0}}Δ(q_{n}^{b})_{0}+ \frac{∂Ψ}{∂(q_{n}^{b})_{1}}Δ(q_{n}^{b})_{1}+ \frac{∂Ψ}{∂(q_{n}^{b})_{2}}Δ(q_{n}^{b})_{2}+ \frac{∂Ψ}{∂(q_{n}^{b})_{3}}Δ(q_{n}^{b})_{3}

= ( \frac{1}{1 + tan^{2}Ψ} ) ( \frac{2(q_{n}^{b})_{3}}{denom._{Ψ}} Δ(q_{n}^{b})_{0}+ \frac{2(q_{n}^{b})_{2}}{denom._{Ψ}} Δ(q_{n}^{b})_{1}+ \frac{2(q_{n}^{b})_{1}}{denom._{Ψ}} Δ(q_{n}^{b})_{2}+ \frac{2(q_{n}^{b})_{0}}{denom._{Ψ}} Δ(q_{n}^{b})_{3})

(97)

denom._{Ψ} ≡ (q_{n}^{b})_{0}^{2}+ (q_{n}^{b})_{1}^{2}− (q_{n}^{b})_{2}^{2}− (q_{n}^{b})_{3}^{2}  (98)

y = arctanx  (99)
\frac{d}{dx}arctanx = \frac{d}{dx}y = ( \frac{dx}{dy}) ^{−1} = cos^{2}y = \frac{1}{1 + tan^{2}y} = \frac{1}{1 + x^{2}}  (100)

Θ = arcsin( 2( (q_{n}^{b})_{0}(q_{n}^{b})_{2}− (q_{n}^{b})_{1}(q_{n}^{b})_{3}) )  (101)

ΔΘ = \frac{∂Θ}{∂(q_{n}^{b})_{0}}Δ(q_{n}^{b})_{0}+ \frac{∂Θ}{∂(q_{n}^{b})_{1}}Δ(q_{n}^{b})_{1}+ \frac{∂Θ}{∂(q_{n}^{b})_{2}}Δ(q_{n}^{b})_{2}+ \frac{∂Θ}{∂(q_{n}^{b})_{3}}Δ(q_{n}^{b})_{3}

= \frac{2(q_{n}^{b})_{2}}{cosΘ}Δ(q_{n}^{b})_{0}+ \frac{−2(q_{n}^{b})_{3}}{cosΘ} Δ(q_{n}^{b})_{1}+ \frac{2(q_{n}^{b})_{0}}{cosΘ}Δ(q_{n}^{b})_{2}+ \frac{−2(q_{n}^{b})_{1}}{cosΘ} Δ(q_{n}^{b})_{3}

(102)

Φ = arctan\frac{2((q_{n}^{b})_{2}(q_{n}^{b})_{3}+ (q_{n}^{b})_{0}(q_{n}^{b})_{1})}{(q_{n}^{b})_{0}^{2}− (q_{n}^{b})_{1}^{2}− (q_{n}^{b})_{2}^{2}+ (q_{n}^{b})_{3}^{2}}  (103)

ΔΦ = \frac{∂Φ}{∂(q_{n}^{b})_{0}}Δ(q_{n}^{b})_{0}+ \frac{∂Φ}{∂(q_{n}^{b})_{1}}Δ(q_{n}^{b})_{1}+ \frac{∂Φ}{∂(q_{n}^{b})_{2}}Δ(q_{n}^{b})_{2}+ \frac{∂Φ}{∂(q_{n}^{b})_{3}}Δ(q_{n}^{b})_{3}

= ( \frac{1}{1 + tan^{2}Φ} ) ( \frac{2(q_{n}^{b})_{1}}{denom._{Φ}} Δ(q_{n}^{b})_{0}+ \frac{2(q_{n}^{b})_{0}}{denom._{Φ}} Δ(q_{n}^{b})_{1}+ \frac{2(q_{n}^{b})_{3}}{denom._{Φ}} Δ(q_{n}^{b})_{2}+ \frac{2(q_{n}^{b})_{2}}{denom._{Φ}} Δ(q_{n}^{b})_{3})

(104)

denom._{Φ} ≡ (q_{n}^{b})_{0}^{2}− (q_{n}^{b})_{1}^{2}− (q_{n}^{b})_{2}^{2}+ (q_{n}^{b})_{3}^{2}  (105)

また

1 ≡ ( (q_{n}^{b})_{0}+ Δ(q_{n}^{b})_{0}) ^{2}+ ( (q_{n}^{b})_{1}+ Δ(q_{n}^{b})_{1}) ^{2}+ ( (q_{n}^{b})_{2}+ Δ(q_{n}^{b})_{2}) ^{2}+ ( (q_{n}^{b})_{3}+ Δ(q_{n}^{b})_{3}) ^{2}  (106)

ゆえに

0 ≡ (q_{n}^{b})_{0}Δ(q_{n}^{b})_{0}+ (q_{n}^{b})_{1}Δ(q_{n}^{b})_{1}+ (q_{n}^{b})_{2}Δ(q_{n}^{b})_{2}+ (q_{n}^{b})_{3}Δ(q_{n}^{b})_{3}  (107)

以上まとめて \frac{1+tan^{2}Ψ}{denom._{Ψ}} ΔΨ cosΘΔΘ \frac{1+tan^{2}Φ}{denom._{Φ}} ΔΦ

0

= [ \begin{matrix}2(q_{n}^{b})_{3} & 2(q_{n}^{b})_{2} & 2(q_{n}^{b})_{1} & 2(q_{n}^{b})_{0} \\ 2(q_{n}^{b})_{2} & −2(q_{n}^{b})_{3} & 2(q_{n}^{b})_{0} & −2(q_{n}^{b})_{1} \\ 2(q_{n}^{b})_{1} & 2(q_{n}^{b})_{0} & 2(q_{n}^{b})_{3} & 2(q_{n}^{b})_{2} \\ (q_{n}^{b})_{0} & (q_{n}^{b})_{1} & (q_{n}^{b})_{2} & (q_{n}^{b})_{3} \end{matrix} ] [ \begin{matrix}Δ(q_{n}^{b})_{0} \\ Δ(q_{n}^{b})_{1} \\ Δ(q_{n}^{b})_{2} \\ Δ(q_{n}^{b})_{3} \end{matrix} ]

(108)

さらに

\frac{1+tan^{2}Ψ}{denom._{Ψ}} ΔΨ cosΘΔΘ \frac{1+tan^{2}Φ}{denom._{Φ}} ΔΦ

0

= [ \begin{matrix}2(q_{n}^{b})_{3} & 2(q_{n}^{b})_{2} & 2(q_{n}^{b})_{1} & 2(q_{n}^{b})_{0} \\ 2(q_{n}^{b})_{2} & −2(q_{n}^{b})_{3} & 2(q_{n}^{b})_{0} & −2(q_{n}^{b})_{1} \\ 2(q_{n}^{b})_{1} & 2(q_{n}^{b})_{0} & 2(q_{n}^{b})_{3} & 2(q_{n}^{b})_{2} \\ (q_{n}^{b})_{0} & (q_{n}^{b})_{1} & (q_{n}^{b})_{2} & (q_{n}^{b})_{3} \end{matrix} ] [ \begin{matrix}−q_{1} & −q_{2} & −q_{3} \\ q_{0} & q_{3} & −q_{2} \\ −q_{3} & q_{0} & q_{1} \\ q_{2} & −q_{1} & q_{0} \end{matrix} ]_{q̃_{n}^{b}} [ \begin{matrix}Δu_{1} \\ Δu_{2} \\ Δu_{3} \end{matrix} ]

(109)

6 ドリフト除去に関して

6.1 エラーモデルを仮定する場合

1 自由度モデルでのドリフト除去運動方程式

θ_{t+1} = θ_{t}+ (ω_{t}− ω_{0t})Δt

θ̂_{t+1} = θ̂_{t}+ (ω̂_{t} − ω̂_{0t})Δt

Δθ_{t+1} = Δθ_{t}+ (\underbrace{Δω}_{e_{1}}−Δω_{0t})Δt

バイアス変動

ω_{0t+1} = (1 − βΔt)ω_{0t}+ \underbrace{N\sqrt{Δt}u}_{e_{2}}

ω̂_{0t+1} = (1 − βΔt)ω̂_{0t}

Δω_{0t+1} = (1 − βΔt)Δω_{0}+ e_{2}

以上あわせて

[ \begin{matrix}Δθ \\ Δω_{0} \end{matrix} ]_{t+1} = [ \begin{matrix}1 & −Δt \\ 0 & 1 − βΔt \end{matrix} ] [ \begin{matrix}Δθ \\ Δω_{0} \end{matrix} ]_{t}+ [ \begin{matrix}Δt & 0 \\ 0 & 1 \end{matrix}] [ \begin{matrix}e_{1} \\ e_{2} \end{matrix}]

6.2 平均値推定

\underline{x}_{k+1} = A_{k}\underline{x}_{k}+ B_{k}\underline{w}  (110)
\underline{z}_{k} = H_{k}\underline{x}_{k}+ \underline{v}  (111)
E[\underline{w}] = \underline{μ} ≠ \underline{0}  (112)
V [\underline{w}] ≡ E[(\underline{w}− \underline{μ})(\underline{w}− \underline{μ})^{T}] = Q  (113)
E[\underline{v}] = 0  (114)
V [\underline{v}] ≡ E[(\underline{v}− 0)(\underline{v}− 0)^{T}] = R  (115)
E[\underline{w}_{ideal}] ≡ \underline{0}  (116)
V [\underline{w}_{ideal}] ≡ V [w] = Q  (117)
\underline{x}_{ideal k+1} = A_{k}\underline{x}_{k}+ B_{k}\underline{w}_{ideal}  (118)  \begin{aligned}\underline{x}_{k+1} & = A_{k}\underline{x}_{k}+ B_{k}\underline{w} \\ & ≡ A_{k}\underline{x}_{k}+ B_{k}\underline{μ}_{k}+ B_{k}\underline{w}_{ideal} \end{aligned}  (119)
P_{k+1} = A_{k}P_{k}A_{k}^{T}+ B_{k}QB_{k}^{T}  (120)  ^{P_{k+1} = A_{k}P_{k}A_{k}^{T}+ B_{k}QB_{k}^{T}  (121)}
ˆ\underline{x}_{ideal k+1} = \underline{x}_{ideal k+1} + K [\underline{z}_{k+1} − H\underline{x}_{ideal k+1}] (122)  \begin{aligned}ˆ\underline{x}_{k+1} & = \underline{x}_{k+1} + K [\underline{z}_{k+1} − H_{k+1} \underline{x}_{k+1}] \\ & = \underline{x}_{k+1} + K [\underline{z}_{k+1} − H_{k+1} \underline{x}_{ideal k}− H_{k+1}B_{k}\underline{μ}_{k}] \end{aligned} ; (123)
K_{k+1} = P_{k+1}H_{k+1}^{T} (H_{k+1}P_{k+1}H_{k+1}^{T} + R_{k+1})^{−1} (124)  ^{K_{k+1} = P_{k+1}H_{k+1}^{T} (H_{k+1}P_{k+1}H_{k+1}^{T} + R_{k+1})^{−1} (125)}
P̂_{k+1} = [I − K_{k+1}H_{k+1}]P_{k+1} (126) ^{P̂_{k+1} = [I − K_{k+1}H_{k+1}]P_{k+1} (127)}

以上より

E [z_{k+1} − H_{k+1}x_{k+1}]= E [z_{k+1} − H_{k+1}x_{ideal k+1} − H_{k+1}B_{k}\underline{μ}_{k}]

= E [z_{k+1} − H_{k+1}x_{ideal k+1}]− E [H_{k+1}B_{k}\underline{μ}_{k}] = 0 − E [H_{k+1}B_{k}\underline{μ}_{k}] = −E [H_{k+1}B_{k}\underline{μ}_{k}] = −(H_{k+1}B_{k})E [\underline{μ}_{k}] (128)

R≡ V [z_{k+1} − H_{k+1}x_{k+1}]

= V [z_{k+1} − H_{k+1}x_{ideal k+1} − H_{k+1}B_{k}\underline{μ}_{k}] = V [z_{k+1} − H_{k+1}x_{ideal k+1}]+ V [H_{k+1}B_{k}\underline{μ}_{k}] = V [z_{k+1} − H_{k+1}x_{ideal k+1}]

(129)

平均値除去について考えてみる

(H_{k+1}B_{k})^{T}E [z_{k+1} − H_{k+1}x_{k+1}]= −(H_{k+1}B_{k})^{T}(H_{k+1}B_{k})E [μ_{k}]  (130)

最小二乗法より

E [μ_{k}] = −( (H_{k+1}B_{k})^{T}(H_{k+1}B_{k}))^{−1} (H_{k+1}B_{k})E [z_{k+1} − H_{k+1}x_{k+1}]  (131)
μ_{k+1} = μ_{k}− E [μ_{k}]  (132)

7 オイラー角を用いた場合

運動方程式は以下のとおり。

\frac{dϕ}{dt} = −cosα(ω⃗_{n/e}^{n})_{Y}− sinα(ω⃗_{n/e}^{n})_{X}  (133)
\frac{d}{dt}Δϕ = sinα(ω⃗_{n/e}^{n})_{Y}Δα − cosαΔ(ω⃗_{n/e}^{n})_{Y}− cosα(ω⃗_{n/e}^{n})_{X}Δα − sinαΔ(ω⃗_{n/e}^{n})_{X}

= ( sinα(ω⃗_{n/e}^{n})_{Y}− cosα(ω⃗_{n/e}^{n})_{X}) Δα

+ \frac{cosα}{r_{e}+ h} Δ(ṙ_{e}^{n})_{X}− \frac{(ṙ_{e}^{n})_{X}cosα}{(r_{e}+ h)^{2}} Δh − \frac{sinα}{r_{e}+ h} Δ(ṙ_{e}^{n})_{Y}+ \frac{(ṙ_{e}^{n})_{Y}sinα}{(r_{e}+ h)^{2}} Δh

= ( sinα(ω⃗_{n/e}^{n})_{Y}− cosα(ω⃗_{n/e}^{n})_{X}) Δα

+ \frac{cosα}{r_{e}+ h} Δ(ṙ_{e}^{n})_{X}− \frac{sinα}{r_{e}+ h} Δ(ṙ_{e}^{n})_{Y}+ ( −\frac{(ṙ_{e}^{n})_{X}cosα}{(r_{e}+ h)^{2}} + \frac{(ṙ_{e}^{n})_{Y}sinα}{(r_{e}+ h)^{2}} ) Δh

(134)

\frac{dλ}{dt} = \frac{−sinα(ω⃗_{n/e}^{n})_{Y}+ cosα(ω⃗_{n/e}^{n})_{X}}{cosϕ}  (135)
\frac{d}{dt}Δλ = \frac{−cosα(ω⃗_{n/e}^{n})_{Y}Δα − sinαΔ(ω⃗_{n/e}^{n})_{Y}− sinα(ω⃗_{n/e}^{n})_{X}Δα + cosαΔ(ω⃗_{n/e}^{n})_{X}}{cosϕ}

+ \frac{−(λ̇ cosϕ)sinϕ}{cosϕ^{2}} Δϕ

= \frac{−cosα(ω⃗_{n/e}^{n})_{Y}Δα − sinαΔ(ω⃗_{n/e}^{n})_{Y}− sinα(ω⃗_{n/e}^{n})_{X}Δα + cosαΔ(ω⃗_{n/e}^{n})_{X}}{cosϕ}

− λ̇ tanϕΔϕ

= \frac{−cosα(ω⃗_{n/e}^{n})_{Y}− sinα(ω⃗_{n/e}^{n})_{X}}{cosϕ} Δα

− \frac{sinα}{cosϕ} ( −\frac{1}{r_{e}+ h} Δ(ṙ_{e}^{n})_{X}+ \frac{(ṙ_{e}^{n})_{X}}{(r_{e}+ h)^{2}}Δh)

+ \frac{cosα}{cosϕ} ( \frac{1}{r_{e}+ h} Δ(ṙ_{e}^{n})_{Y}− \frac{(ṙ_{e}^{n})_{Y}}{(r_{e}+ h)^{2}}Δh)

− λ̇ tanϕΔϕ

= \frac{−cosα(ω⃗_{n/e}^{n})_{Y}− sinα(ω⃗_{n/e}^{n})_{X}}{cosϕ} Δα

+ \frac{sinα}{(r_{e}+ h)cosϕ} Δ(ṙ_{e}^{n})_{X}+ \frac{cosα}{(r_{e}+ h)cosϕ} Δ(ṙ_{e}^{n})_{Y}

− \frac{sinα(ṙ_{e}^{n})_{X}+ cosα(ṙ_{e}^{n})_{Y}}{(r_{e}+ h)^{2}cosϕ} Δh

− λ̇ tanϕΔϕ

(136)

\frac{dα}{dt} = λ̇ sinϕ  (137)
\frac{d}{dt}Δα = sinϕΔλ̇ + λ̇ cosϕΔϕ  (138)

Δω⃗_{n/e}^{n} ≈ \frac{1}{r_{e}+ h} ( \begin{matrix}Δ(ṙ_{e}^{n})_{Y} \\ −Δ(ṙ_{e}^{n})_{X} \\ 0 \end{matrix} ) − \frac{1}{(r_{e}+ h)^{2}} ( \begin{matrix}(ṙ_{e}^{n})_{Y} \\ −(ṙ_{e}^{n})_{X} \\ 0 \end{matrix} ) Δh  (139)

Δρ_{X} ≡ Δ(ω⃗_{n/e}^{n})_{X} = \frac{1}{r_{e}+ h} Δ(ṙ_{e}^{n})_{Y}− \frac{(ṙ_{e}^{n})_{Y}}{(r_{e}+ h)^{2}}Δh  (140)

Δρ_{Y}≡ Δ(ω⃗_{n/e}^{n})_{Y}= −\frac{1}{r_{e}+ h} Δ(ṙ_{e}^{n})_{X}+ \frac{(ṙ_{e}^{n})_{X}}{(r_{e}+ h)^{2}}Δh  (141)
Δρ_{Z} ≡ Δ(ω⃗_{n/e}^{n})_{Z} = 0  (142)

DCM[ϕ,λ,α] = [ \begin{matrix}−cosα sinϕ cosλ − sinα sinλ & −cosα sinϕ sinλ + sinα cosλ & cosα cosϕ \\ sinα sinϕ cosλ − cosα sinλ & sinα sinϕ sinλ + cosα cosλ & −sinα cosϕ \\ −cosϕ cosλ & −cosϕ sinλ & −sinϕ \end{matrix} ] (143)

ΔDCM[ϕ,λ,α] = [ \begin{matrix}cαsϕsλΔλ − cαcϕcλΔϕ + sαsϕcλΔα − sαcλΔλ − cαsλΔα \\ −sαsϕsλΔλ + sαcϕcλΔϕ + cαsϕcλΔα − cαcλΔλ + sαsλΔα \\ cϕsλΔλ + sϕcλΔϕ \end{matrix}

−cαsϕcλΔλ − cαsϕsλΔϕ + sαsϕsλΔα − sαsλΔλ + cαcλΔα sαsϕcλΔλ + sαcϕsλΔϕ + cαsϕsλΔα − cαsλΔλ − sαcλΔα

−cϕcλΔλ + sϕsλΔϕ

\begin{matrix}−cαsϕΔϕ − sαcϕΔα \\ sαsϕΔϕ − cαcϕΔα \\ −cϕΔϕ \end{matrix} ]

= [ \begin{matrix}−cαcϕcλΔϕ + (cαsϕsλ − sαcλ) Δλ + (sαsϕcλ − cαsλ) Δα \\ sαcϕcλΔϕ − (sαsϕsλ + cαcλ) Δλ + (cαsϕcλ + sαsλ) Δα \\ sϕcλΔϕ + cϕsλΔλ \end{matrix}

−cαsϕsλΔϕ − (cαsϕcλ + sαsλ) Δλ + (sαsϕsλ + cαcλ) Δα sαcϕsλΔϕ + (sαsϕcλ − cαsλ) Δλ + (cαsϕsλ − sαcλ) Δα

sϕsλΔϕ − cϕcλΔλ

\begin{matrix}−cαsϕΔϕ − sαcϕΔα \\ sαsϕΔϕ − cαcϕΔα \\ −cϕΔϕ \end{matrix} ]

= [ \begin{matrix}−cαcϕcλ & −cαsϕsλ & −cαsϕ \\ sαcϕcλ & sαcϕsλ & sαsϕ \\ sϕcλ & sϕsλ & −cϕ \end{matrix} ] Δϕ

+ [ \begin{matrix}(cαsϕsλ − sαcλ) & (cαsϕcλ + sαsλ) & 0 \\ −(sαsϕsλ + cαcλ) & (sαsϕcλ − cαsλ) & 0 \\ cϕsλ & −cϕcλ & 0 \end{matrix}] Δλ

+ [ \begin{matrix}(sαsϕcλ − cαsλ) & (sαsϕsλ + cαcλ) & −sαcϕ \\ (cαsϕcλ + sαsλ) & (cαsϕsλ − sαcλ) & −cαcϕ \\ 0 & 0 & 0 \end{matrix} ] Δα

(144)

\begin{aligned}ω⃗_{e/i}^{n} & = DCM[ϕ,λ,α] ω⃗_{e/i}^{i} \\ & = DCM[ϕ,λ,α] ( \begin{matrix}0 \\ 0 \\ Ω_{e/i} \end{matrix} ) \end{aligned}  (145)

Δω⃗_{e/i}^{n} = ΔDCM[ϕ,λ,α] ( \begin{matrix}0 \\ 0 \\ Ω_{e/i} \end{matrix} )

= Ω_{e/i} ( \begin{matrix}−cαsϕΔϕ − sαcϕΔα \\ sαsϕΔϕ − cαcϕΔα \\ −cϕΔϕ \end{matrix} )

= Ωe/i[ \begin{matrix}−cαsϕ & 0 & −sαcϕ \\ sαsϕ & 0 & −cαcϕ \\ −cϕ & 0 & 0 \end{matrix} ] [ \begin{matrix}Δϕ \\ Δλ \\ Δα \end{matrix} ]

(146)

\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} \} − DCM[ϕ,λ,α]\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) \end{matrix} \} \end{aligned}  (147)

\frac{d}{dt}Δṙ_{e}^{n} = Rot[q̃_{n}^{b}^{∗}]Δa^{b}− 2Rot[q̃_{n}^{b}^{∗}]a^{b}× Δu_{n}^{b}+ Δg^{n}+ṙ_{e}^{n}× ( 2Δω⃗_{e/i}^{n} + Δω⃗_{n/e}^{n}) − ( 2ω⃗_{e/i}^{n} + ω⃗_{n/e}^{n}) × Δṙ_{e}^{n}

− ΔDCM[ϕ,λ,α]( ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) ) − DCM[ϕ,λ,α]Δ( ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) )

(148)

ṙ_{e}^{n}× 2Δω⃗_{e/i}^{n} = 2Ω_{e/i} [ \begin{matrix}0 & −(ṙ_{e}^{n})_{Z} & (ṙ_{e}^{n})_{X} \\ (ṙ_{e}^{n})_{Z} & 0 & −(ṙ_{e}^{n})_{X} \\ −(ṙ_{e}^{n})_{Y} & (ṙ_{e}^{n})_{X} & 0 \end{matrix} ] [ \begin{matrix}−cαsϕ & 0 & −sαcϕ \\ sαsϕ & 0 & −cαcϕ \\ −cϕ & 0 & 0 \end{matrix} ] [ \begin{matrix}Δϕ \\ Δλ \\ Δα \end{matrix} ]

= 2Ω_{e/i} [ \begin{matrix}−sαsϕ(ṙ_{e}^{n})_{Z}− cϕ(ṙ_{e}^{n})_{Y} & 0 & cαcϕ(ṙ_{e}^{n})_{Z} \\ −cαsϕ(ṙ_{e}^{n})_{Z}+ cϕ(ṙ_{e}^{n})_{X} & 0 & −sαcϕ(ṙ_{e}^{n})_{Z} \\ cαsϕ(ṙ_{e}^{n})_{Y}+ sαsϕ(ṙ_{e}^{n})_{X} & 0 & sαcϕ(ṙ_{e}^{n})_{Y}− cαcϕ(ṙ_{e}^{n})_{Y} \end{matrix} ] [ \begin{matrix}Δϕ \\ Δλ \\ Δα \end{matrix} ]

(149)

ṙ_{e}^{n}× Δω⃗_{n/e}^{n} = [ \begin{matrix}0 & −(ṙ_{e}^{n})_{Z} & (ṙ_{e}^{n})_{Y} \\ (ṙ_{e}^{n})_{Z} & 0 & −(ṙ_{e}^{n})_{X} \\ −(ṙ_{e}^{n})_{Y} & (ṙ_{e}^{n})_{X} & 0 \end{matrix} ] [ \begin{matrix}Δρ_{X} \\ Δρ_{Y} \\ Δρ_{Z} \end{matrix} ]

= [ \begin{matrix}−(ṙ_{e}^{n})_{Z}Δρ_{Y} \\ (ṙ_{e}^{n})_{Z}Δρ_{X} \\ −(ṙ_{e}^{n})_{Y}Δρ_{X}+ (ṙ_{e}^{n})_{X}Δρ_{Y} \end{matrix} ]

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

(150)

ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) = Ω_{e/i}^{2}(R_{normal}+ h)cosϕ ( \begin{matrix}−cosλ \\ −sinλ \\ 0 \end{matrix} )  (151)

Δ( ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) ) = Ω_{e/i}^{2}Δ( (R_{normal}+ h)cosϕ ( \begin{matrix}−cosλ \\ −sinλ \\ 0 \end{matrix} ) )

= Ω_{e/i}^{2}cosϕ ( \begin{matrix}−cosλ \\ −sinλ \\ 0 \end{matrix} ) Δh

− Ω_{e/i}^{2}(R_{normal}+ h)sinϕ ( \begin{matrix}−cosλ \\ −sinλ \\ 0 \end{matrix} ) Δϕ

+ Ω_{e/i}^{2}(R_{normal}+ h)cosϕ ( \begin{matrix}sinλ \\ −cosλ \\ 0 \end{matrix} ) Δλ

(152)

\frac{d}{dt}Δu_{n}^{b} = \frac{1}{2} \{Rot[q̃_{n}^{b}^{∗}]Δω⃗_{b/i}^{b} − Δω⃗_{e/i}^{n} − Δω⃗_{n/e}^{n} − 2(ω⃗_{e/i}^{n} + ω⃗_{n/e}^{n}) × Δu_{n}^{b}\}  (153)