KitSprout

自己做的一個簡單室內定位實現，這種 LS 的方法應該算是最入門的方法，簡單明瞭、容易理解與學習，本來想結合到小四軸上面不過一直沒有時間，或許有人會想試看看所以分享出來，目前這個方法雖然簡單，但若不加濾波估測的話，實用性頗低，因為容易受到干擾，可以自己再去改良。

定位的研究其實從很久以前就有了，從飛機、汽車的導航到工廠物流、機器人...等，應用與實現方法數不勝數，但近年來，基於自動化、智慧化的趨勢，定位的需求也逐漸增加，在室外已經有全球衛星導航系統（Global Positioning System, GPS）提供定位的服務，民用的 GPS 誤差基本上都可以做到 1~5 公尺，搭配慣性導航系統（Inertial Navigation System, INS）甚至可以達到公分等級，但在室內由於建築物的金屬水泥結構關係，電磁波不易穿透，導致在室內的訊號強度遠低於室外的訊號，或是完全沒有訊號，所以在室內定位的需求變得無法使用現有的 GPS 來取代，而是使用室內定位系統（Indoor Positioning System, IPS）來替代室內的定位需求，像是建築中的救援定位、停車場的導航、商場顧客的購物資訊或是機器人的自動導航等。

由於定位方法非常之多，像是影像、磁力..等等，所以這裡僅介紹基於電磁波的室內定位方法，依電磁波的定位方法做分類的話．大致可以分成以下四種：

• 到達時間定位（Time of Arrival, TOA）
  
• 到達時間差定位法（Time-Difference of Arrival, TDOA）
  
• 接收信號角度定位（Angle of arrival, AOA）
  
• 接收訊號強度定位（Received Signal Strength, RSS）
  

詳細說明可以參考 元智大學老人福祉科技研究中心－室內定位技術簡介

前兩種基於時間與時間差的方法都需要有較高的硬體規格才可以實現，而第三種需要仰賴陣列天線等，可以測得訊號傳輸方向的裝置，在成本上也較高，只有最後一種，基於訊號強度的定位方法，幾乎不用成本，因為大部分的接收機都可以很容易實現訊號強度的檢測，但由於電磁波訊號強度容易受到環境影響，使得雜訊變多、都普勒效應、多重路徑效應...等等的情況，所以準確度會低於前三種方法。

基於電磁波訊號強度的定位方法可以在細分成下面兩種：

• 電磁波衰減模型 Model Based → 依據電磁波強度與距離成對數關係的模型來做距離的計算。
• 電磁波指紋 Fingerprint → 建立電磁波強度地圖（Radio Map），透過地圖來查詢位置。

第一種方法簡單，模型準確，距離的計算精度也會增加，但由於在室內，多重路徑效應往往難以僅透過模型來解決，第二種方法理論上精確度會高於前者，但第二種方法需要大量繪製地圖的前置工作，若事後若有干擾，地圖仍須更新，精度高但布置成本亦高。
　

室內定位是我的碩士研究題目，目前主要是使用藍牙 4.0（Bluetooth Low Energy, BLE）裝置抓取接收的訊號強度來實現室內定位的功能，為什麼我會選擇 BLE 而不使用 Zigbee 是或 WI-FI 呢？其實在工業上因為 Zigbee 的組網能力具有很大的優勢，這點是 BLE 無法比較的，但 Bluetooth 在民間普及程度遠高於 Zigbee（從 Nokia 時代手機上就有 Bluetooth 了），功耗上也遠低於 WI-FI，加上 Nrdic nRF51 系列的出現，因採用 ARM CrtexM0，支援已經習慣的 Keil MDK 開發環境，所以就果斷選擇藍牙陣營了，相信在 BLE 之後，藍牙仍會不斷的壯大阿。

目前是使用之前自己購買的 nRF51 開發板做開發，不過因為運算效率上的問題，對於之後要實現的演算法應該會一個問題，所以之後打算使用明年上市的 nRF52 系列的芯片。
　

下面就說明簡單的定位方法實現

假定環境中設置 個發射器，第 個發射器座標為 ，移動機器人接收端座標為 如下圖所示：

簡單的接收訊號強度與距離的關係可以表示如下：

其中 是距離發射源 公尺的接收訊號強度（dBm），也就是所謂的 RSS； 是距離發射源的 公尺接收訊號強度（dBm）； 是路徑衰減指數。上式可整理成距離與 RSS 關係如下：

若令 ，則可以把它化簡成下面簡單的示意圖：

每個發射器與接收器的距離 可以表示如下：

理想下，在已知發射機座標 和計算出來的距離 之後，就可以透過這些方程組求解出接收機的座標 了，但現實往往與理想不同，在環境的干擾下，訊號強度會有波動，使得接收的訊號會在一個範圍變動，如下圖所示：

在此狀況下，因為沒有唯一解，透過聯立解方程組的方法就無法應用，所以就引入了之前介紹過的最小二乘法 Least Squares，求解誤差最小值。

將上式距離在 與 相減，並經過整理得到下式：

整理成矩陣形式：

上式可以透過高斯消去法求解 ，或是透過最小二乘法求解，如下式所示

透過 MATLAB 做模擬

https://github.com/Hom-Wang/MATLAB/tree/master/indoorPositioning-LS

實際接收 RSS 實驗

目前在實驗室內放置 beacon 和使用 BLE 開發板獲取接收訊號強度 RSS，並透過 UART 傳到 MATLAB 上實現定位演算法，效果大約可以達到 1 公尺以下，環境中有相同頻率的 WI-FI 以及電腦主機、金屬等干擾源，但仍在是在無動態干擾源的情況下做測試出來的結果，環境還是較偏向理想，與有動態干擾的影響應像是人走動情況該還是存在一定的差異，另外與濕度、溫度應該也有一定的關聯，畢竟在 2.4GHz 頻段下，電磁波特別容易被水分子吸收。

之後有空再補上細部的實驗數據與程式碼。

下面是使用 nRF51 Series 實現 beacon 廣播與掃描獲取 RSS 的程式，有興趣可以自己實驗看看
Github - NRF51_BLE_Beacon

Github - https://github.com/Hom-Wang/NRF51

[2020/07/01] kSerial 在微控制器上將資料實時傳到 MATLAB 顯示分析
如要使用建議可以參考新版本的程式 http://kitsprout.logdown.com/posts/883899

[本文]
更新改良之前寫的 MATLAB 示波器功能，可直接透過藍牙連接，一個封包 2N + 4 byte，每個封包帶 N 個 int16 的資料。

MATLAB 部分沒有特別做優化，目前 250Hz 沒有問題，更新頻率更高會因為電腦 UART 或 MATLAB 方面等問題，顯示會變得不流暢，另外之前一直遇到隨時間增加導致延遲的問題暫時還無法解決，本來以為是因為儲存資料太多，但經過實驗後確定不是此問題，應該是 figure 不斷更新導致的，250Hz 下約 10000 筆資料後就會產生明顯延遲，所以目前無解，這次更新加入了自動找 Serial COM Port 的功能，可以自己尋找可用的 COM Port，減少設定的步驟，還有 packet loss rate 的檢測，速度快的時候大概丟包率大約 10%，有點大...，調低更新速率應該可以明顯降低丟包率，但拿來做實驗已經很夠用了。

目前比較常拿來用在感測器的資料分析與演算法的驗證上，像是加速度計、陀螺儀等。

連接方式：
只要透過 UART 轉 usb 直接連接到電腦，有 COM Port 就可以傳輸了，個人是比較常使用單晶片接藍牙從機 + 筆電藍牙功能來做實驗，不需要另外再接線到電腦。

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

packetLens = int16Lens * 2 + 4

byte[0] = 'S'
byte[1] = data_01_H8
byte[2] = data_01_L8
byte[3] = data_02_H8
byte[4] = data_02_L8
byte[5] = data_03_H8
byte[6] = data_03_L8
...
byte[packetLens - 5] = data_n_H8
byte[packetLens - 4] = data_n_L8
byte[packetLens - 3] = check sum
byte[packetLens - 2] = '\r'
byte[packetLens - 1] = '\n'

close all
clear all

% ***********************

% **** set parameters

%comPort = 'COM5';

baudRate = 115200;
y_axisMax =  16000;
y_axisMin = -16000;
window_width = 800;
recvDataLens = 24;      % recvDataLens = int16Lens * 2 + 4

% ***********************


if rem((recvDataLens - 4), 2) > 0
    error('ERROR : recvDataLens must = int16Lens * 2 + 4');
end

% Auto find available serial port

delete(instrfindall);
info  = instrhwinfo('serial');
comPort = info.AvailableSerialPorts

% Serial config

s = serial(comPort, 'BaudRate', baudRate, 'DataBits', 8, 'StopBits', 1, 'Parity', 'none', 'FlowControl', 'none');
s.ReadAsyncMode = 'continuous';
fclose(s);
fopen(s);

% Init data

window_w = window_width;
window_d = window_w * 0.9;  % display window

window = -window_d;  
runtimes = 0;
recvData = 0;

fig1 = figure(1);
set(fig1, 'Position', [200, 100, 1600, 700], 'color', 'w');  % 1920*1080

signal = plot(runtimes, recvData);
axis([window, window + window_w, y_axisMin, y_axisMax]);
xlabel('runtimes');
ylabel('data');
grid on;
hold on;

state = 0;
%for i = 1 : 1000

while ishandle(fig1)
    n_bytes = get(s, 'BytesAvailable');
    if n_bytes == 0
        n_bytes = recvDataLens * 2;
    end
    recvData = fread(s, n_bytes, 'uint8');
    data_s = find(recvData == 83);

    dataNum = size(data_s, 1);
    if dataNum >= 1
        i = 1;
        while (i) & (dataNum >= 1)
            if (n_bytes - data_s(dataNum)) < recvDataLens
                dataNum = dataNum - 1;
            else
                i = 0;
            end
        end
    end

    if dataNum >= 1
        count = 0;
        for i = 1 : dataNum
            if (recvData(data_s(i) + recvDataLens - 2) == 13) & (recvData(data_s(i) + recvDataLens - 1) == 10)
                check = sum(recvData(data_s(i) + 1 : data_s(i) + recvDataLens - 4));
                if rem(check, 256) == recvData(data_s(i) + recvDataLens - 3)
                    count = count + 1;
                    runtimes = runtimes + 1;
                    for j = 2 : 2 : recvDataLens - 4
                        dataCov(j/2) = unsigned2signed(recvData(data_s(i) + j - 1) * 2^8 + recvData(data_s(i) + j));
                    end
                    data(:, runtimes) = transpose(dataCov);
                    state = 1;
                end
            end
        end

        if state == 1
            window = window + count;
            displayData = runtimes - window_d + 1;
            if displayData > 0
                times = [displayData : runtimes];
                plot(times, data(2, displayData : end), 'r', times, data(3, displayData : end), 'g', times, data(4, displayData : end), 'b');
            else
                times = [1 : runtimes];
                plot(times, data(2, :), 'r', times, data(3, :), 'g', times, data(4, :), 'b');
            end
            axis([window, window + window_w, y_axisMin, y_axisMax]);
            drawnow
            state = 0;

            fprintf('runtimes = %d\n', runtimes);
            fprintf('Acc.X = %d, Acc.Y = %d, Acc.Z = %d\n', data(2, end), data(3, end), data(4, end));
            fprintf('Gyr.X = %d, Gyr.Y = %d, Gyr.Z = %d\n', data(5, end), data(6, end), data(7, end));
            fprintf('Mag.X = %d, Mag.Y = %d, Mag.Z = %d\n', data(8, end), data(9, end), data(10, end));
        end
    end
end

fclose(s);
delete(s);

data = double(data);

% check packet loss rate

check = data(1, 2 : end) - data(1, 1 : end-1);
pl = 0;
for i = 1 : runtimes - 1
    if check(i) >= 2
        pl = pl + 1;
    end
end
plr = pl / (runtimes - 1) * 100;
fprintf('\ntotal packet = %d, packet loss = %d, rate = %.4f %%\n', runtimes, pl, plr);

void Serial_SendDataMATLAB( int16_t *sendData, uint8_t lens )
{
  uint8_t tmpData[32] = {0};  // tmpData lens >= 2 * lens + 4
  uint8_t *ptrData = tmpData;
  uint8_t dataBytes = lens << 1;
  uint8_t dataLens = dataBytes + 4;
  uint8_t count = 0;
  uint16_t tmpSum = 0;

  tmpData[0] = 'S';
  while(count < dataBytes) {
    tmpData[count+1] = Byte8H(sendData[count >> 1]);
    tmpData[count+2] = Byte8L(sendData[count >> 1]);
    count = count + 2;
  }
  for(uint8_t i = 0; i < dataBytes; i++)
    tmpSum += tmpData[i+1];
  tmpData[dataLens - 3] = (uint8_t)(tmpSum & 0x00FF);
  tmpData[dataLens - 2] = '\r';
  tmpData[dataLens - 1] = '\n';

  do {
    Serial_SendByte(*ptrData++);
  } while(--dataLens);
}

int main( void )
{
  int16_t testLostRate = 0;
  int16_t IMU_Buf[10] = {0};

  while(1) {
    MPU9250_getData(IMU_Buf);
    IMU_Buf[0] = testLostRate++;
    Serial_sendDataMATLAB(IMU_Buf, 10);
    Delay_1ms(4);
  }
}

之前實驗的影片：

Github Link : https://github.com/KitSprout/WheelLED

　　本作品由裝在車輪上打氣孔的 LED 燈而發想的，並結合電影 ＂創：光速戰記＂ 裡的機車照明設計，構想了一個車輪燈照明的設計，但在實現的過程中發現市面上已有類似照明概念的車燈 Revolights，所以在製作上我們的設計了有別於 Revolights 的機構來固定 LED，同時結合了車輪車燈在照明方面可調整照明仰角的特色，加入穿戴式裝置手套來控制，並針對個人騎乘自行車時所遇到的問題與憂心處做解決方案，從而設計出此作品。目前基本照明功能都已實現，因現於製作時間的限制，仍有許多的想法功能尚未實現，但已具有一定的完程度，與驗證其製作上的可行性，相信這會是一個改善騎車體驗、增加行車安全的作品！

一、作品簡介

　　隨著全球健康意識與環保意識抬頭，自行車運動不僅減少二氧化碳的排放，同時長時間騎乘自行車也屬於有氧運動，可以有效消耗脂肪與熱量，增加騎乘者的運動量與肺活量，但普遍的自行車在行車安全與財產安全方面的並沒有像汽機車等齊全及完善。

　　在 2010 年間，美國卡內基美隆大學的學生提出了Project Aura [1][2] 專案，改善了在夜間行車安全方面的問題，但仍然需要裝前方的照明與後方的警示燈，為了更加完善與改善自行車在夜間的行車及財產方面的安全，在 Project Aura 與 Revolights 的基礎上，構想了一個除了原本的功能外，還可以自動或手動調整照明亮度、照明傾角，且具有剎車警示功能的自行車車燈，並搭配穿戴式的自行車手套來連結人與車，達到人車互動以及防盜的效果。

二、作品特色

本作品主要有以下特色：

• 安裝簡潔 → 實際的照明與控制部分如下圖所示，本作品將照明裝置、感測器、控制系統全部的裝置都集中安裝在車輪內框與車輪軸心上（紅色部分），車輪外不需要安裝任何東西，捨棄傳統旋轉編碼器測量車輪轉速的方案，改以加速度計與陀螺儀等感測器為基礎的演算法來估測當下車輪的旋轉速度與角度，藉此減少整體安裝的複雜程度，使自行車整體架構更為簡潔。

• 前輪可調照明亮度與仰角 → 可手動調整照明亮度與照明仰角來為騎乘者提供更好更安全的行車體驗。細部安裝架構與照明示意圖如下圖所示，藍色與綠色部分為固定 PCB 與 LED 燈的機構，機構為自行設計並採用 3D Printer 印製(Wayne設計)，以便易於相容不同規格的輪胎，讓更多的使用者不需購買特定規格的輪胎即可使用本產品。黃色部分由數顆 LED 燈組成，LED 燈照明方向從輪胎旋轉軸心向輪胎外胎照射，透過車輪上感測器的回授轉速與角度來控制 LED 燈亮暗的時機，藉此實現視覺暫留與固定照明方向的效果。

• 後輪煞車燈警示 → 後輪結構、硬體裝置與前輪相同，透過車輪上的感測器來感測自行車車輪的轉速，當感測到後輪車輪轉速變化大於預先設定數值時，後輪的車燈會顯示出對應的訊息與燈號來警示後方車輛，減少後車因自行車減速而造成後方車輛追撞問題。

• 停車智慧省電 → 自行車照明是必須從騎乘開始到結束都必須可以正常工作的裝置，若在行車途中突然沒電或是故障，對於夜間行車安全是一大問題，所以在節能部分利用停車的空檔，調小或關閉車燈，藉此延長行車的照明時間。

• 傾斜振動警示 → 當自行車子與使用者間距離超過特定距離時，且車體發生不尋常的傾斜或震動，表示可能車子正在遭竊或是被撞到撞倒，此時車上的裝置將會發送警示訊息，以告知使用者自行車有財產安全上的疑慮。

• 穿戴式裝置的人車互動 → 穿戴式裝置設計如下圖所示，將電路與控制器結合到自行車手套上，裝置分成兩部分，一部分（綠色）放置在手腕上，主要由振動馬達與微控制器組成，另一部分（藍色）則放置在食指上，由感測器與按鍵（紅色）組成，也可以依需求自行增加感測器在拇指、中指等地方。兩個車輪與穿戴式裝置之間透過低功耗藍牙（Bluetooth Low Energy, BLE）做連結與通訊，並且可以透過白名單的形式，授權給第三方穿戴式裝置使用者，像是朋友、家人… 等等，另外由 BLE 的服務特性，可以很容易使用智慧型手機、手錶等裝置來替代原本的裝置，此特性可以增加使用者的選擇。自行車結合穿戴式裝置，礙於騎乘時的行車安全，穿戴式裝置上不採用顯示螢幕，反而是透過裝置的振動與LED燈亮暗、顏色變化來傳遞訊息，像是電池電量過低、溫度過高、提醒補充水分、使用者與自行車相距距離太遠、自行車不安全等情況，都會透過上述方法告知使用者。使用者也可以透過感測器來感測手部動作，藉此來控制自行車的車燈、控制模式等功能，為防止騎乘自行車的動作誤下指令，設計了按鍵在食指旁，藉由母指按按鍵來進入體感操作。

三、系統架構

作品系統架構主要可以分成三大部分
第一部分 WheelLED-Light，負責控制 LED 亮度角度等資訊。→完成度高
第二部分 WheelLED-Core，用來計算角度、速度等資訊來控制 WheelLED-Light，實現定向照明的效果。
第三部分 WheelLED-Wearable，用來體感控制車燈的照明亮度、角度、範圍。

四、使用情境

• 情境 A － 行進與停止
  
  自行車行進時，前後車輪車燈會自動亮預設的照明仰角、亮度；當停下時，會進入到預先設定的停車燈示，預設的停車燈示為向自行車下方照射，並降低亮度，藉此減少直射前方車輛之狀況，以及減少電量消耗。

• 情境 B － 煞車或減速
  
  當自行車在行進時速度減緩，後輪車燈會自動將減緩的速度變化量直接表現在車燈的亮度上面，也就是說當車速變化越明顯燈會越亮，藉此使後方駕駛更容易了解傳遞的煞車訊息。

• 情境 C － 照明亮度、仰角調整
  
  目前設定了兩種手勢來調整照明亮度與仰角，將手食指指向行車方向，並按下按鍵，左右旋轉手腕，可以調整照明的亮度；將手指食指保持與行車方向垂直，並按下按鍵，上下旋轉，則可以調整車燈的亮度。相較於透過螢幕與按鍵操作亮度與仰角，透過體感控制的方法，可以有效減少在行車間注意力分散的問題。

• 情境 D － 短暫離開自行車
  
  在需要短暫離開自行車時可以透過體感方式，鎖定自行車，使自行車進入安全模式，其方法為將食指指向地上，按下按鍵，順時針或逆時針旋轉至水平，若車燈同時閃爍，表示鎖定完成；若要解鎖，則與鎖定動作相反，由水平旋轉至垂直，車燈會連續閃爍兩次，表示解鎖完成。

• 情境 E － 與自行車距離太遠
  
  在短暫離開自行車的情況下（安全模式），穿戴式裝置上的藍牙會透過接收訊號強度（RSS）來，來判斷與自行車距離是否會太遠，若超過預先設定的距離時，穿戴式裝置會讓振動馬達連續短震，直到距離小於設定距離，藉此警告使用者，超過距離自行車可能不安全。

• 情境 F － 離開自行車時車體傾斜
  
  在安全模式下，若車體發生不尋常的傾斜或震動，表示可能車子正在遭竊或是被撞、撞倒，此時穿戴式裝置會讓振動馬達連續一長震兩短震震動，並且會閃爍紅色LED燈，藉此告知使用者自行車有財產安全上的疑慮。

• 情境 G － 建議補充水分
  
  穿戴式裝置會自動在騎乘時計時騎乘時間，若騎乘時間超過預先設定的時間時，裝置會閃爍綠色 LED 燈，並長震馬達 4~6 次，告知使用者需要補充水分，防止使用者在無意間因流汗過多而導致脫水。

五、實際操作

最小二乘法又稱最小平方法，是一種數學方法，透過最小的誤差平方和來找出最佳的參數或一些問題，工程上應該算是系統判別裡一門學問。最常見的應用應該就是在迴歸分析或稱為擬合（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）的模型參數的方法。

安裝 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 ... 等等，頗方便的。

移到桌面（我是存在桌面上）
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 編輯。

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

UDOO SHOP → http://shop.udoo.org/other/neo.html

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

平均法是濾波上最容易實現的一種方法之一，常常被拿來應用，像是每 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 的運算量在不同長度下將不會變，效率應該也高於前者。

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

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

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

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

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

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

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

上式反推得到下列關係，

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

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

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

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

慣性測量單元（Inertial Measurement Unit, IMU）捕捉物體運動的資訊、測量物體姿態的一種電子元件，又稱為慣性感測器，主要由陀螺儀（Gyroscope）與加速度計（Accelerometer）組成，部分的應用中有時也會加入磁力計（Magnetometer）與氣壓計（Barometer）來輔助估算相關訊息，經常使用在載具的導航上面。

在四軸上面四種感測元件主要的功能：

• 陀螺儀 → 通常做為主要姿態的計算，優點是反應快，但缺點是誤差容易累積
• 加速度計 → 可以透過重力加速度來校正陀螺儀，運動加速度部分也可以用來計算位移與速度等
• 磁力計 → 同樣也是輔助、校正陀螺儀，解決重力加速度無法檢測到水平角度旋轉的問題
• 氣壓計 → 計算飛行器高度
  

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

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

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

在說明之前，我們先定義兩個座標系統，世界座標系G以及載體座標系L，世界座標系為地球座標，載體坐標系對準載具，感測器因為放置在載具上面，所以讀取的數值都是相對於載體座標系，表示如下面，除非不同或是需要注意才會再行標示。

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

陀螺儀（Gyroscope or Gyro）

陀螺儀是測量角速度（Angular velocity）的元件，基於角動量守恆的理論而設計出來，在物體姿態運算中佔有非常重要的地位，陀螺儀具有短時間準確、靈敏的特性，但誤差會隨溫度與時間變化，長時間下姿態的計算，誤差累積會使其漸漸變的不可靠，所以需要透過其他感測器來輔助校正，通常我們以 dps（degree per second）來表示角速度，角速度與速度不同，角速度是單位時間內旋轉的角度，而速度則是單位時間內移動的距離，所以若是沒有旋轉是沒有角速度的，若只對 X 軸旋轉，則 Y 軸與 Z 軸的輸出為 0，X 軸的輸出為旋轉的角速度。

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

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

理想的角速度 y_g,c 是我們想要的，所以校正的動作其實就是把 S_g、b_g 與 n_g 求出來並去除。

加速度計（Accelerometer or G-Sensor）

加速度計是測量加速度（Acceleration）的元件，除了載體的運動加速度外，也包含地球質量所產生的重力加速度，當感測器自由落體時，運動加速度與重力加速度會互相抵銷，感測器三軸輸出合為零，通常以 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 則可以得到傾斜角度，但需要注意的，這僅限於靜止下，當有運動加速度的影響，得到的角度會是不準確的。

下面是一個加速度計的簡單數學模型，與陀螺儀類似（其實大家都長得差不多），加速度計的輸出 y_a 會等於矩陣 S_a 乘上理想的加速度 y_a,c 加上偏移 b_a 與雜訊 n_a，理想的加速度 y_a,c 包含線性加速度 a 以及重力加速度 g，矩陣 S_a 包含未對準的誤差以及單位的轉換（g/LSB），若感測器與載具完全對準的情況，矩陣 S_a 也是一個對角矩陣，偏移 b_a、雜訊 n_a 則也會由溫度、感測器內部等干擾引起。

磁力計（Magnetometer or Compass）

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

下面以二維平面為例，將磁力計X軸與Y軸經歸一化後的資料在不同影響下畫出來的圖，圖(a)為理想、不受干擾的磁場，是一個圓心在原點的圓形，表示直接獲取水平方向的夾角時不會有誤差，圖(b)是受到硬磁干擾的磁場，也是圓形分布的磁場，但整體的磁場強度會被施加一定值，導致圓心編離中心，圖(c)是受到軟磁干擾的磁場，會讓磁場方向改變產生形變，使得圓型變成橢圓，但圓心仍在中心，圖(d)是同時受到硬磁與軟磁干擾的情況，偏離中心的橢圓形，這也是大部分磁力計測到的圖形，如果在不校正成圖(a)的理想情況下，透過三角函數來計算水平方向角度就會產生誤差，誤差隨干擾程度增加，同理，應用於三維空間中，三軸磁力計輸出繪製出之圖形將會變成橢球分布，校正目的則是將橢球校正回圓球。

下面是一個磁力計的簡單數學模型，磁力的輸出 y_m 會等於軟磁干擾矩陣 S 乘上理想的磁場 y_m,c 加上硬磁干擾偏移 H 與雜訊 n_m。

氣壓計（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

上一篇 → 從零開始做四軸 (四) - 飛行器組成
下一篇 → 從零開始做四軸 (六) - 感測器校正