In the case that the accuracy is not high, the displacement can be obtained by integrating the three axis acceleration. Freescale gives the official method
Cache.freescale.com/files/senso…
Abstract
This document describes and implements the algorithm to solve the position using the MMA7260QT three-axis accelerometer and the low-power 9S08QG8 eight-bit microcontroller.
In today’s advanced electronics market, there are many multifunctional products that add many features and intelligence. Location and gaming are only part of the market that benefit from location information. One alternative to obtaining this information is through the use of inertial sensors. The signals obtained from these sensors require some processing because there is no direct conversion between acceleration and position.
To get the position, we need to integrate the acceleration twice. This paper introduces a simple algorithm to realize double integration of acceleration. To get the double integral of acceleration, a simple integral is done twice, because you can also get the velocity incidentally.
The algorithm to be shown next can be applied to any sensor axis, so the one, two, and three dimensional positions can be calculated. When obtaining three-dimensional position information, the influence of gravity acceleration should be especially removed. The following algorithm implementation also includes an example of a two-dimensional system (such as a mouse).
Potential applications
The potential applications of this algorithm are personal navigation, car navigation and back-up GPS, anti-theft devices, map tracking, 3D games, computer mice and more. All of these products require algorithms to solve location information.
The algorithm presented in this paper is useful when the displacement accuracy is not strictly required. Other situations and effects, especially in applications, need to be considered when adopting the algorithm in this paper. With minor modifications and adjustments to the final program, this algorithm can achieve higher accuracy.
Theoretical knowledge and algorithms
The best way to understand this algorithm is to review the mathematics of integration.
Acceleration is the rate at which an object’s velocity changes. Meanwhile, velocity is the rate at which the position of the same object changes. In other words, velocity is the derivative of position and acceleration is the derivative of velocity, hence the following formula:
An integral is the opposite of a derivative. If the acceleration of an object is known, then we can use a double integral to get the position of the object. Assuming that the initial condition is 0, the following formula can be obtained:
One way to understand this formula is to define the integral as the region under the curve, and the result of the integral is the sum of very small regions whose width tends to zero. In other words, the sum of the integral represents the magnitude (velocity) of a physical variable.
Using the previous concept of the region under the curve, we can come to the conclusion that sampling a signal gives an instantaneous value of the size of the signal, so that a small region can be obtained between two samples. Sampling times must be the same in order to obtain consistent values. The sampling time represents the width of the region and the sampled value represents the height of the region. To eliminate multiplications with fractions (microseconds or milliseconds), we assume that time is a unit.
Now, we know that the sampling time for each representative region width is equal to 1. The next conclusion:
The value of the integral can be approximately equal to the sum of the areas.
If the sampling time approaches 0, the conclusion will be correct. However, in practice, the following errors will be generated, which will accumulate during the processing process.
These errors are called sampling losses. To reduce these errors, let’s make a further assumption. The resulting region can be viewed as a combination of two smaller regions:
Region 1 is the value of the previous sample (square), and region 2 is a triangle that is half the difference between the previous sample and the current sample.
In this way, we now have a first-order approximation (interpolation) of the signal.
The current error is much lower than the previous approximation.
Although the acceleration can be positive or negative, the sampled value is always positive (based on the output characteristics of MMA7260QT), so an offset judgment needs to be made, in other words, a reference is required. This procedure is the calibration procedure.
Calibration procedures are used for acceleration values without movement. In this case, the obtained acceleration value can be regarded as zero reference point. Values below zero reference point represent negative values (deceleration) and values above zero reference point represent positive values (acceleration).
The accelerometer output ranges from 0V to Vdd, and it is usually obtained by an AD converter. The value of 0 is close to Vdd/2. The calibration values obtained previously are affected by the orientation of the chip and the static acceleration (gravitational acceleration) of the decomposition at each axis. If the device is parallel to the earth’s surface, the calibration value will be close to Vdd/2.
The following diagram shows the results of the calibration procedure.
Subtracting the zero reference value from the sampled signal, we obtain the true sampled acceleration.
A1 is positive acceleration, A2 is negative acceleration.
If we consider this data as sampled data, the signal value will look very similar to the following figure.
By using the above integral Formula, Formula 1, we will obtain a proportional approximation of the velocity. Again, to get the position, you have to integrate again. Applying the same formula and steps to the obtained velocity values, we now obtain a proportional approximation for an instantaneous position. (See Figure 6)
Notes on software design
The following steps and recommendations should be considered when applying this algorithm to real-world implementations.
- There is some noise in the signal, so the signal must be filtered digitally. The filtering in this algorithm is a moving average algorithm, and the value to be processed is the average result of a certain number of sample values.
- Even though some of the previously filtered data may be faulty due to mechanical noise, another filter must be implemented. Depending on the number of filtered samples, a window of true acceleration can be selected (typically 16±2 sampling times).
- Inactivity is critical to getting the right data. The calibration routine needs to be at the beginning of the application. The calibration must be as accurate as possible.
- The true value of acceleration is equal to the sampling value minus the calibration value; It can be positive or negative. This should never be ignored when you’re defining variables, you need to define them as signed numbers.
- Faster sampling frequency means more accurate results, because faster sampling frequency means less error. But it requires more memory, time, and hardware considerations.
- The time between samples must be the same. If The Times are different, an error will be generated.
- Linear approximation (interpolation) between two sample results is recommended for more accurate results.
Code comments
1. Calibration procedure
This calibration procedure removes the offset component of the acceleration sensor’s output because of the presence of gravitational acceleration (static acceleration).
The calibration program averages the acceleration when the accelerometer is in a state of no motion. The more the number of samples, the more accurate the calibration results of the acceleration.
1 voidCalibrate(void) 2 { 3 unsignedint count1; 4 count1 =0; 5 do{ 6 ADC_GetAllAxis(); 7 sstatex = sstatex +Sample_X; // Accumulate Samples 8 sstatey = sstatey +Sample_Y; 9 count1++; 10 }while(count1! =0x0400); // 1024 times 11 sstatex=sstatex>>10; // division between 1024 12 sstatey=sstatey>>10; 13}Copy the code2. The filter
Low pass filtering is a good method to eliminate signal noise (both mechanical and electronic) in accelerometers. Noise reduction is essential for positioning applications in order to reduce most errors when integrating the signal.
A simple way of filtering the sampled signal is to perform a rolling average. Filtering simply averages a set of samples, and it is important to get a stable average of the total number of samples. Too many samples may result in data loss, while too few may result in inaccurate results.
1 do{ 2 accelerationx[1]=accelerationx[1]+Sample_X; //filtering routine for noise attenuation 3 accelerationy[1]=accelerationy[1]+Sample_Y; //64 samples are averaged. The resulting 4 count2++; // average represents the acceleration of 5 // an instant. 6 }while(count2! =0x40); // 64 sums of the acceleration sample 7 accelerationx[1]= accelerationx[1]>>6; // division by 64 8 accelerationy[1]= accelerationy[1]>>6;Copy the code3. Mechanical filtering window
When in the inactive state, a small error in acceleration may be treated as a constant velocity because it does not equal zero after the sample values are added up. In the ideal case where there is no movement, all the samples are zero. This constant velocity represents a continuous moving and unstable position.
Even though some of the previously filtered data may be incorrect, a window must be implemented to distinguish between “valid data” and “invalid data” in inactive states.
1 if ((accelerationx[1] <=3)&&(accelerationx[1] >= -3)) //Discrimination window applied to
2 {
3 accelerationx[1] = 0;
4 } // the X axis acceleration variable
Copy the code
4. Locate
Double integration is the step required to obtain position from acceleration data. The first integral gives you speed, and the second one gives you position.
1 //first integration
2 velocityx[1]= velocityx[0]+ accelerationx[0]+((accelerationx[1]- accelerationx[0])>>1)
3 //second integration
4 positionX[1]= positionX[0]+ velocityx[0]+((velocityx[1]- velocityx[0])>>1);
Copy the code
5. Data transmission
The main purpose of this section is for debugging and display; The 32-bit result in this function is split into four 8-bit data pieces.
1 if (positionX[1]>=0) 2 { //This line compares the sign of the X direction data 3 direction= (direction | 0x10); // if its positive the most significant byte 4 posx_seg[0]= positionX[1] & 0x000000FF; // is set to 1 else it is set to 8 5 posx_seg[1]= (positionX[1]>>8) & 0x000000FF; // the data is also managed in the 6 // subsequent lines in order to be sent. 7 posx_seg[2]= (positionX[1]>>16) & 0x000000FF; // The 32 bit variable must be split into 8 posx_seg[3]= (positionX[1]>>24) & 0x000000FF; // 4 different 8 bit variables in order to 9 // be sent via the 8 bit SCI frame 10 } 11 else 12 { 13 direction=(direction | 0x80); 14 positionXbkp=positionX[1]-1; 15 positionXbkp=positionXbkp^0xFFFFFFFF; 16 posx_seg[0]= positionXbkp & 0x000000FF; 17 posx_seg[1]= (positionXbkp>>8) & 0x000000FF; 18 posx_seg[2]= (positionXbkp>>16) & 0x000000FF; 19 posx_seg[3]= (positionXbkp>>24) & 0x000000FF; 20}Copy the code
6. “End of movement” check
Based on the concept that the integral represents the region below the curve, velocity is the result of accelerating the region below the curve.
If we look at A typical motion: an object moving along an axis from point A to point B, A typical acceleration results in the following figure:
Looking at the graph above, the acceleration increases and then decreases until the velocity reaches its maximum (acceleration is always positive, which means accelerating in the same direction). And then accelerate in the opposite way until it reaches zero again. At this point a stable displacement and a new position are achieved.
In the real world, where the area under the positive side of the curve is not equal to the area above the negative side, the integral result will never reach zero velocity and will therefore be an oblique position (never stable).
Because of this, forcing the speed down to zero is critical. This is done by constantly reading the acceleration value and comparing it to zero. If this is the case in a certain number of samples (sample==0), the speed will simply return to 0.
1 if (accelerationx[1]==0) // we count the number of acceleration samples that equals zero 2 { 3 countx++; 4 } 5 else 6 { 7 countx =0; 8 } 9 if (countx>=25) // if this number exceeds 25, we can assume that velocity is zero 10 { 11 velocityx[1]=0; 12 velocityx[0]=0; 13}Copy the code
7. The source code
1 #include <hidef.h>
2 #include "derivative.h"
3 #include "adc.h"
4 #include "buzzer.h"
5 #include "SCItx.h"
6 #pragma DATA_SEG MY_ZEROPAGE
7 unsigned char near Sample_X;
8 unsigned char near Sample_Y;
9 unsigned char near Sample_Z;
10 unsigned char near Sensor_Data[8];
11 unsigned char near countx,county ;
12 signed int near accelerationx[2], accelerationy[2];
13 signed long near velocityx[2], velocityy[2];
14 signed long near positionX[2];
15 signed long near positionY[2];
16 signed long near positionZ[2];
17 unsigned char near direction;
18 unsigned long near sstatex,sstatey;
19 #pragma DATA_SEG DEFAULT
20 void init(void);
21 void Calibrate(void);
22 void data_transfer(void);
23 void concatenate_data(void);
24 void movement_end_check(void);
25 void position(void);
26 void main (void)
27 {
28 init();
29 get_threshold();
30 do
31 {
32 position();
33 }while(1);
34 }
35 /*******************************************************************************
36 The purpose of the calibration routine is to obtain the value of the reference threshold.
37 It consists on a 1024 samples average in no-movement condition.
38 ********************************************************************************/
39 void Calibrate(void)
40 {
41 unsigned int count1;
42 count1 = 0;
43 do{
44 ADC_GetAllAxis();
45 sstatex = sstatex + Sample_X; // Accumulate Samples
46 sstatey = sstatey + Sample_Y;
47 count1++;
48 }while(count1!=0x0400); // 1024 times
49 sstatex=sstatex>>10; // division between 1024
50 sstatey=sstatey>>10;
51 }
52 /*****************************************************************************************/
53 /******************************************************************************************
54 This function obtains magnitude and direction
55 In this particular protocol direction and magnitude are sent in separate variables.
56 Management can be done in many other different ways.
57 *****************************************************************************************/
58 void data_transfer(void)
59 {
60 signed long positionXbkp;
61 signed long positionYbkp;
62 unsigned int delay;
63 unsigned char posx_seg[4], posy_seg[4];
64 if (positionX[1]>=0) { //This line compares the sign of the X direction data
65 direction= (direction | 0x10); //if its positive the most significant byte
66 posx_seg[0]= positionX[1] & 0x000000FF; // is set to 1 else it is set to 8
67 posx_seg[1]= (positionX[1]>>8) & 0x000000FF; // the data is also managed in the
68 // subsequent lines in order to
69 posx_seg[2]= (positionX[1]>>16) & 0x000000FF; // be sent. The 32 bit variable must be
70 posx_seg[3]= (positionX[1]>>24) & 0x000000FF; // split into 4 different 8 bit
71 // variables in order to be sent via
72 // the 8 bit SCI frame
73 }
74 else {direction=(direction | 0x80);
75 positionXbkp=positionX[1]-1;
76 positionXbkp=positionXbkp^0xFFFFFFFF;
77 posx_seg[0]= positionXbkp & 0x000000FF;
78 posx_seg[1]= (positionXbkp>>8) & 0x000000FF;
79 posx_seg[2]= (positionXbkp>>16) & 0x000000FF;
80 posx_seg[3]= (positionXbkp>>24) & 0x000000FF;
81 }
82 if (positionY[1]>=0) { // Same management than in the previous case
83 direction= (direction | 0x08); // but with the Y data.
84 posy_seg[0]= positionY[1] & 0x000000FF;
85 posy_seg[1]= (positionY[1]>>8) & 0x000000FF;
86 posy_seg[2]= (positionY[1]>>16) & 0x000000FF;
87 posy_seg[3]= (positionY[1]>>24) & 0x000000FF;
88 }
89 else {direction= (direction | 0x01);
90 positionYbkp=positionY[1]-1;
91 positionYbkp=positionYbkp^0xFFFFFFFF;
92 posy_seg[0]= positionYbkp & 0x000000FF;
93 posy_seg[1]= (positionYbkp>>8) & 0x000000FF;
94 posy_seg[2]= (positionYbkp>>16) & 0x000000FF;
95 posy_seg[3]= (positionYbkp>>24) & 0x000000FF;
96 }
97 delay = 0x0100;
98 Sensor_Data[0] = 0x03;
99 Sensor_Data[1] = direction;
100 Sensor_Data[2] = posx_seg[3];
101 Sensor_Data[3] = posy_seg[3];
102 Sensor_Data[4] = 0x01;
103 Sensor_Data[5] = 0x01;
104 Sensor_Data[6] = END_OF_FRAME;
105 while (--delay);
106 SCITxMsg(Sensor_Data); // Data transferring function
107 while (SCIC2 & 0x08);
108 }
109 /*****************************************************************************************/
110 /******************************************************************************************
111 This function returns data format to its original state. When obtaining the magnitude and
112 direction of the position, an inverse two's complement is made. This function makes the two's
113 complement in order to return the data to it original state.
114 It is important to notice that the sensibility adjustment is greatly impacted here, the amount
115 of "ones" inserted in the mask must be equivalent to the "ones" lost in the shifting made in
116 the previous function upon the sensibility modification.
117 ******************************************************************************************/
118 void data_reintegration(void)
119 {
120 if (direction >=10)
121 {positionX[1]= positionX[1]|0xFFFFC000;} // 18 "ones" inserted. Same size as the
122 //amount of shifts
123 direction = direction & 0x01;
124 if (direction ==1)
125 {positionY[1]= positionY[1]|0xFFFFC000;}
126 }
127 /******************************************************************************************
128 This function allows movement end detection. If a certain number of acceleration samples are
129 equal to zero we can assume movement has stopped. Accumulated Error generated in the velocity
130 calculations is eliminated by resetting the velocity variables. This stops position increment
131 and greatly eliminates position error.
132 ******************************************************************************************/
133 void movement_end_check(void)
134 {
135 if (accelerationx[1]==0) //we count the number of acceleration samples that equals cero
136 { countx++;}
137 else { countx =0;}
138 if (countx>=25) //if this number exceeds 25, we can assume that velocity is cero
139 {
140 velocityx[1]=0;
141 velocityx[0]=0;
142 }
143 if (accelerationy[1]==0) //we do the same for the Y axis
144 { county++;}
145 else { county =0;}
146 if (county>=25)
147 {
148 velocityy[1]=0;
149 velocityy[0]=0;
150 }
151 }
152 /*****************************************************************************************/
153 /******************************************************************************************
154 This function transforms acceleration to a proportional position by integrating the
155 acceleration data twice. It also adjusts sensibility by multiplying the "positionX" and
156 "positionY" variables.
157 This integration algorithm carries error, which is compensated in the "movenemt_end_check"
158 subroutine. Faster sampling frequency implies less error but requires more memory. Keep in
159 mind that the same process is applied to the X and Y axis.
160 *****************************************************************************************/
161 void position(void)
162 {
163 unsigned char count2 ;
164 count2=0;
165 do{
166 ADC_GetAllAxis();
167 accelerationx[1]=accelerationx[1] + Sample_X; //filtering routine for noise attenuation
168 accelerationy[1]=accelerationy[1] + Sample_Y; //64 samples are averaged. The resulting
169 //average represents the acceleration of
170 //an instant
171 count2++;
172 }while (count2!=0x40); // 64 sums of the acceleration sample
173 accelerationx[1]= accelerationx[1]>>6; // division by 64
174 accelerationy[1]= accelerationy[1]>>6;
175 accelerationx[1] = accelerationx[1] - (int)sstatex; //eliminating zero reference
176 //offset of the acceleration data
177 accelerationy[1] = accelerationy[1] - (int)sstatey; // to obtain positive and negative
178 //acceleration
179 if ((accelerationx[1] <=3)&&(accelerationx[1] >= -3)) //Discrimination window applied
180 {accelerationx[1] = 0;} // to the X axis acceleration
181 //variable
182 if ((accelerationy[1] <=3)&&(accelerationy[1] >= -3))
183 {accelerationy[1] = 0;}
184 //first X integration:
185 velocityx[1]= velocityx[0]+ accelerationx[0]+ ((accelerationx[1] -accelerationx[0])>>1);
186 //second X integration:
187 positionX[1]= positionX[0] + velocityx[0] + ((velocityx[1] - velocityx[0])>>1);
188 //first Y integration:
189 velocityy[1] = velocityy[0] + accelerationy[0] + ((accelerationy[1] -accelerationy[0])>>1);
190 //second Y integration:
191 positionY[1] = positionY[0] + velocityy[0] + ((velocityy[1] - velocityy[0])>>1);
192 accelerationx[0] = accelerationx[1]; //The current acceleration value must be sent
193 //to the previous acceleration
194 accelerationy[0] = accelerationy[1]; //variable in order to introduce the new
195 //acceleration value.
196 velocityx[0] = velocityx[1]; //Same done for the velocity variable
197 velocityy[0] = velocityy[1];
198 positionX[1] = positionX[1]<<18; //The idea behind this shifting (multiplication)
199 //is a sensibility adjustment.
200 positionY[1] = positionY[1]<<18; //Some applications require adjustments to a
201 //particular situation
202 //i.e. mouse application
203 data_transfer();
204 positionX[1] = positionX[1]>>18; //once the variables are sent them must return to
205 positionY[1] = positionY[1]>>18; //their original state
206 movement_end_check();
207 positionX[0] = positionX[1]; //actual position data must be sent to the
208 positionY[0] = positionY[1]; //previous position
209 direction = 0; // data variable to direction variable reset
210 }
211 /*****************************************************************************************/
Copy the code
Schematic diagram
conclusion
This document provides some basic concepts for implementing localization algorithms using accelerometers.
This particular algorithm is very useful when the accuracy of displacement is not very strict. Other considerations and application-specific implications should be considered when implementing this example.
This integration algorithm is suitable for low-end embedded applications because of its simplicity and small number of instructions. Nor does it involve any floating point operations.