title: Getting a trajectory from accelerometer and gyroscope (IMU)
body: <p>I am well aware of the existence of <a href="https://stackoverflow.com/questions/8264518/using-accelerometer-gyroscope-and-compass-to-calculate-devices-movement-in-3d">this question</a> but mine will differ. I also know that there could be significant errors with this approach but I want to understand the configuration also theoretically.</p>
  
  <p>I have some basic questions which I find hard to answer for myself clearly. There is a lot of information about accelerometers and gyroscopes but I still haven't found an explanation "from first principles" of some basic properties.</p>
  
  <p>So I have a plate sensor that contains an accelerometer and gyroscope. There is also a magnetometer which I skip for now. </p>
  
  <ul>
  <li>The accelerometer gives information in each time t about the temporary acceleration vector a = (ax, ay, az) in m/s^2 according to the fixed coordinate system to the sensor.</li>
  <li>The gyroscope gives a 3D vector in deg/s which says the temporary speed of rotation of the three axes (Ox, Oy and Oz). From this information, one can get a rotation matrix that corresponds to an infinitesimal rotation of the coordinate system (according to the previous moment). <a href="https://stackoverflow.com/questions/7779236/finding-a-quaternion-from-gyroscope-data">Here</a> is some explanation how to obtain a quaternion, that represents R.</li>
  </ul>
  
  <p>So we know that the infinitesimal movement can be calculated considering that the acceleration is the second derivative of the position.</p>
  
  <p>Imagine that your sensor is attached to your hand or leg. In the first moment we can consider its point in 3D space as (0,0,0) and the initial coordinate system also attached in this physical point. So for the very first time step we will have </p>
  
  <p><strong>r</strong>(1) = 0.5<strong>a</strong>(0)dt^2</p>
  
  <p>where <strong>r</strong> is the infinitesimal movement vector, <strong>a</strong>(0) is the acceleration vector.</p>
  
  <p>In each of the following steps we will use the calculations</p>
  
  <p><strong>r</strong>(t+1) = 0.5<strong>a</strong>(t)dt^2 + <strong>v</strong>(t)dt + <strong>r</strong>(t)</p>
  
  <p>where <strong>v</strong>(t) is the speed vector which will be estimated in some way, for example as (<strong>r</strong>(t)-<strong>r</strong>(t-1)) / dt.</p>
  
  <p>Also, after each infinitesimal movement we will have to take into account the data from the gyroscope. We will use the rotation matrix to rotate the vector <strong>r</strong>(t+1).</p>
  
  <p>In this way, probably with tremendous error I will get some trajectory according to the initial coordinate system. </p>
  
  <p>My queries are:</p>
  
  <ol>
  <li>Am I principally correct with this algorithm? If not, where am I wrong?</li>
  <li>I would very much appreciate some resources with a working example where the first principles are not skipped.</li>
  <li>How should I proceed with using the Kalman's filter to obtain a better trajectory? In what way exactly do I pass all the IMU data (accelerometer, gyroscope and magnetometer) to the Kalman filter?</li>
  </ol>