Measuring Displacement Using Accelerometers: Part 1-Calculating Gravity Vectors

So for my final year project I had to convert the signals from an accelerometer to displacements for a movement of a few millimetres and I decided to write about my experiences(mainly problems) doing this. I can’t say exactly what the device was as it was built for a research group but I’m going to go through the theory involved with getting displacements from an accelerometer. I’ll be making a few posts to cover the different topics. If You came here from the YouTube video and want to see the code you can jump to the third post. The movement I was measuring involved the sensor moving forward/backwards and upward/downward as well as the sensor tilting forward and backwards. The sensor used was an MPU-9150 which has 9 degrees of freedom(DOF), only the accelerometer and gyroscope were used in this application(3DOF each), a diagram of the axes can be seen below.mpu axes So because accelerometers measure acceleration due to gravity this would have to be canceled to isolate the acceleration caused by the movement, this wouldn’t be too difficult if all 3 axes from the accelerometer and gyro were being recorded, using some 3d vector and trig calculations this could be done easily enough but a problem I faced was having a fixed sample rate and other sensors which had to be recorded alongside the ACC and gyro, this meant that only the necessary axes from these two could be recorded. The Y and Z axes of the ACC were used to measure upward/downward and forward/backwards accelerations of the sensor and the X axis of the gyro were recorded the measure the forward/backwards tilt of the sensor. Because of the limited information to calculate the orientation of the sensor, I had to find a few workarounds to isolate the movement accelerations accurately.
So I came up with three ways I calculate the gravity vectors and tested them against each other, the first method work under the assumption that the angle change on this application would be around 5 degrees or less that the angle change wouldn’t have a large effect on the gravity vectors and if the average of the initial gravity vector was taken for each axis, this could be subtracted from the acceleration signals containing the movement resulting in the movement acceleration remaining. If this method worked, the gyro wouldn’t have to be implemented but as you can guess from me mentioning using the gyro earlier, this method didn’t work but I’ll explain why below. The formula for this can also be seen below.
The symbol a(n) represents the acceleration at sample n and i represents the average window(from sample 0 to sample i will be used to find the average offset due to gravity i.e. i=100 the average of the first 100 samples will be taken to find the initial offset). Because of the noisy output of the accelerometer, it was low-pass filtered before the average was found. This method sees gravity as a DC offset which once found, can be subtracted then we’ll be left with acceleration from the movement, but this wasn’t true in testing. Even though the angle change was small, the change in gravity vectors was big enough compared to the movement acceleration to give inaccurate results, this can be seen in the graph below.5_degree_angle.jpg The above shows the acceleration measured on the Z axis from a 5 degree angle change(blue) and the calculated gravity offset(orange) and as you can see, even this small angle change causes a noticeable gap between the two and once the “isolated” acceleration is integrated, this gap will cause large errors resulting in the displacement being incorrect. At this point, it was decided to include the gyro to measure the angle of tilt.
To use the gyro, the initial angle would have to be initialised as the gyro doesn’t know its starting position, this was done by finding the inverse tangent of the initial gravity vectors on the Y and Z axis.
 Z_{init}=\frac{\sum_{0}^{i}Z(i)}{i} \qquad Y_{init}=\frac{\sum_{0}^{i}Y(i)}{i}
 \Theta(0) =tan^{-1}(\frac{Y_{init}}{Z_{init}})
Now that the initial angle was known the gyro could be used to find the angle during the recording by adding to this angle. The gyro doesn’t output angle but instead outputs angular velocity so this would have to be integrated to give us the angle(I’m going to go into more detail about integration in a further post). Similar to the ACC the gyro has an offset which needs to be removed to find the angle, this offset unlike the ACC is a constant offset caused by a bias in each axis, this can be easily removed using the averaging method mention before and from testing, it was found that the gyro is quite reliable for measuring angle.

The angle measured by the gyro is the angle(θ) between gravity and the Z axis in the 2D triangle ZY and if the full extent of gravity was shared between the Y and Z axes, this would be enough information to calculate the gravity vectors by using the following equations.theta
 Z_{g}(n)=Cos(\Theta (n))*9.81
 Y_{g}(n)=Sin(\Theta (n))*9.81
But if there is any extent of gravity shared on the X axis these equations become incorrect, if the angle(α) between the X-axis and the gravity vector in the XYZ vector plane is known, the equation can be changed to as follows and the results would be correct.alpha_x
 Z_{g}(n)=Cos(\Theta (n))*Sin(\alpha )*9.81
 Y_{g}(n)=Sin(\Theta (n))*Sin(\alpha )*9.81
The above equation assumes that the angle α remains constant from the initial conditions as there is no way to calculated it dynamically, this is a safe assumption for this case as the movement being recorded involves very little to no rotation left or right. A way α could be calculated is using the Pythagoras theorem to find the X axis gravity and then α can be calculated.
 X_{init} =\sqrt{9.81^2-Z_{init}^2-Y_{init}^2} \qquad \alpha =Cos^{-1}(\frac{X_{init}}{9.81})
This works well for most angles of α and θ but when either is close to 0,45 or 90 degrees the square root will return complex numbers showing that the slight errors in the Z and Y axes which are causing problems so this method couldn’t be used. The way I found around this was to use the equation without α, this assumes that α is 90 but because it won’t always be 90, there’ll be an offset between the raw accelerometry data and the calculated gravity and if the offset is found, it can be subtracted from the calculated to give the right answer.
 Z_{g}(n)=Z_{g}(n)+(Z_{init}-(\frac{\sum_{0}^{i}Z_{g}}{i}))\qquad Y_{g}(n)=Y_{g}(n)+(Y_{init}-(\frac{\sum_{0}^{i}Y_{g}}{i}))
In the graph above is the raw acceleration data from the linear movement(blue), calculated gravity vector assuming α is 90(orange) and the calculated gravity vector plus the offset. looking at the yellow graph it follows fairly well to the blue showing that this method can be used to calculate the gravity vectors throughout a recording.

Another method that was investigated using the gyro was using the area of the triangle ZY and tangent of the angle θ. The was this method works under the same assumption as before that the angle α has to remain constant, if this is true and the gravity on the Y and Z axes can change but the area of the triangle ZY will remain constant. Using this and Pythagoras theorem we can calculate the Y and Z gravity vectors dynamically. The formula manipulation can be seen below
\qquad A = \frac{Z_{init}*Y_{init}}{2} \qquad \qquad \Theta(0) =tan^{-1}(\frac{Y_{init}}{Z_{init}})
 Z_{g}(n)=\frac{2*A}{Y_{g}(n)} \qquad Y_{g}(n)=Z_{g}(n)*tan(\Theta (n))=\frac{2*A*tan(\Theta(n))}{Y_{g}(n)}
Y_{g}(n)= \left \{ \begin{aligned} &\sqrt{2*A*tan(\Theta (n))}, && \text{if}\ Y_{init}> 0 \\ &-\sqrt{2*A*tan(\Theta (n))}, && \text{otherwise} \end{aligned} \right.
Because the equation for the Y gravity vector cannot return a negative, a condition must be set, if Yinit is negative, multiple that equation by -1. An example of this working is shown below
The above is the same data as the previous graph but using this new method to calculate the gravity vector. Like the previous method, it follows the blue graph very well but as this post is getting a bit long, I’m  going to compare the methods and discuss which one I used and why in my next post.

<< Part 2Part 3 >>


2 thoughts on “Measuring Displacement Using Accelerometers: Part 1-Calculating Gravity Vectors

Leave a Reply

Fill in your details below or click an icon to log in: Logo

You are commenting using your account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s