Integrated Navigation System Design for Micro Planetary Rovers: Comparison of Absolute Heading Estimation Algorithms and Nonlinear Filtering

View full-text article in PMC

. 2016 May 23;16(5):749. doi: 10.3390/s16050749

Algorithm 2. Optimization based absolute attitude estimation algorithm (q-Method)

Concatenate observation vectors:

$\begin{array}{l} W = [S_{S} g_{S}] ∴ m e a s u r e m e n t v e c t o r s i n S- f r a m e \\ V = [S_{F} g_{F}] ∴ p r e d i t c t i o n v e c t o r s i n F- f r a m e \end{array}$
Calculate: B = WV^T; Q = B + B^T
Extract Z from B matrix: Z = [B₂₃ − B₃₂B₃₁ − B₁₃B₁₂ − B₂₁]
Form 4 × 4 matrix K: $K_{4 \times 4} = [\begin{matrix} Q - I σ & Z^{T} \\ Z & σ \end{matrix}] ∴ σ = t r (B), I = I_{3 \times 3}$
Find Eigen data of K. The Eigenvector corresponding to the largest Eigen value of K is q vector:

$q_{_{4 \times 1}} = [\begin{matrix} q_{v} \\ q_{s} \end{matrix}]; 4 \times 1 unit quaternion representing best fit rotation from F to S .$
Desired rotation matrix can be found from quaternion vector:

$\begin{array}{l} C_{F}^{S} = (q_{s}^{2} - q_{v}^{T} q_{v}) I + 2 q_{v} q_{v}^{T} - 2 q_{s} q_{v}^{\times} ∴ q_{v}^{\times} = [\begin{matrix} 0 & - q_{v_{3}} & q_{v_{2}} \\ q_{v_{3}} & 0 & - q_{v_{1}} \\ - q_{v_{2}} & q_{v_{1}} & 0 \end{matrix}] \\ q_{v}^{\times} : s k e w - s y m m e t r i c m a t r i x f o r m o f q_{v} . \end{array}$
Attitude angles extraction:

$\begin{array}{l} C_{S}^{T} = C_{F}^{T} C_{S}^{F}, a transformation matrix (function of RPY) from b-frame to n-frame . \\ ∴ C_{F}^{T} i s f u n c t i o n o f l o n g i t u d e, l a t i t u d e (r o v e r p o s i t i o n) . \end{array}$
Hence roll, pitch and heading angles can be derived from matrix ( $C_{S}^{T}$ ) as:

$\begin{array}{l} ϕ = a \sin (C_{S}^{T}_{32}, C_{S}^{T}_{33}) \\ θ = a \sin (C_{S}^{T}_{31}) \\ ψ = a \tan 2 (C_{S}^{T}_{21}, C_{S}^{T}_{11}) \end{array}$