I am well aware of the existence of this question 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.
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.
So I have a plate sensor that contains an accelerometer and gyroscope. There is also a magnetometer which I skip for now.
- 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.
- 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). Here is some explanation how to obtain a quaternion, that represents R.
So we know that the infinitesimal movement can be calculated considering that the acceleration is the second derivative of the position.
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
r(1) = 0.5a(0)dt^2
where r is the infinitesimal movement vector, a(0) is the acceleration vector.
In each of the following steps we will use the calculations
r(t+1) = 0.5a(t)dt^2 + v(t)dt + r(t)
where v(t) is the speed vector which will be estimated in some way, for example as (r(t)-r(t-1)) / dt.
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 r(t+1).
In this way, probably with tremendous error I will get some trajectory according to the initial coordinate system.
My queries are:
- Am I principally correct with this algorithm? If not, where am I wrong?
- I would very much appreciate some resources with a working example where the first principles are not skipped.
- 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?
Your conceptual framework is correct, but the equations need some work. The acceleration is measured in the platform frame, which can rotate very quickly, so it is not advisable to integrate acceleration in the platform frame and rotate the position change. Rather, the accelerations are transformed into a relatively slowly rotating frame and the integration to velocity change and position change is done there. Typically a locally-level frame (e.g. North-East-Down or Wander Aziumuth) or an Earth-centered frame (ECEF or ECI). Gravity and Coriolis force must be included in the acceleration.
Derivations from first principles can be found in many references, one of my favorites is Strapdown Inertial Navigation Technology by Titterton and Weston. Derivations of the inertial navigation equations in locally-level and Earth-fixed frames are given in Chapter 3.
As you've recognized in your question - the initial velocity is an unknown constant of integration. Without some estimate of initial velocity the trajectory resulting from integrating the inertial data can be wildly wrong.
来源：https://stackoverflow.com/questions/42176603/getting-a-trajectory-from-accelerometer-and-gyroscope-imu