在要求精度不高的情況，可以使用三軸加速度積分得到位移，飛思卡爾給出了官方方法，下文來自翻譯說明
http://cache.freescale.com/files/sensors/doc/app_note/AN3397.pdf?fsrch=1&sr=2
摘要
此文檔描述並使用MMA7260QT三軸加速計和低功耗的9S08QG8八位單片機實現求解位置的算法 。
在今天先進的電子市場，有不少增加了許多特性和智能的多功能的產品。定位和遊戲只是得益於獲取到的位置信息的一部分市場。一個獲取這種信息的可選方案是通過使用慣性傳感器。從這些傳感器中取得的信號需要進行一些處理，因爲在加速度和位置之間沒有一種直接轉換。
爲了獲得位置，需要對加速度進行二次積分。本文介紹一種簡單的算法實現加速度的二重積分。爲了獲取加速度的二重積分，一個簡單的積分要進行兩次，因爲這樣也可以順便獲取速度。
接下來要展示的算法，能夠應該於任何傳感軸，所以一維、二維、三維的位置都可以被計算出。當在獲取三維位置信息時，要特別地除去重力加速度的影響。下面的算法實現還包括了一個二維繫統的例子（比如鼠標）。
應用潛力
這種算法的應用潛力在於個人導航、汽車導航和（back-up）GPS、防盜設備、地圖追蹤、3D遊戲、計算機鼠標等等。這類產品都需要用到求解位置信息的算法。
本文所介紹的算法在位移精度要求不是很嚴格的情況下很有用。其他的情況和影響特別是應用，當採用本文算法時，需要考慮一下。對最終程序進行微小的修改和調整，這種算法能夠達到更高的精度。
理論知識和算法
理解本文算法的最好方法是回顧一下數學上的積分知識。
加速度是一個對象速度的變化速率。同時，速度是同樣一個對象位置的變化速率。換句話說，速度是位置的導數，加速度是速度的導數，因此如下公式：
積分和導數相反。如果一個物體的加速度已知，那麼我們能夠利用二重積分獲得物體的位置。假設初始條件爲0，那麼有如下公式：
一個理解這個公式的方法是將積分定義成曲線下面包圍的區域，積分運算結果是極小區域的總和，區域的寬度趨近於0。換句話說，積分的和表示了一個物理變量的大小（速度）。
利用前面的一個概念——曲線下面的區域，我們能得出一個結論：對一個信號採樣，得到該信號大小的瞬時值，所以能夠在兩次採樣之間得到一個小的區域。爲了獲得連貫的值採樣時間必須相同。採樣時間代表這塊區域的寬，同時採樣得到的值代表區域的高。爲了消除帶有分數的乘法（微秒或毫秒），我們假定時間爲一個單位。
現在，我們知道了每個代表區域寬度的採樣時間等於1。下一個結論是：
積分的值可以約等於區域面積之和。
如果採樣時間趨近於0，那麼結論將是正確的。但在實際中，將會產生如下錯誤，在處理的過程中，這個誤差將會一直累積。
這些錯誤稱爲採樣損失。爲了減少這些錯誤，我們再做進一步的假設。結果區域能夠看成由兩塊小的區域的組合：
區域1是前一次採樣的值（方形），區域2是一個三角形，是前一次採樣和當前採樣之差的一半。
通過這種方法，我們現在有一個一階近似（插值）的信號。
現在的錯誤比以前的近似的低得多。
儘管加速度有正有負，但採樣的值總是正的（基於MMA7260QT的輸出特性），因此需要做一個偏移判斷，換句話說，需要一個參考。這個程序即爲校準程序。
校準程序用於在沒有移動情況下的加速度值上。這時，獲得的加速度值可以看成是零參考點。低於零參考點的值代表負值（減速），高於零參考點的值代表正值（加速）。
加速度計的輸出範圍爲0v到Vdd，並且它通常由AD轉換器得到。0值接近Vdd/2。前面獲得的校準值會被芯片的方向和分解在各軸的靜態加速度（重力加速度）所影響。如果裝置剛好平行於地球表面，那麼校準值將會很接近Vdd/2。
接下來的這張圖用於展示校準程序的結果。
從採樣的信號減去零參考值，我們獲得真正的採樣加速度。
A1代表正加速度，A2代表負加速度。
如果我們將這數據看作是採樣數據，那麼信號值將和下圖非常接近。
通過使用上面的積分公式，Formula 1，我們將獲得速度的比例近似值。同樣，爲了獲取位置，需要再進行一次積分。在獲得的速度數值上應用相同的公式和步驟，我們現在獲得了一個瞬時位置的比例近似值。（見圖6）
軟件設計相關注意事項
當在現實世界的實現中應用這種算法，應該考慮一下下面的步驟和建議。
- 信號存在一定的噪聲，所以信號必須經過數字濾波。本算法中的濾波是一種移動平均算法，要處理的值是一定數量採樣值的平均結果。
- 即使以前過濾的一些數據可能由於機械噪聲導致錯誤，所以必須實現另一個濾波器。根據過濾的樣品數，一個真實加速度的窗口能夠被選擇（一般爲16±2採樣次數）。
- 無運動狀態對獲得正確的數據是至關重要的。校準例程需要在應用程序的開頭。該校準值必須儘可能準確。
- 加速度的真實值等於採樣值減去校準值；它可以是正數或負數。當你在定義變量的時候，這絕對不能被忽略，需要定義爲有符號數。
- 更快的採樣頻率意味着更精確的結果，因爲採樣頻率越快誤差越少。但是需要更多的內存、時間和硬件方面的考慮。
- 兩次採樣之間的時間必須要相同。如果這個時間不相同，將會產生錯誤。
- 兩次採樣結果之間的線性近似值（插值）被推薦用於更精確的結果。
代碼註釋
1. 校準程序
這個校準程序移除了加速度傳感器的輸出偏移分量，因爲存在重力加速度（靜態的加速度）。
校準程序在加速度計處於無運動狀態時，對加速度求平均值。採樣的數量越多，加速度的校準結果越精確。
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 }
2. 濾波
低通濾波是消除加速度計中信號噪音(包括機械的和電子的)很好的方法。爲了當對信號進行積分時減少大部分的錯誤，減少噪音對定位應用是至關重要的。
一個簡單的低通過濾採樣信號的方式是執行滾動平均值。過濾簡單地獲得一組採樣的平均值，獲得穩定的採樣總數的平均值是很重要的。採樣數太多可能造成數據丟失，而太少又可能導致結果不精確。
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;
3. 機械濾波窗口
當處於無運動狀態時，加速度上的微小錯誤可能會被當作一個常量速度，因爲在採樣值加和之後，它並不等於0。在沒有運動的理想情況下，所有的採樣值都爲0。該常速表示一個連續的移動和不穩定的位置。
即使之前過濾的一些數據可能不正確，因此一個用於區別無運動狀態的"有效數據"和"無效數據"的窗口必須實現。
1 if ((accelerationx[1] <=3)&&(accelerationx[1] >= -3)) //Discrimination window applied to
2 {
3 accelerationx[1] = 0;
4 } // the X axis acceleration variable
4. 定位
二重積分是利用加速度數據獲取位置所需要的步驟。第一次積分獲得速度，第二次積分獲得位置。
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);
5. 數據傳輸
這部分主要目的是用於調試和顯示；這個函數中32位的結果被切割分4個8位數據。
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 }
6. "移動結束"檢查
基於積分表示曲線下方區域的概念，速度是加速曲線下方的區域的結果。
如果我們觀察一個典型的移動:一個物體沿一個軸從點A移動到點B，一個典型的加速度結果如下圖:
觀察上面的圖，加速度先增加後減少直到速度達到最大值（加速度始終爲正代表往同一方向加速）。然後以相反的方式加速，直到它再次到達0。在這一點上達到一個穩定的位移和新的位置。
在真實世界中，其中曲線正側下方的區域面積不等於負側上方的區域面積，積分結果將永遠不會達到零速度，因此將是一個傾斜定位（從未穩定）。
正因爲如此，將速度強制減爲0非常關鍵。這是通過不斷讀取加速度值和0進行比較而實現的。如果在一定數量的採樣中，這種情況存在（sample==0）的話，速度將簡單地返回爲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 }
7. 源代碼
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 /*****************************************************************************************/
原理圖
總結
本文檔爲利用加速度計實現定位算法提供了一些基本的概念。
這種特定的算法在對位移精度要求不是很嚴格中非常有用。其他注意事項和具體的應用程序的影響應該在實現這個示例時被考慮。
這種積分算法適合於低端嵌入式應用，因爲它的簡單性和少量的指令。它也沒有涉及任何浮點運算。