2015年11月29日 星期日

[Algorithm] 最小二乘法 Least Squares

最小二乘法又稱最小平方法,是一種數學方法,透過最小的誤差平方和來找出最佳的參數或一些問題,工程上應該算是系統判別裡一門學問。最常見的應用應該就是在迴歸分析或稱為擬合(regression or fitting)上,下面舉一個例子:

假設有 3 個資料座標,我想找出一條直線 L:y = ax + b 的 a, b 使得這 3 個座標到直線的距離合有最小值,其實就是找出這 3 筆資料的迴歸線,如下圖所示


方法一

定義一函數 F


其 F 的平方合表示成 Q,其中 N 為資料數,當有 3 筆資料,N = 3


欲使 Q 為最小值,必有


依上述條件計算出結果


整理成矩陣形式



這裡就可以計算出直線 L 的參數了,若是想使用 C 語言或是其他程式來計算的話,建議直接用高斯消去法來處理,因為運算量上面,高斯消去法較低於逆矩陣的運算量。


方法二

另一種方法是計算聯立方程式,


將上式表示下列形式


因為 A 矩陣並不是方陣,所以左邊同時乘上自己的轉置,轉換成方陣後就可以求逆矩陣了


展開上式, 可以得到下面的結果


結果與方法一是一樣的。


這是一種 LS 的應用,LS 就是一種找出誤差合最小(不一定為 0)的模型參數的方法。


2015年11月27日 星期五

[UDOO NEO] 建立 FTP 伺服器


安裝 vsftpd
> sudo apt-get install vsftpd

編輯 vsftpd.conf 內容
> sudo nano /etc/vsftpd.conf



將 vsftpd.conf 裡的下面部分修改後儲存離開

anonymous_enable=NO
local_enable=YES
write_enable=YES



重新開機
> sudo reboot

windows 端可以用 FTP 軟體來連線,不過要確定 UDOO 的 IP 位址,目前因為是使用學校的固定 IP,所以沒甚麼問題,固定 IP google 一下資訊應該都很多。

另外就是 windows 端的 FTP 軟體建議可以使用 MobaXterm,除了 FTP 外,還有 Serial、VNC、SSH ... 等等,頗方便的。

2015年11月26日 星期四

[UDOO NEO] 簡單編譯一個程式 Hello World


移到桌面(我是存在桌面上)
> cd /home/udooer/Desktop

建立 Hello 資料夾
> mkdir Hello

進入 Hello 資料夾
> cd Hello

透過 nano editor 開啟 hello.c(sudo 指令需要輸入密碼,預設是 udooer)
> sudo nano hello.c

輸入以下程式,並儲存(ctrl+o)離開(ctrl+x)
#include <stdio.h>;
int main()
{
    printf("Hello World!!\n");
    return;
}



編譯 hello.c
> gcc hello.c -o hello

執行 hello
> ./hello


因為 int 打錯了,所以重新再開啟 nano 編輯。


[Unboxing] UDOO NEO FULL 入手開箱

上禮拜五買的 UDOO NEO 終於到了,一片大約 2300(含 5% 稅和 VISA 手續費),因為有滿金額免運所以比較便宜,單獨買含運費的話,大概 2600 台幣左右,貴一些,東西是直接在官方商店購買的,本來想說買便宜一點的版本 UDOO NEO EXTENDED,但都缺一個乙太網路接頭在那邊實在看不順眼...所以就買了 UDOO NEO FULL,詳細的規格與版本差異可以參考 UDOO SHOP 的內容。



下午特別去附近的三井買一個 SanDisk Ultra 32G 讀 80MB/s 的 SD 卡和 Micro HDMI 轉接頭,雖然有點小貴,但是急用也就算了,除了中文部分還在找方法外,整體用起來都還正常,效能比樹梅派高也是理所當然的,有內建 WIFI 和 Bluetooth 4.0 確實蠻方便,唯一美中不足的就是相關資源和 USB 接口較少,因為最近才上架販售,所以相關資料方面有不少部分都還在新增與建立,中文資料也不多,遇到問題需要花更多時間自己試,但就當作是一種練習吧,另外就是 USB 接口只有 2 個,但如果外接 USB HUB 或是透過 VCN/SSH 連線的話,應該就不成問題了。












2015年11月9日 星期一

[Algorithm] 移動平均濾波 Moving Average

平均法是濾波上最容易實現的一種方法之一,常常被拿來應用,像是每 10 筆資料做一次平均值之類的,但這種方式有下面幾個缺點:

1、假設每秒取樣 n 筆資料時,每 m 筆資料做平均值,更新速度會等於 n/m Hz,低於取樣速度。
2、資料曲線不夠平滑,每筆平均數間容易產生落差。


移動平均顧名思義就是移動 + 平均,此方法可以解決上述問題,在不影響更新速度的同時增加平滑度,運算量也不大,下面將介紹簡單的移動平均與加權的移動平均兩種方法。


簡單移動平均 Simple Moving Average, SMA

移動平均顧名思義就是移動 + 平均,在固定的"視窗大小"下取平均值,而這個視窗會移動,其實就是摺積(Convolution)

gif 圖片截自 wikipedia - Convolution https://en.wikipedia.org/wiki/Convolution


下面是一個實際的範例,紅色框框表示視窗,程式上可以理解成 FIFO,大小為 4 筆資料,初值為 0,總共有 8 筆資料,有問號的 8 筆資料是移動平均算出來的資料,一開始因為 FIFO 的資料沒有被填滿,所以誤差比較大。



大致的概念就是現一個長度 N 的 FIFO,最新的資料進去,最舊的資料出來,FIFO 裡面始終保持 N 筆最新的資料,對裡面 N 筆資料做平均值就是移動平均了,N 的大小會影響到訊號的響應速度,N 越大,越容易抗雜訊,但響應速度會變慢,N 越小,越不易抗雜訊,但響應速度越快,當 N = 1 時,其實就是實際的資料。

下面是自己用 MATLAB 撰寫的移動平均,之前也有寫過一個,不過是直接讓 FIFO 的資料移動,也就是頭跟尾始終保持最新和最舊的資料,但在這種方法下,更新 FIFO 的部分運算量會隨著 FIFO 的長度增加而增加,所以就另外寫了一個直接將最舊的資料替代成最新的資料,更新 FIFO 的運算量在不同長度下將不會變,效率應該也高於前者。


簡單移動平均 moveAve_s.m
function [aveData, p_front] = moveAve_s( newData, moveAveFIFO, fifoLens, windowLens )

    p_front = moveAveFIFO(1);
    p_rear  = mod(p_front - windowLens + fifoLens, fifoLens) + 1;

    sumData = 0;
    if p_front > p_rear
        for i = p_front : -1 : p_rear
            sumData = sumData + moveAveFIFO(i + 1);
        end
    else
        for i = p_front : -1 : 1
            sumData = sumData + moveAveFIFO(i + 1);
        end
        for i = fifoLens : -1 : p_rear
            sumData = sumData + moveAveFIFO(i + 1);
        end
    end

    aveData = sumData / windowLens;
    p_front = mod(p_front, fifoLens) + 1;

end


加權移動平均 Weighted Moving Average, WMA

加權移動平均與簡單的移動平均只差在每次做累加的時候會乘上權重,透過權重來調整資料的重要性或影響性。


加權移動平均 moveAve_w.m
function [aveData, p_front] = moveAve_w( newData, moveAveFIFO, weighted, fifoLens, windowLens )

    p_front = moveAveFIFO(1);
    p_rear  = mod(p_front - windowLens + fifoLens, fifoLens) + 1;

    sumData = 0;
    count = 1;
    if p_front > p_rear
        for i = p_front : -1 : p_rear
            sumData = sumData + moveAveFIFO(i + 1) * weighted(count + 1);
            count = count + 1;
        end
    else
        for i = p_front : -1 : 1
            sumData = sumData + moveAveFIFO(i + 1) * weighted(count + 1);
            count = count + 1;
        end
        for i = fifoLens : -1 : p_rear
            sumData = sumData + moveAveFIFO(i + 1) * weighted(count + 1);
            count = count + 1;
        end
    end

    aveData = sumData / weighted(1);
    p_front = mod(p_front, fifoLens) + 1;

end


驗證兩種移動平均方法 testMoveAve.m
clear all;

timeScal = 0.01;
runTimes = 10;

fifoLens = 256;
windowLens = 32;
fifo = zeros(1, fifoLens+1);
fifo(1) = windowLens;

weighted = zeros(1, windowLens + 1);
for i = 1 : windowLens
    weighted(i + 1) = 2^((windowLens - i) / 4);
end
weighted(1) = sum(weighted(2 : end));

saveLens = 0;
for t = 0 : timeScal : runTimes
    newData = sin(t) + randn();

    p_front = fifo(1);
    fifo(p_front + 1) = newData;
    [sma_data, p_front] = moveAve_s(newData, fifo, fifoLens, windowLens);
    [wma_data, p_front] = moveAve_w(newData, fifo, weighted, fifoLens, windowLens);
    fifo(1) = p_front;

    saveLens = saveLens + 1;
    save_org(saveLens)  = sin(t);
    save_orgn(saveLens) = newData;
    save_sma(saveLens)  = sma_data;
    save_wma(saveLens)  = wma_data;
end

hold on;
grid on;
plot([0 : timeScal : runTimes], save_org,  'g');
plot([0 : timeScal : runTimes], save_orgn, 'r');
plot([0 : timeScal : runTimes], save_sma,  'b');
plot([0 : timeScal : runTimes], save_wma,  'c');

xlabel('t(s)');
ylabel('data');
legend('org', 'org + noise', 'sma', 'wma');


Github→https://github.com/Hom-Wang/MATLAB/tree/master/moveAve

2015年11月3日 星期二

[Algorithm] Cholesky 正定矩陣分解

最近在搞 UKF,需要求解 P 矩陣的開跟號,所以找了一篇文章線代啟示錄 - Cholesky 分解參考,賺寫一個 Cholesky 矩陣分解的程式,暫時先用 MATLAB 實現,方便之後再移植到單晶片上做處理。


簡單講一下原理,假定有一個正定矩陣 A,我們希望他可以被分解成一個 P 矩陣乘上自己的轉置,而這個 P 矩陣是一個下三角矩陣,也就是我們想要求得的分解結果,如下圖所示,


如果 A 矩陣是一個正定矩陣,則可以表示成下面形式,L 和 U 分別為上三角和下三角矩陣(U 和 L 打反了,將就下),X 為對角矩陣,並且 A 矩陣的轉置會等於 A 矩陣,


藉由 A 矩陣的轉置會等於 A 矩陣的條件,可以得到下列關係,並且帶回前式整理,


可以得到 A 矩陣和 P 矩陣的關係,這裡以 3x3 的矩陣做展開,再過歸納,



上式反推得到下列關係,


透過上述關係可歸納出在 n x n 的正定矩陣分解出來的 P 矩陣如下。



最後透過 MATLAB 來做驗證,MATLAB 有一個 Chol() 的指令,用來做 cholesky 分解的,下面自己撰寫好 cholesky 分解後,透過 A = P P' 來驗證結果,當然也可以透過 Chol() 來判斷是否正確,程式沒有很完善,在正定矩陣的判斷等等沒有處理,如果要實際應用,建議需要另外加入一些條件處理特殊狀況。


cholesky 分解, cholesky.m
function sqrt_matrix = cholesky( pdMatrix )

    [n, m] = size(pdMatrix);
 sqrt_matrix = zeros(n);
 for j = 1 : n
        for i = j : n
            if i == j
                tempSum = 0;
                for k = 1 : i - 1
                    tempSum = tempSum + sqrt_matrix(i, k)^2;
                end
                sqrt_matrix(i, j) = sqrt(pdMatrix(i, j) - tempSum);
            else
                tempSum = 0;
                for k = 1 : j - 1
                    tempSum = tempSum + sqrt_matrix(i, k) * sqrt_matrix(j, k);
                end
                sqrt_matrix(i, j) = (pdMatrix(i, j) - tempSum) / sqrt_matrix(j, j);
            end
        end
    end

end

cholesky 驗證, testCholesky.m
n = 10;
X = diag(rand(n, 1));
U = orth(rand(n, n));

pdMatrix = U' * X * U;

P_U = chol(pdMatrix, 'upper');
P_L = chol(pdMatrix, 'lower');

P   = cholesky(pdMatrix);

check = roundn(pdMatrix, -10) == roundn(P * P', -10);
if sum(sum(check)) == n * n
    sprintf('Equal')
else
    sprintf('Not Equal')
end

Github → https://github.com/Hom-Wang/MATLAB/tree/master/cholesky

2015年10月8日 星期四

[QCopter] 從零開始做四軸 (五) - 感測器原理

在四軸飛行器上面常會使用一些感測器做為回授,來取得飛行器的姿態、航向或是高度等資訊,常見有陀螺儀、加速度計、磁力計以及氣壓計這四種,由於微機電系統(Micro Electro Mechanical System, MEMS) 技術的發達,把機械系統縮小到微米(10E-6)或是奈米(10E-9)等級,使的這些感測器得以封裝起來,變成小的 IC。

慣性測量元件(Inertial Measurement Unit, IMU)是一個用來測量姿態角、方向、速度等等的裝置,通常由陀螺儀與加速度計組成,有時候也會加入磁力計與氣壓計來輔助計算,常常使用在載具的導航上面。

上述四種感測器的種類與型號眾多,但依輸出可以分成數位與類比兩種,數位輸出通常使用像是 SPI、I2C 等通訊介面來獲取感測器資料,電路處理上較簡單,功能較多,可能包含濾波器等,而另一種則是類比的輸出,輸出的資料大小會與電壓成正比,會把原始感測到的資訊直接輸出,雖然原始,但比較靈活,通常使用 ADC 來讀取電壓資訊。

在感測器選型時,常常會看到一軸陀螺儀、三軸加速度計等等,其軸數表示為可以感測到物理量的分量,沒有方向的物理量像是溫度、氣壓等,都是單軸就可以得到完整的資訊,但有方向的物理量像是角速度、加速度等,會將物理量投影到軸上,投影就像是把一個建築物(三維)畫在紙(二維)上一樣,在紙上你看不到建築物的後面,但實際上你可以走到建築物後面看,所以軸越多,可以感測到的資訊就越完整。

一直生存在一維空間的生物,就像活在線上,他們只知道前後走,沒有左右的概念,一直生存在二維空間的生物,就像活在紙上,只知道前後左右走,沒有上下的概念,一直生存在三維空間的生物,不只可以前後左右走,還可以上下移動,能做的、能看到的東西都比一維、二維生物來的多,那四維空間的生物會看到甚麼?能做甚麼?這我也不知道,畢竟大部分的人都已經習慣三維空間了,要往高維度的空間去思考是困難的。

這裡以三軸陀螺儀、三軸加速度計、三軸磁力計和氣壓計做說明,首先要先了解三軸之間的關係,假設有三個向量 X、Y、Z,其方向分別為三個軸的方向,那這三個向量會互相垂直、正交(Orthogonality),也就像是 X dot product Y = 0 或 X cross product Y = Z,實心點垂直於螢幕出來,叉叉點垂直於螢幕進入,可以用右手定則來了解。



陀螺儀(Gyroscope or Gyro)

陀螺儀是測量角速度的元件,陀螺儀有短時間準確、靈敏的特性,但用來計算角度時,長時間下的誤差累積會使其變的不可靠,通常我們以 dps(degree per second)來表示角速度,角速度與速度不同,角速度是單位時間內旋轉的角度,而速度則是單位時間內移動的距離,所以若是沒有旋轉是沒有角速度的,若只對 X 軸旋轉,則 Y 軸與 Z 軸的輸出為 0,X 軸的輸出為旋轉的角速度。


下面一張圖是角度、角速度、角加速度的關係,紅線是角速度綠線是角加速度,角速度的一階微分,藍線是角度,角速度的一階積分,所以只要知道取樣時間以及角速度,就可以對其作積分得到當下的角度了。


但是實並非如此,原因就在於感測器都會有誤差與雜訊,如何把誤差與噪音雜訊給濾除掉也是一門學問,下面的數學式子表示從感測器讀到的角速度 w_measure 會等於矩陣 R_gyro 乘上理想的角速度 w_ideal 加上偏移 w_bias 與雜訊 n_noise,矩陣 R_gyro 包含未對準誤差(Misalignment)以及單位轉換(dps/LBS),若感測器與載具完全對準的情況(大部份安裝不准等情況會造成未對準誤差),矩陣 R_gyro 會是一個對角矩陣(Diagonal Matrix),偏移 w_bias、雜訊 n_noise 則由溫度、感測器內部等干擾引起。


理想的角速度 w_ideal 是我們想要的,所以校正的動作其實就是把 R_gyro、w_bias 與 n_noise 求出來並去除。


加速度計(Accelerometer or G-Sensor)

加速度計是測量加速度的元件,除了運動加速度外,也包含地球質量所產生的重力加速度,也就是當感測器自由落體時,運動加速度與重力加速度會剛好抵銷,有輸出都會變成 0,通常以 g 或是 m/s^2 來表示,因為可以感測到重力加速度,所以在有重力的環境中加速度計也常用來做傾斜計,透過重力在三軸上的投影來計算角度。

加速度計常常會搭配陀螺儀來應用,因為重力加速度並不會隨時間快速變化,長時間具有可靠性,所以通常透過此特性來較正陀螺儀,防止誤差的累積。

下圖是在靜止下(僅存在 1g 的重力加速度),不同角度的加速度計,角度 1~8 的 Z 軸因為垂直於重力加速度,所以皆為 0g,


角度 1 (    0 deg)的輸出(0, +g, 0)
角度 2 (  45 deg)的輸出(+0.707g, +0.707g, 0)
角度 3 (  90 deg)的輸出(+g, 0, 0)
角度 4 (135 deg)的輸出(+0.707g, -0.707g, 0)
角度 5 (180 deg)的輸出(0, -g, 0)
角度 6 (225 deg)的輸出(-0.707g, -0.707g, 0)
角度 7 (270 deg)的輸出(-g, 0, 0)
角度 8 (315 deg)的輸出(-0.707g, +0.707g, 0)

歸納出 → X = g*sin(theta), Y = g*cos(theta) → theta = atan(X / Y)

表示在 Z 軸垂直於重力時,輸出的 X, Y 值相除,並取 atan 則可以得到傾斜角度,但需要注意的,這僅限於靜止下,當有運動加速度的影響,得到的角度會是不準確的。

下面是一個加速度計的簡單數學模型,與陀螺儀類似(其實大家都長得差不多),加速度計的輸出 a_measure 會等於矩陣 K_accel 乘上理想的加速度 a_ideal 加上偏移 a_bias 與雜訊 n_noise,理想的加速度 a_ideal 包含運動加速度 a_motion 以及重力加速度 a_gravity,矩陣 K_accel 包含未對準的誤差以及單位的轉換(g/LBS),若感測器與載具完全對準的情況,矩陣 K_accel 也是一個對角矩陣,偏移 a_bias、雜訊 n_noise 則也會由溫度、感測器內部等干擾引起。



磁力計(Magnetometer or Compass

磁力計電子羅盤是用來測量磁場的元件,通常以高斯(Gauss)或特斯拉(Tesla)來表示磁場大小,因為地球本身存在磁場,所以磁力計可以藉由磁場在三軸上的投影來計算出航向角度,但事實上環境中的磁場不僅僅只有地球磁場,還包含了許多的干擾源,依特性可以分成硬磁(Hard Iron)干擾與軟磁(Soft Iron)干擾,硬磁干擾是像永久磁鐵、電池這些指固定強度的干擾,軟磁干擾則是指像電磁鐵、電量變化等,當磁力來源消失時,磁力會變弱,會改變磁力線強度或方向的干擾。

下圖是將以磁力計 X 軸與 Y 軸為座標在不同影響下畫出來的圖,紅色是理想、不受干擾的磁場,是一個圓心在原點的圓形,綠色是受到硬磁干擾的磁場, 也是圓形的磁場,但圓心會編離中心,深藍色是受到軟磁干擾的磁場,會讓圓形變形成橢圓形,但圓心仍在中心,淺藍色是同時受到硬磁與軟磁干擾的形況,偏離中心的橢圓形,這也是大部分磁力計測到的圖形,如果不校正成圓形的話,在行像的計算就會產生誤差,導致問題出現。


下面是一個磁力計的簡單數學模型,磁力的輸出 h_measure 會等於矩陣 M_mag 乘上理想的磁場 h_ideal 加上偏移 h_bias 與雜訊 n_noise,矩陣 M_mag 包含未對準的準誤、單位的轉換(gauss/LBS)、硬磁干擾、軟磁干擾,偏移 h_bias、雜訊 n_noise 則也會由溫度、感測器內部等干擾引起。




氣壓計(Barometer)

氣壓計是用來測量氣壓的元件,通常使用帕(Pa)或百帕(hPa)來表示,一個大氣壓力大約是 1013.25 hPa,大氣壓會隨著高度的提升而下降,其關係為每上升 9 公尺,大氣壓力降低 100 Pa,透過此關係可以使用氣壓計計算出目前的海拔高度,以目前 MEMS 氣壓計可以做到 1 Pa RMS 的高解析度情況下,大約可以檢測到 9 公分的高度變化,但這都在是比較理想的情況下,實際上還會有空氣的流動、溫度與濕度的影響,使得計算、測量的誤差產生。



詳細的感測器校正方法之後會再說明,這一章只先說明感測器的一些原理與模型。

下面這篇文章也值得參考:
A Guide To using IMU (Accelerometer and Gyroscope Devices) in Embedded Applications
http://www.starlino.com/imu_guide.html

下一篇→從零開始做四軸 (六) - 數學模型