2x1=10

because numbers are people, too
Persönliches
Fotografie
Programmierung
    • To tilt compensate, or not to tilt compensate

      When div­ing into the field of atti­tude detec­tion from sen­sor read­ings a cou­ple of answers seem omnipresent:

      • You need an accelerom­e­ter and a mag­ne­tome­ter.
      • You need to tilt com­pen­sate.
      • You need to low-pass fil­ter.
      • Or use the com­pli­men­ta­ry fil­ter.
      • Or bet­ter still, use the mys­ti­cal Kalman fil­ter (but it’s too com­pli­cat­ed to explain).

      Start­ing from the above and hav­ing an accelerom­e­ter and a mag­ne­tome­ter at hand, the old ques­tion a wise man once asked is:

      To tilt com­pen­sate, or not to tilt com­pen­sate, that is the ques­tion—
      Whether ’tis Nobler in the mind to suf­fer
      The Slings and Arrows of out­ra­geous Stack­Over­flow answers,
      Or to take Arms against a Sea of trou­bles,
      And by using the atti­tude matrix, end them? To tilt: to roll.

      In oth­er words: Is tilt com­pen­sa­tion required?

      The Tilt Compensation

      To make a long sto­ry short, the com­mon way to cal­cu­late yaw angle \(\psi\) (read: psi) by tilt-com­pen­sat­ing a 3-axis mag­ne­tome­ter using pitch angle \(\theta\) (read: theta) and roll angle \(\phi\) (read: phi) is by re-map­ping the sen­sor read­ings to the ref­er­ence X-Y plane by cal­cu­lat­ing

      \(
      \begin{align}
      \begin{bmatrix}
      \hat{m}_x \\ \hat{m}_y
      \end{bmatrix} =
      \begin{bmatrix}
      cos(\theta) && sin(\phi) \cdot sin(\theta) && cos(\phi) \cdot sin(\theta) \\
      0 && cos(\phi) && -sin(\theta)
      \end{bmatrix} \cdot
      \begin{bmatrix}
      m_x \\ m_y \\ m_z
      \end{bmatrix}
      \end{align}
      \)

      In oth­er words:

      \(
      \begin{align}
      \hat{m}_x &= m_x \cdot cos(\theta) + m_y \cdot sin(\phi) sin(\theta) + m_z \cdot cos(\phi) sin(\theta) \\
      \hat{m}_y &= m_y \cdot cos(\phi) - m_z \cdot sin(\phi)
      \end{align}
      \)

      And then tak­ing the arc­t­an­gent to get the yaw angle:

      \(
      \begin{align}
      \psi &= tan^{-1}(-\frac{\hat{m}_y}{\hat{m}_x}) \\
      &= atan2(-\hat{m}_y, \hat{m}_x)
      \end{align}
      \)

      Obvi­ous­ly, \(\theta\) and \(\phi\) would be need­ed before doing so, and they may be obtained from the accelerom­e­ter — assum­ing aero­nau­tic ref­er­ence sys­tem, i.e. Tait-Bryan XYZ — by cal­cu­lat­ing

      \(
      \begin{align}
      \phi &= tan^{-1}(\frac{a_y}{a_z}) \\
      \theta &= tan^{-1}(-\frac{a_x}{\sqrt{a_x^2 + a_z^2}})
      \end{align}
      \)

      Since the denom­i­na­tor in the \(\theta\) cal­cu­la­tion is nev­er neg­a­tive, the angle is con­strained to a range of \(\pm 90^\circ\) , where­as \(\phi\) and \(\psi\) may con­tain val­ues in the range of \(\pm 180^\circ\) .

      The ques­tion is, since we do have two axes point­ing down and some­what north or for­ward — just don’t be fooled to assume that a mag­ne­tome­ter will mag­i­cal­ly do that, or you will be bad­ly sur­prised by the read­ings it’ll give you — so, since we have these axes, why the need to tilt com­pen­sate?

      A good Attitude

      The short answer is: We don’t need to, and the solu­tion smells a lot like the TRIAD algo­rithm. (That might be due to the fact that it is designed to sort of do what we want.)

      The solu­tion takes advan­tage out of two facts:

      • The cross prod­uct of two vec­tors is a vec­tor orthog­o­nal to them (in layman’s terms: It is in a \(90^\circ\) angle to both of them)
      • The dot prod­uct of two vec­tors is the cosine of the angle between them.

      Assum­ing the accelerom­e­ter mea­sures pos­i­tive up, i.e. gives a neg­a­tive read­ing for the grav­i­ty when the top side is actu­al­ly on the top, and that both sen­sors are some­what cal­i­brat­ed to not total­ly bork up our cal­cu­la­tions, then here’s the gen­er­al pro­ce­dure:

      First, we invert the accelerom­e­ter vec­tor so that it indeed points up. After nor­mal­iza­tion, it then forms our new local \(\vec{z}\) axis:

      \(
      \begin{align}
      \vec{z} = -\frac{\vec{a}}{|\vec{a}|} = \frac{-\vec{a}}{\sqrt{a_x^2 + a_y^2 + a_z^2}}
      \end{align}
      \)

      We then cross \(\vec{z}\) with the mag­ne­tome­ter vec­tor \(\vec{m}\) and nor­mal­ize in order to get our local \(\vec{y}\) axis:

      \(
      \begin{align}
      \vec{y} = \frac{\vec{z} \times \vec{m}}{|\vec{z} \times \vec{m}|}
      \end{align}
      \)

      Since \(\vec{y}\) is orthog­o­nal to \(\vec{m}\) and \(\vec{z}\) , the actu­al pitch of the mag­ne­tome­ter vec­tor does not mat­ter as long as \(\vec{m}\) is not collinear with \(\vec{z}\) . In that case you are stand­ing either on the north or the south pole and there real­ly is no defined north. In addi­tion, you are prob­a­bly freez­ing and have oth­er prob­lems.

      Since \(\vec{y}\) and \(\vec{z}\) are now well defined (that is, ortho­nor­mal and point­ing in the gen­er­al direc­tion of suc­cess), we cross them again to get an orthog­o­nal \(\vec{x}\) axis:

      \(
      \begin{align}
      \vec{x} = \vec{y} \times \vec{z}
      \end{align}
      \)

      Renor­mal­iza­tion is sug­gest­ed for prac­ti­cal rea­sons, but not required math­e­mat­i­cal­ly.

      We now need to define a set of ref­er­ence vec­tors, say \(\vec{X}\) , \(\vec{Y}\) and \(\vec{Z}\) in upper case nota­tion. To make life easy, we’ll sim­ply assume them to be unit vec­tors, that is

      \(
      \begin{align}
      \vec{X} =
      \begin{bmatrix}
      1 \\ 0 \\ 0
      \end{bmatrix} \quad
      \vec{Y} =
      \begin{bmatrix}
      0 \\ 1 \\ 0
      \end{bmatrix} \quad
      \vec{Z} =
      \begin{bmatrix}
      0 \\ 0 \\ 1
      \end{bmatrix} \\
      \end{align}
      \)

      Once this is done, we can build the atti­tude (or direc­tion cosine) matrix \(\underline{A}\) from the dot prod­ucts of the vec­tors (remem­ber that the dot prod­uct of two vec­tors is the cosine of the angle between them), as fol­lows:

      \(
      \begin{align}
      \underline{A} &= \begin{bmatrix}
      \vec{x} \cdot \vec{X} && \vec{y} \cdot \vec{X} && \vec{z} \cdot \vec{X} \\
      \vec{x} \cdot \vec{Y} && \vec{y} \cdot \vec{Y} && \vec{z} \cdot \vec{Y} \\
      \vec{x} \cdot \vec{Z} && \vec{y} \cdot \vec{Z} && \vec{z} \cdot \vec{Z}
      \end{bmatrix}
      \end{align}
      \)

      TRIAD for­mu­lates this as

      \(
      \begin{align}
      \underline{A} &=
      \begin{bmatrix}
      \vec{x} \; \vdots \; \vec{y} \; \vdots \; \vec{z}\end{bmatrix} \cdot
      \begin{bmatrix}
      \vec{X} \; \vdots \; \vec{Y} \; \vdots \; \vec{Z}\end{bmatrix} \\
      &=
      \begin{bmatrix}
      x_x && y_x && z_x \\
      x_y && y_y && z_y \\
      x_z && y_z && z_z
      \end{bmatrix} \cdot
      \begin{bmatrix}
      X_x && Y_x && Z_x \\
      X_y && Y_y && Z_y \\
      X_z && Y_z && Z_z
      \end{bmatrix}
      \end{align}
      \)

      Now since we just defined \(\vec{X}\) , \(\vec{Y}\) and \(\vec{Z}\) to be unit vec­tors, we have

      \(
      \begin{align}
      \underline{A} &=
      \begin{bmatrix}
      x_x && y_x && z_x \\
      x_y && y_y && z_y \\
      x_z && y_z && z_z
      \end{bmatrix} \cdot
      \begin{bmatrix}
      1 \; && 0 \; && 0 \\
      0 \; && 1 \; && 0 \\
      0 \; && 0 \; && 1
      \end{bmatrix} = \begin{bmatrix}
      x_x && y_x && z_x \\
      x_y && y_y && z_y \\
      x_z && y_z && z_z
      \end{bmatrix}
      \end{align}
      \)

      It can’t get more con­ve­nient than that. No mat­ter how we get there, this matrix \(\underline{A}\) con­tains all angu­lar rela­tion­ships between all axes, so it is just a mat­ter of extract­ing them. Sim­i­lar to the method described in the tilt com­pen­sa­tion algo­rithm, we can get our angles from the matrix as fol­lows:

      \(
      \begin{align}
      \theta &= -sin^{-1}(\underline{A}_{1, 3}) \\
      \phi &= tan^{-1}(\underline{A}_{2, 3}, \underline{A}_{3, 3}) = atan2(\underline{A}_{2, 3}, \underline{A}_{3, 3}) \\
      \psi &= tan^{-1}(\underline{A}_{1, 2}, \underline{A}_{1, 1}) = atan2(\underline{A}_{1, 2}, \underline{A}_{1, 1})
      \end{align}
      \)

      This gives us

      \(
      \begin{align}
      \theta &= -sin^{-1}(\vec{z} \cdot \vec{X}) \\
      \phi &= atan2(\vec{z} \cdot \vec{Y}, \vec{z} \cdot \vec{Z}) \\
      \psi &= atan2(\vec{y} \cdot \vec{X}, \vec{x} \cdot \vec{X})
      \end{align}
      \)

      And, again, since our ref­er­ence axes are unit vec­tors, that reduces to:

      \(
      \begin{align}
      \theta &= -sin^{-1}(z_x) \\
      \phi &= atan2(z_y, z_z) \\
      \psi &= atan2(y_x, x_x)
      \end{align}
      \)

      Again, because of the (arc)sine in the \(\theta\) cal­cu­la­tion, pitch angle can­not exceed \(\pm 90^\circ\) .
      So, did we tilt com­pen­sate? Sort of, by encod­ing roll and pitch infor­ma­tion in \(\vec{y}\) and \(\vec{x}\) (mind you, we hat to cal­cu­late these). But then again we didn’t. In a way.

      Final Words

      One inter­est­ing (read: nasty) behav­iour is that head­ing and roll angle seem to jump about \(180^\circ\) when the device’s actu­al pitch angle exceeds \(\pm 90^\circ\) , that is, rolls over. In this case, the cal­cu­lat­ed pitch angle will go down or up again, respec­tive­ly, since it’s, by def­i­n­i­tion, con­strained to a \(\pm 90^\circ\) range. The head­ing and roll angles, on the oth­er hand, will be flipped by \(180^\circ\) , since the device is now — essen­tial­ly — in a reverse posi­tion. If you nev­er rotate a device con­ti­nous­ly for \(180^\circ\) and watch the the out­put, this will not be a prob­lem. But if you do, say, if you intend to use your derived angles to update a Kalman fil­ter, then sim­ply take care to han­dle this case in order to not mess up it’s state esti­mate with seem­ing­ly bogus read­ings.

      Januar 24th, 2014 GMT +1 von
      Markus
      2014-01-24T03:07:37+01:00 2014-02-15T15:02:37+01:00 · 0 Kommentare
      Sensor Fusion Accelerometer Tilt Compensation Magnetometer TRIAD
      Signal Processing

      Hinterlasse einen Kommentar

      Hier klicken, um das Antworten abzubrechen.

    1. « newer
    2. 1
    3. …
    4. 28
    5. 29
    6. 30
    7. 31
    8. 32
    9. 33
    10. 34
    11. …
    12. 43
    13. older »
    • Kategorien

      • .NET
        • ASP.NET
        • Core
        • DNX
      • Allgemein
      • Android
      • Data Science
      • Embedded
      • FPGA
      • Humor
      • Image Processing
      • Kalman Filter
      • Machine Learning
        • Caffe
        • Hidden Markov Models
        • ML Summarized
        • Neural Networks
        • TensorFlow
      • Mapping
      • MATLAB
      • Robotik
      • Rust
      • Signal Processing
      • Tutorial
      • Version Control
    • Neueste Beiträge

      • Summarized: The E-Dimension — Why Machine Learning Doesn’t Work Well for Some Problems?
      • Use your conda environment in Jupyter Notebooks
      • Building OpenCV for Anaconda Python 3
      • Using TensorFlow’s Supervisor with TensorBoard summary groups
      • Getting an image into and out of TensorFlow
    • Kategorien

      .NET Allgemein Android ASP.NET Caffe Core Data Science DNX Embedded FPGA Hidden Markov Models Humor Image Processing Kalman Filter Machine Learning Mapping MATLAB ML Summarized Neural Networks Robotik Rust Signal Processing TensorFlow Tutorial Version Control
    • Tags

      .NET Accelerometer Anaconda Bitmap Bug Canvas CLR docker FPGA FRDM-KL25Z FRDM-KL26Z Freescale git Gyroscope Integration Drift Intent J-Link Linear Programming Linux Magnetometer Matlab Mono Naismith OpenCV Open Intents OpenSDA Optimization Pipistrello Player/Stage PWM Python Sensor Fusion Simulink Spartan 6 svn tensorflow Tilt Compensation TRIAD ubuntu Windows Xilinx Xilinx SDK ZedBoard ZYBO Zynq
    • Letzte Kommetare

      • Lecke Mio bei Frequency-variable PWM generator in Simulink
      • Vaibhav bei Use your conda environment in Jupyter Notebooks
      • newbee bei Frequency-variable PWM generator in Simulink
      • Markus bei Using TensorFlow’s Supervisor with TensorBoard summary groups
      • Toke bei Using TensorFlow’s Supervisor with TensorBoard summary groups
    • Blog durchsuchen

    • Januar 2014
      M D M D F S S
      « Dez   Feb »
       12345
      6789101112
      13141516171819
      20212223242526
      2728293031  
    • Self

      • Find me on GitHub
      • Google+
      • Me on Stack­Ex­change
      • Ye olde blog
    • Meta

      • Anmelden
      • Beitrags-Feed (RSS)
      • Kommentare als RSS
      • WordPress.org
    (Generiert in 0,540 Sekunden)

    Zurück nach oben.