0
$\begingroup$

I am currently trying to (re)create an AHRS (Attitude Heading Reference System) for mobile phones using the following whitepaper: PDF

I'm not well-versed in Matrix & Vector math, and find myself getting stuck at 4.1 (The Process Model (aka the state & transition equations)) on Page 5 (having already completed the ASGD & EHC).

In particular, I'm vexed by the fact that they're using a Matrix(/Vector) INSIDE of another Matrix (eq 11-13.)

My Questions:

  • How would one go about multiplying/adding/etc matrices if one of the positions inside of the matrix contains another matrix (or vector)? (see eq. 11)
  • Does the $\mathsf x$ in eq 13 & eq 19 imply a skew symmetric matrix? (if so, how would that matrix look for eq 19 $ [e_k\mathsf x] ?) $
  • Could someone run me through an example using these equations? (preferably visually, so I can see how the Matrices change)

Relevant Formulas:

Definitions

q : Quaternion rotation ($q_0=w,q_1=x, etc.$)

$\omega$ : Gyro Input (angular velocity)

$T_s$ : Sampling-Time (e.g. 20 ms)

State Equations

11: $\dot q = \Omega[\omega] q $

12: $ \Omega[\omega] = \frac{1}{2} \left[\begin{matrix} 0 & -\omega^T \\ \omega & [\omega \mathsf x] \end{matrix}\right] $

where

13: $ [\omega\mathsf x] = \left[ \begin{matrix} 0 & \omega_z & -\omega_y \\ -\omega_z & 0 & \omega_x \\ \omega_y & -\omega_x & 0 \end{matrix}\right] $

and $ \omega = [\begin{matrix} \omega_x & \omega_y & \omega_z \end{matrix}]^T $

State Transition

18: $ q_{k+1} = \Phi(\Omega_k,T_s)q_k + {}^qw_k $ $ = \left[I_{3\mathsf x \mathrm 3}(1- \frac {\Delta \theta^2} {8} + \frac{1}{2}\Omega_kT_s\right]q_k + {}^qw_k $

where $ \Delta\theta^2 = (\omega_xT_s)^2 + (\omega_yT_s)^2 + (\omega_zT_s)^2$

19: $ {}^qw_k = -\frac{T_s}{2}\left[ \begin{matrix}[e_k \mathsf x] + q_0I_{3x3} \\ -e^T_k \end{matrix} \right] {}^gw $

and $ e_k = [\begin{matrix}q_{1,k} & q_{2,k} & q_{3,k}\end{matrix}]^T$

which yields

Noise Covariance Matrix

$ Q_k = (\frac{T_s}{2})^2 \Xi_k\sigma^2_gI\Xi_k^T $

$\endgroup$
  • $\begingroup$ After using Matlab to try out some stuff, I'm now assuming that eq 12 would be a 4x4 matrix (as the matrix just expands)? $\endgroup$ – Frank v Hoof Nov 30 '17 at 16:44
  • $\begingroup$ The matrix in Eq 12 is called a partitioned matrix. $\endgroup$ – frank Dec 1 '17 at 15:38

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.