|
四軸——標定加速度計
這幾天都在標定加速度計，終于完成了。
- 方案
傳感器總會有缺陷，測量的值有誤差，所以要校正。要校正加速度計，最簡單有效的辦法就是利用重力了。以不同方向在靜止狀態(tài)測量加速度，然后把數(shù)據(jù)擬合到當?shù)刂亓铀俣龋屯瓿蓪铀俣扔嫷臉硕?，直接無視Datasheet上的精度參數(shù)。
- 采集數(shù)據(jù)
采集數(shù)據(jù)就要用到上次做的“黑匣”了，測量到的部分數(shù)據(jù)如下。
里面的0x55AA是幀開始符，表示一組測量數(shù)據(jù)的開始，緊接著就是3個double類型的值，小端格式，表示x,y,z方向的原始測量值。
- 理論依據(jù)
這次標定加速度計，最大的收獲是——把困擾多年的“最小二乘法”弄懂了。
設(shè)x‘,y’,z‘為實際加速度分量，G0為當?shù)刂亓铀俣戎?。理想情況下有下面的等式：
但測量有誤差，設(shè)測量值為[x,y,z]，則測量值和實際值可以用下面的公式擬合。擬合方式為線性擬合，其中a',b',c'是比例因子，d',e',f'為偏移因子。通過這條公式，可以把測量值“變成”實際值（下面稱作“估計值”），標定加速度計的目標，就是確定[a',b',c',d',e',f']這六個參數(shù)（下面稱作“參數(shù)”），使估計值更接近實際值。
代入理想情況的等式，有：
這分明是橢球嘛，展開得：
為方便推導和運算，寫成如下形式：
比較以上兩式，有：（注意要加上系數(shù)k）
反過來有：
現(xiàn)在已經(jīng)在靜止狀態(tài)測量出一些數(shù)據(jù)，記作[xn,yn,zn]，表示第n組測量值，對應(yīng)的實際值都是重力G0。為了衡量參數(shù)的合適程度，引入誤差函數(shù)Δn，表示第n個估計值與實際值的偏差，如果硬要說出具體意義，就是：估計值和實際值的平方差的1/k倍。
Δn是第n組數(shù)據(jù)的偏差，我們的目標是整體偏差最小，所以引入整體偏差指數(shù)I，等于全部數(shù)據(jù)的偏差的平方和（“最小二乘法”的“二乘”就是指平方）。
再次強調(diào)，我們的目標是找出一組參數(shù)，使整體的估計值最接近實際值。換句話說，就是：已知[xn,yn,zn]，求使I最小的[a',b',c',d',e',f']。直接求[a',b',c',d',e',f']有困難，要利用[a,b,c,d,e,f,g]，不廢話嗎？這就是我們引入abc的原因啊。所以不用猶豫了，動用高數(shù)中的偏導吧，用I分別對[a,b,c,d,e,f,g]求偏導，然后通通都要是0，這是I最小的必要條件。于是得到下面7條方程，對于[a,b,c,d,e,f,g]，都是線性方程，組合起來就是一組7*7的齊次線性方程組。
仔細觀察上面的方程，很有規(guī)律。其實可以寫成更簡潔（傳說中的數(shù)學的優(yōu)雅?。┑男问健Ｒ肓邢蛄縱和pn，v為變量組，pn為系數(shù)組，單個偏差可以寫成它們的點積。
神奇的一刻到了，方程組可以用一條式表示！再經(jīng)過一些變換（向量點積交換，證明不出，歸納出來的。），得到終極的矩陣形式。而且矩陣A相當利于程序?qū)崿F(xiàn)，算法復雜度甚至是完美的“online”！
呵呵，一不小心，把最小二乘法的一般形式推導出來了。
至此，問題歸結(jié)到線性方程組的求解，理論上已經(jīng)沒有任何障礙（那是當然的，都已經(jīng)用了上百年了）。
- 編寫軟件
先梳理一下思路。現(xiàn)在有一個原始的RCD格式的數(shù)據(jù)，首先要提取出[xn,yn,zn]，然后用[xn,yn,zn]算出向量pn，再用pn轉(zhuǎn)置自乘，累加成矩陣A，接著求以A為系數(shù)矩陣的齊次線性方程組，算出[a,b,c,d,e,f,g]，這樣就可以得到最佳的[a',b',c',d',e',f']了。
第一步需要了解RCD文件的結(jié)構(gòu)，那是做黑匣的時候自己設(shè)計的，沒問題；然后加加減減得到A，這個也不難；以前混ACM的時候，對著《線性代數(shù)》寫過解線性方程組的庫，解方程組小意思；最后再次加加減減，[a',b',c',d',e',f']就出來啦。關(guān)鍵代碼如下： 輸入數(shù)據(jù)循環(huán)。
累加生成矩陣。
解方程，vp就是[a,b,c,d,e,f,g]啦。
[a,b,c,d,e,f,g]轉(zhuǎn)[a',b',c',d',e',f']。
- 校驗結(jié)果
一共14339組數(shù)據(jù)，左邊是原始采樣值，中間是標定后的加速度，右邊是估計值與實際值的誤差。
重力加速度取廣州的9.7883m/s2，大多數(shù)誤差在0.5%以下，結(jié)果還算滿意。
后來試過周期性忽略某些采樣點，得出不同的參數(shù)，發(fā)現(xiàn)參數(shù)只有3～4位有效數(shù)字
|
