KitSprout

March 26, 2016

【New Design & Test】2016.03.26

這次送了五款 PCB，其中包含了一個全新的設計，暫時命名為 IMUCube v1.0，另外幾個不同的版本 SmartBLE 1.2 改良測試，以及上架版本的SmartIMU v2.0、MicroMultimeter v2.0和OLED_0.96-inch v2.0，最近常送的 PCB 廠真的做得很慢... 這次花了大概 10 天才做好，讓很是期待這次送廠的我等到快瘋了，幸好最後順豐的神速遞送，24HR 左右就從深圳運回來了，剛好在星期五前拿到，不然就得等到下禮拜一了。

↑ 第一片是 SmartIMU v2.0 的上架版本，修改了上次的輸出失誤，焊好驗證過沒問題就會開始上架販售，提供給想應用的使用者。

↑ 第二片是 MicroMultimeter v2.0 的上架版本，這片也不小心忘了 V-Cut，變成必須要透過裁板機才能分開...此片修正主要調整分壓電組的部分，之前的設計無法測量短路與斷路，驗證過沒問題也會開始上架販售。

↑ 第三片是 OLED_0.96-inch v2.0 的上架版本，主要調整分壓電組的部分，之前的設計無法測量短路與斷路，驗證過沒問題也會開始上架販售。

↑ 第四片主要測試不同版本的 SmartBLE 1.2，有基本與加放大的原始版本，另外加入了 IPEX 座的版本與 nRF52832 的版本，不過目前因為還沒有購買 nRF52832，所以這部分暫時不做測試，最後就是有加入一個 SmartIMU 的電池底座，附加一個按鈕，方便測試與應用。

↑ 第五片是 OLED_0.96-inch v2.0 的上架版本，主要調整分壓電組的部分，之前的設計無法測量短路與斷路，驗證過沒問題也會開始上架販售。

IMUCube v1.0

Github Link：https://github.com/KitSprout/IMUCube

IMUCube 是一個 IMU 與 LED 結合的模組，由 STM32F103T8(微控制器)、MPU6500(三軸加速度計、三軸陀螺儀)、WS2812B(RGB LED) 組成，裡面預留了一個 ADC 來測量電池電壓，並拉出 UART 與 SWD 來做開發，目前將先以電子骰做第一種 IMU 與 LED 的結合應用。

March 14, 2016

【BOOK】最近買了幾本書來投資自己

最近這幾個禮拜買了兩本剛出版沒多久熱烘烘的簽名新書來投資自己

第一本是＂無刷直流 BLDC 馬達控制實務：使用 Atmel SAM C21 ARM Cortex-M0+ 控制核心＂，二月底出版的，打算補充一下 BLDC 的相關技術，為之後的打算實現的 QCopterESC 做準備。

拿到的時候快速看了一下，有點小失望，原本以為數學會多一點的，沒想到程式居多，這本書應該要搭配官方推出的開發套件會比較有價值，邊看邊做應該很容易上手，不過小小的窮學生，沒有閒錢買幾千幾萬的套件，只好先買本書來過過癮...但書中也有部分的章節頗有興趣的，像是啟動、轉速計算與校準以及電動車相關，不過最近也在找無感 BLDC FOC 的相關書籍，但像這種的書籍似乎不多。

　
第二本書是＂SolidWorks專業工程師訓練手冊[1]：基礎零件篇＂，三月多出版的，這本等了一年快兩年了，為了讓自己可以在設計模組與實現功能有更多的變化，打算學習機構設計，原本是預計搭配 3D Pinter 來做練習的，不過計畫總是跟不上變化，存的錢都拿去買開發板和材料了，也懶得自己裝一台，因為故障壞了還必須要自己修，費時又費力...所以現在就單純的透過軟體與書上的範例來學習，若有需要做出來的就直接送外面或找同學印。

這本書很多地方寫得蠻詳細的，詳細到覺得沒有必要...可能我比較喜歡精簡的書搭配練習題來學習吧，這本書雖然標榜可以讓初學者自學，但部分指令功能感覺都是在使用者大概了解介面與位置的前提下做教學的，需要花些時間來找，不過熟悉一些應該就不會是問題，今天花了個小時的時間把基本的草圖、填料功能了解以及習慣 SolidWorks 2015 的操作介面，整體還學的蠻順利的。

另外這本書還有配套的 youtube 教學影片與 ppt，還沒有看過，之後有時間可以看看。
Youtube : https://www.youtube.com/user/sonyic08

March 6, 2016

【New Design & Test】2016.03.06

過年期間花了一個禮拜左右的時間新設計與更新了五款模組，有兩款屬於全新的設計，暫時命名為 QCopterRC-S v1.0 與 MiniMobileRobot v1.0，目前還在測試調整中，另外三款則是從之前設計的舊版本升級更新上來，分別為SmartIMU v2.0、SmartBLE v1.2 和 MicroMultimeter v2.0、因為年間 PCB 廠休息到 2/15，積了不少訂單，所以這次等了特別久，直到 3/1 才拿到 PCB 並把它們焊完測試、驗證。

送了四大片 PCB，製作的品質感覺比之前差，可能是剛開廠需要暖身吧...，但還是算便宜，反而比較訝異的是四層板的價格，比幾年前送的價格便宜許多，試算一下 100x100 mm 10 片大概 1000 台幣左右，看來以後可以多嘗試四層板設計來減少面積與與增加穩定。
　
↑ 第一片是 SmartIMU v2.0 的正式版，採用四層板設計，厚度 1.0 mm，之前沒有先製作過測試的版本，因為測試的成本頗高所以就風險一下，拿到板子核心部分實測過基本都沒有問題，可以正常運作，不過外部那國王的 SWD 燒錄杜邦母座真的是太誇張了... 居然整個不見，連鋪銅也更新過，一千多元就這樣浪費掉了，一開始還以為是板廠的問題，再檢查過 Gerber 才驚訝到，做過太多沒有問題的板子反而讓我對輸出前檢查更為輕率了，就當個教訓吧...

↑ 第二片是 uMultimeter v2.0 的測試版本，採用雙層板設計，厚度 1.6 mm，除了本體外，還包含了擴充的模組、螢幕，除了電表探棒的擴充安裝後的感覺不是很喜歡外，其他都算滿意。

↑ 第三片是 SmartBLE v1.2 和 MiniMobileRobot v1.0 的測試版本，採用雙層板設計，厚度 1.0 mm，SmartBLE 基本的燒錄都正常，與上一版本無異，不過燒錄 BLE 的 Softdevice 後卻不能運作，之前版本焊的三片都沒問題，這次版本焊的三片都出問題，不知道是焊接問題還是設計問題，目前還要 debug 才會知道，另外加 PA 的 BLE 目前還是沒辦法測試，因為鋪銅地的熱容太大，PA IC 難以用焊槍焊上去，只能等之後買熱風槍或是下一版本調整才能測試，而 MiniMobileRobot 的部分，目前焊好可以正常燒錄，還沒有實際跑過，應該是沒有問題，唯一可惜的是馬達的驅動做得太急了，只放了一個 MOS，忘記要設計成正反轉...

↑ 最後一片是 QCopterRC-S 的測試版本，採用雙層板設計，厚度 1.6 mm，基於之前文章裡的構想 STM32F469I-DISCO 開箱設計出來的，因面積成本問題，所以特別縮水一下，先測試腳位的連接與電池的放置以及基本控制器的功能。

這次花了一個禮拜的時間更新不少的東西，有點急，不少的細節都沒注意到，除了 SmartIMU 很遺憾外，其他都還可以接受，畢竟是用來測試問題的版本，下面將更詳細的說明製作的內容，如果想看更多的 PCB 或之前設計的 PCB 圖片可以到下面連結。

more photo...
　
　

SmartIMU v2.0

Github Link：https://github.com/Hom-Wang/SmartIMU

SmartIMU 是一個集成微控制器 (STM32F411C)、3-Axis 加速度計、3-Axis 陀螺儀、3-Axis 電子羅盤與氣壓計 (MPU9250、LPS25H) 於一體的 10DOF 模組，可以直接透過 SPI/I2C 來獲取原始感測器資料，也可以透過另外的 SPI、I2C、UART 或是 USB 來讀取微控制器處理好的資訊，像是載具的角度、加速度、速度、位移、高度 ... 等等，除了上述功能外，也預留的了十幾個 I/O 與其對應功能來做擴充的應用， QCopterNano 即使用 SmartIMU 作為飛控。

SmartBLE v1.2

Github Link：https://github.com/Hom-Wang/SmartBLE

MicroMultimeter v2.0

Github Link：https://github.com/KitSprout/MicroMultimeter

MicroMultimeter 是一個多功能的數位電表，著重於簡單、攜帶以及擴充性，以應付日常電路、微控制器開發除錯及測量為主要目的。MicroMultimeter 同時也是一個開源硬體、軟體的一個項目，相關的資源都可以從 GitHub 上取得，基本功能有電壓測量、二極體測量、短路斷路測量、PWM 輸入輸出、波形顯示，同時也可以透過 USB VCP 功能，將訊號傳送至 PC 端處理與儲存，另外搭配其包含 UART、SPI、I2C、PWM 的擴充埠，讓配件模組化，藉此可再增加更多樣化的應用及延伸，像是溫溼度計、轉速計、無線示波器甚至是邏輯分析功能。

遙控器 QCopterRC-S v1.0

Github Link：https://github.com/QCopter/QCopterRemoteControl-S

QCopterRC-S 是基於之前設計的 QCopterRC 的一個分支，以現有的開發板 STM32F469-DISCO 加上另外設計的 Shield 來實現 QCopterRC 的功能，QCopterRC-S　由核心控制與按鍵搖桿兩部分組成，兩部分透過排針與母座連接，核心控制部分有微控制器(STM32F411C)、6-Axis 感測器(MPU6500)、SmartBLE/nRF24L01 模組以及穩壓模組，按鍵搖桿部分則由不同的可變電阻與按鍵組成。

微控制器從感測器、按鍵、可變電阻、電池電壓讀取資訊、處理，再透過 UART 傳給 STM32F469-DISCO 做應用，或是透過無線傳輸模組做遙控，可以獨立實現無螢幕的遙控器，另外 STM32F411 與 STM32F469-DISCO 共用無線傳輸模組、感測器和 UART，所以也可以透過 STM32F469-DISCO 直接讀取感測器與模組，不須再經由 STM32F411 做轉接。

QCopterRC-S 預計之後會透過 3D 印表機製作外殼，讓電池與機構的整合可以更好，也讓使用的體驗可以更完善，當然也讓自己可以有機會往機構設計與製作方面學習與實踐。

more photo...
　

MiniMobileRobot v1.0

Github Link：https://github.com/KitSprout/MiniMobileRobot

MiniMobileRobot 是看到現成的小車而設計的，因為現成販售的小車只有馬達與機構，沒有控制器，所以就設計了一塊底板，帶微控制器(STM32F030F)與馬達驅動電路，MiniMobileRobot 上面除了微控制器與驅動外，還設了一個開關與電池電壓的回授，並拉出四個包含 UART 與 I2C 的 I/O，方便連接感測器、藍牙模組等。

↑ 從淘寶購買的小車原型，約 85 人名幣。

↑ 小車背面。

↑ 小車正面。

more photo...

March 6, 2016

【Works】SmartIMU v2.0

Github Link：https://github.com/Hom-Wang/SmartIMU

從 SmartIMU 的第一版到現在也已經隔了兩年左右，讓我們先來回顧之前設計的 SmartIMU 吧，
之前設計和實驗的 hackpad 紀錄：https://hom.hackpad.com/SmartIMU-E9zzbSI7QyK

SmartIMU 一開始是設計成雙層板(v1.0, v1.1)，後來因為腳位配置與操作的問題所以改成四層板(v1.2)，並且在 v1.1的時候加入測試底座來測試半孔 PCB，SmartIMU 是自己設計的第一款四層板，也是目前設計密度最高的開發板、最滿意的作品之一，整體的材料成本大約在台幣 300 左右，只是焊接具有一定難度就是了。
　

SmartIMU v2.0

這次新版本的 SmartIMU v2.0 仍然採用四層板設計，且腳位相容於之前的版本，依功能可分成兩部分－核心與擴充，核心部分可透過裁板機分離成貼片半孔板，大小約 12x15 mm，集微控制器與感測器，而擴充部分則是結合之前設計的測試底板，為方便測試與實驗所製作的，也是為了方便製作上而改良的，大小約 25x25 mm，可以直接插在麵包板上做實驗。
　
　
SmartIMU v2.0 更新改良的地方：

微控制器從 STM32F401 換成 STM32F411，增加運算的效能
感測器從 MS5611 換成 LPS25H/B，增加氣壓計的分辨率與減少體積
LED 的數量增加至 3 個，並改成 RGB1212 的 LED 燈
有緣震盪器 24 MHz 改成無源振盪器 16MHz，方便進入低功耗模式及系統時脈的計算
感測器連接將以一個 SPI 與兩個 CS 完成，使得外部直接讀取感測器也可以直接讀取到氣壓計
將測試底座與核心板結合，減少製作上的複雜度與成本
可以與 SmartBLE 連接、結合，加入電源開關、電池與 BLE 傳輸功能

↑ 因輸出有問題，所以只有核心部分可以使用...

↑ 左邊是版本 v1.2 有沉金，右邊版本 v2.0 沒有沉金，體積不變，效能增加。

↑ 因為輸出錯誤，所以只能拿之前的概念版本來做連接的示意，下面是 SmartBLE，帶有電池與開關。

2016/04/01 更新－預計販售版本，並新增電池底座 SmartIMU-BAT。

more photo...

March 6, 2016

【Works】SmartBLE v1.2

Github Link：https://github.com/Hom-Wang/SmartBLE

SmartBLE 是一個藍牙 4.1 模組，有 SmartBLE 與 SmartBLE-PA 兩種版本，皆採用 ARM CortexM0 nRF51822，最高工作時脈 16MHz，相容與 nRF24L01P 傳輸，板子分成核心與擴充兩部分，兩者之間透過 1.27mm 孔分開，核心板拉出了十幾個 I/O，可以實現 UART、SPI、I2C、ADC 等功能，擴充板部分則設有三個 LED 與一個 KEY 來方便使用者操作與應用，電源方面加入了 CR2032 的 3V 電池座以及開關，加強與增廣 SmartBLE 的應用情景，最後在擴展部分設計與 SmartIMU 相容的腳位，可直接與 SmartIMU 連接、通訊。

未來改版打算升級成 ARM CortexM4 nRF52832，並加入室內定位的演算法來提供定位的服務。
　
↑ 核心示意圖，左為 SmartBLE-PA，右為 SmartBLE。

↑ SmartBLE 實體正面。

↑ SmartBLE 實體背面。

↑ 用來測試 SmartIMU 與 SmartBLE 的底板，同時也可以拿來幫助 SmartBLE 固定。

more photo...

March 5, 2016

【Works】MicroMultimeter v2.0

Github Link：https://github.com/KitSprout/MicroMultimeter

MicroMultimeter 是一個多功能的數位電表，著重於簡單、攜帶以及擴充性，以應付日常電路、微控制器開發除錯及測量為主要目的。MicroMultimeter 同時也是一個開源硬體、軟體的一個項目，相關的資源都可以從 GitHub 上取得，基本功能有電壓測量、二極體測量、短路斷路測量、PWM 輸入輸出、波形顯示，同時也可以透過 USB VCP 功能，將訊號傳送至 PC 端處理與儲存，另外搭配其包含 UART、SPI、I2C、PWM 的擴充埠，讓配件模組化，藉此可再增加更多樣化的應用及延伸，像是溫溼度計、轉速計、無線示波器甚至是邏輯分析功能。
　
MicroMultimeter 的設計理念是「把實驗室帶回家」，不過當然不是把高頻、高能等的實驗帶回家，而是把基本的功能與需求實現出來，著重於為控制器的開發除錯，透過 MicroMultimeter 來替代電源供應器、示波器、訊號產生器、三用電表...等儀器，降低家中實驗的儀器成本與門檻。

MicroMultimeter v2.0

使用了幾個月自製的電表後，了解了之前設計的問題與瑕疵，所以這次新版本的 MicroMultimeter v2.0為了更好的改善使用體驗，只保留了一開始大致的功能構想與規劃，完全重新設計電表本體、OLED 螢幕以及擴充模組，整體面積約 32x32 mm，透過 Micro USB 來供電，本體的材料成本大約台幣 350 左右。

這次的更新除了不同的布線與布局外，主要還更改了以下幾項：

微控制器從 F103CB 更換成 F303CB，升級成 CortexM4，同時也增加了不少的功能，像是 DAC、OP、比較器...等
將擴充埠縮至螢幕下方，讓整體體積縮小，從原本長方形變成正方形
調整擴充埠的腳位，增加了更多的 I/O，但同時也少了定流電路的數量
修正了按鍵的設計錯誤的問題
新增了一個麵包板擴充模組的原型

測試版本的實體圖片

↑ uM 含 OLED 模組正面

↑ uM 不含 OLED 模組正面

↑ uM 背面

↑ 自己設計的幾個介面，目前直接從上一版本移植過來，沒有改太多東西

↑ GPIO 的擴充，拉出 16 個 I/O

↑ Probe 的擴充，連接電表探棒，新的版本加入了 BJT 來實現自動切換測量模式，但外型還需要再改善...

↑ 麵包版擴充，目前只是測試原型，只有電池供電與 I/O 的功能，之後有好的想法會再加入

more photo...

February 9, 2016

從零開始做四軸飛行器 (六) - 感測器校正

從零開始做四軸系列離上一篇大概有四個月了，本來是打算在第六部分寫飛行器模型的，不過只敘述模型不加控制器感覺沒甚麼內容，也就是普通的旋轉加直線運動方程式而已，但加控制器的方法又有多到數不清，而且要透過 MATLAB 來驗證又要花上不少時間寫程式，幾乎難產，所以就暫時先插入已經完成的感測器校正部分，之後看飛控板和遙控器更新改板的進度再來調整飛行器模型篇的位子吧。

這裡的感測器校正不探討溫度對於感測器在誤差方面的影響，因為沒有特別用溫度來校正過 IMU 相關感測器，不確定在溫度產生的誤差方面對於校正是否有顯著或是影響大的結果，之後如果有空來測量不同的工作時間長度下，芯片工作溫度的變化後，或許可以評估一下是否有這些必要，但目前就先假設感測器的溫度固定，不會因為外在或內部的溫度而影響噪音的大小。

陀螺儀（Gyroscope or Gyro）校正

對陀螺儀校正的部分主要校正偏差（Bias）的誤差，不對安裝誤差做校正，因為對於安裝物差的校正需要一些＂絕對＂的測量數據，這些數據通常都需要外部的儀器來輔助校正與測量，舉一個簡單的例子，今天我想要找出下列方程式中的 S_g 矩陣，找出這一個矩陣的一個方法就是在不同的角速度下測量，外部的裝置可以產生一個固定的角速度給予感測器，或是透過另一個可靠的感測器來做提供實際的結果（可能有些人會想為什麼不直接用感測器的輸出？因為感測器的輸出是包含誤差的，所以才需要被校正，如果感測器沒有誤差當然沒有問題，可以使用，不過就不需要做校正了...），而感測器將測量到的結果與實際的結果作比較，就可以得出 S_g 矩陣，那不用外部的儀器可做嗎？可以，如果有人可以產生一個固定或是準確的角速度，就可以透過這個角速度來校正陀螺儀，不過個人是沒辦法實現這高難度的動作，只能先假設 S_g 矩陣是一個單位矩陣或是一個對角矩陣（Diagonal Matrix）。

$\large \begin{align*} &y_{g}=S_{g}\,y_{g,c}+b_{g}+n_g,\\ & where\; y_{g} = \begin{bmatrix} \omega _{1} \\ \omega _{2} \\ \omega _{3} \end {bmatrix}, S_{g}= \begin{bmatrix} S_{g11} & S_{g12} & S_{g13}\\ S_{g21} & S_{g22} & S_{g23}\\ S_{g31} & S_{g32} & S_{g33} \end{bmatrix} b_{g}= \begin{bmatrix} b_{g1}\\ b_{g2} \\ b_{g3} \end{bmatrix} \end{align*}$

在假設 S_g 是一個對角矩陣後，問題就簡化了許多，在 n_b 為高斯噪音（Gaussian Noise）下，平均值為 0，靜止下對陀螺儀取樣 y_g = 0，所以這時測量到的就是 b_g，只要在每次測量的結果減掉 b_g，並乘上一個放大縮小的變數，就可以得出理想的陀螺儀資料了，說的簡單一點就是在靜止下取平均值就可以得出 b_g。

$\large \begin{align*} &y_{g,c} = S_{g}^{-1}\,(y_{g}-b_{g}-n_{b}),\\ &where\; S_{g}^{-1}= \begin{bmatrix} S_{g11}^{-1} & 0 & 0\\ 0 & S_{g22}^{-1} & 0\\ 0 & 0 & S_{g33}^{-1} \end{bmatrix} \end{align*}$
　

加速度計（Accelerometer or G-Sensor）校正

首先先放上感測器的模型

$\large \begin{align*} &y_{a}=S_{a}\,y_{a,c}+b_{a}+n_a,\\ & where\; y_{a} = \begin{bmatrix} a_{1} \\ a_{2} \\ a_{3} \end {bmatrix}, S_{a}= \begin{bmatrix} S_{a11} & S_{a12} & S_{a13}\\ S_{a21} & S_{a22} & S_{a23}\\ S_{a31} & S_{a32} & S_{a33} \end{bmatrix} b_{a}= \begin{bmatrix} b_{a1}\\ b_{a2} \\ b_{a3} \end{bmatrix} \end{align*}$

對加速度計校正的部分除了 b_a 矩陣的偏差外，還會找出 S_a 矩陣來校正比例誤差與焊接、安裝產生的未對準誤差，這邊提供兩種校正的方法，第一種是基於最小平方法的校正方法，需要放置在特定角度下做校正，另一種則是高斯牛頓法，透過重力加速度固定的特性校準，不需放置在特定角度下來校準。

2017/07/16 補上最小平方法校正

．加速度計的最小平方法校正

先整理成以下形式

$\large \begin{align*} &y_{a,c} = S_{a}^{-1}(y_{a} - b_{a}) = S_{a}^{-1}\begin{bmatrix} I & -b_{a} \end{bmatrix}\begin{bmatrix} y_{a}^{T} & 1 \end{bmatrix}^{T} = KY, \\ &where\; K = S_{a}^{-1}\begin{bmatrix} I & -b_{a} \end{bmatrix} = \begin{bmatrix} S_{a}^{-1} & -S_{a}^{-1}b_{a} \end{bmatrix}\in \mathbb{R}^{3\times 4}, Y = \begin{bmatrix} y_{a}^{T} & 1 \end{bmatrix}^{T}\in \mathbb{R}^{4\times 1} \end{align*}$

擴展延伸至 N 種不同姿態，則 y_a,c 與測量矩陣 Y 則可重新寫成

$\large \begin{align*} &y_{a,c} \in \mathbb{R}^{3\times N}, Y = \begin{bmatrix} y_{a}^{(1)} & y_{a}^{(2)} & ... & y_{a}^{(N)} \\ 1 & 1 & ... & 1 \end{bmatrix} \in \mathbb{R}^{4\times N} \end{align*}$

若已知不同姿態下的，則可以透過最小二乘法求解矩陣 K

$\large \begin{align*} K = (y_{a,c}Y^{T})(YY^{T})^{-1} \end{align*}$

經由上式關係，可以反推回 S_a 與 b_a 矩陣

$\large \begin{align*} S_{a} = K(1:3, 1:3)^{-1}, b_{a} = - S_{a} K(1:3, 4) \end{align*}$

．加速度計的高斯牛頓法校正

為了減少運算，三軸間的正交誤差不特別做校正，因為使用的感測器是三軸集成於一顆芯片的，所以直接假設三軸互相正交，則 S 的反矩陣（K 矩陣）會是一個對證矩陣，並且表示成無偏差的形式，為的是減少展開的複雜度，所以問題可以化簡成下面式子

$\large \begin{align*} &y_{a,c} = S_{a}^{-1}(y_{a} - b_{a}) = K(y_{a} - b_{a}) = K\bar{y}_{a}, \\ &where\; \bar{y}_{a} = \begin{bmatrix} \bar{a}_{1} \\ \bar{a}_{2} \\ \bar{a}_{3} \end{bmatrix} = \begin{bmatrix} a_{1} - b_{a1} \\ a_{2} - b_{a2} \\ a_{3} - b_{a3} \end{bmatrix}, K = \begin{bmatrix} k_{11} & k_{12} & k_{13}\\ k_{12} & k_{22} & k_{23}\\ k_{13} & k_{23} & k_{33} \end{bmatrix} \end{align*}$

把右式展開則可以表示如下，其中 k 表示第 k 次的加速度計資料

$\large \begin{align*} (&y_{a,c})_{k} = {\begin{bmatrix} a_{c1} \\ a_{c2} \\ a_{c3} \end{bmatrix}}_{k} = \begin{bmatrix} k_{11}\bar{a}_{1} + k_{12}\bar{a}_{2} + k_{13}\bar{a}_{3} \\ k_{12}\bar{a}_{1} + k_{22}\bar{a}_{2} + k_{23}\bar{a}_{3} \\ k_{13}\bar{a}_{1} + k_{23}\bar{a}_{2} + k_{33}\bar{a}_{3} \end{bmatrix}_{k} \end{align*}$

再來就是透過在沒有運動加速度的條件下，加速度計三軸輸出平方和會等於重力加速度的關係來做校正，此種方法不需要依預先設定好的角度與位置來校正，校正的自由度較大

$\large \begin{align*} g^{2} = a_{c1}^{2} + a_{c2}^{2} + a_{c3}^{2} \end{align*}$

將上式表示成誤差函數，並展開與整理，結果如下

$\large \begin{align*} e & \equiv a_{c1}^{2} + a_{c2}^{2} + a_{c3}^{2} - g^{2}\\ & = (k_{11}^{2} + k_{12}^{2} + k_{13}^{2})(a_{1} - b_{1})^{2}\\ & + (k_{12}^{2} + k_{22}^{2} + k_{23}^{2})(a_{2} - b_{2})^{2}\\ & + (k_{13}^{2} + k_{23}^{2} + k_{33}^{2})(a_{3} - b_{3})^{2}\\ & + 2(k_{11}k_{12} + k_{12}k_{22} + k_{13}k_{23})(a_{1} - b_{1})(a_{2} - b_{2})\\ & + 2(k_{12}k_{13} + k_{22}k_{23} + k_{23}k_{33})(a_{2} - b_{2})(a_{3} - b_{3})\\ & + 2(k_{11}k_{13} + k_{12}k_{23} + k_{13}k_{33})(a_{3} - b_{3})(a_{1} - b_{1}) - g^{2}\\ & = A\bar{a}_{1}^{2} + B\bar{a}_{2}^{2} + C\bar{a}_{3}^{2} + 2D\bar{a}_{1}\bar{a}_{2} + 2E\bar{a}_{2}\bar{a}_{3} + 2F\bar{a}_{3}\bar{a}_{1} - g^{2} \end{align*}$

定義 E 函數為誤差的平方和，若可以找出 E 函數的極值（最小值）時，就可以得出 K 矩陣與 B 矩陣了

$\large \begin{align*} &E(X) = \sum_{k=1}^{N}e_{k}^{2}(X) = e^{T}e,\\ &where \,\, e = \begin{bmatrix} e_{1} & e_{2} & ... & e_{N} \end{bmatrix}^{T} \end{align*}$

因為 E 函數可以表示成 e'*e 的形式，所以這裡解極值的方法我們採用高斯牛頓法（Gauss-Newton Method），詳細的演算法說明請自行參考 wikipedia: Gauss-Newton Method ，迭代過程如下式，對誤差的 Jocabian 與 Hessian 反矩陣相乘，其中 Hessian 矩陣中省略掉了二次微分項，自己實際用 MATLAB 模擬過，有無省略結果其實差不多，有時好、有時差，但在運算量上差別還頗大的，所以這裡把二次微分向給忽略掉。

$\large \begin{align*} X_{k+1} & = X_{k} - \alpha \begin{pmatrix}\nabla^{2} E(X))^{-1}(\nabla E(X)\end{pmatrix}\\ & = X_{k} - \alpha \begin{pmatrix} \sum_{k=1}^{N}(\nabla e_{k}^{T}\nabla e_{k} + e_{k} \nabla^{2} e_{k}) \end{pmatrix}^{-1} \begin{pmatrix} \sum_{k=1}^{N}e_{k} \nabla e_{k}^{T} \end{pmatrix}\\ &\approx X_{k} - \alpha \begin{pmatrix} \sum_{k=1}^{N}\nabla e_{k}^{T}\nabla e_{k} \end{pmatrix}^{-1} \begin{pmatrix} \sum_{k=1}^{N}e_{k} \nabla e_{k}^{T} \end{pmatrix}\\ \end{align*}$

而 X 就是我們想要求得的 K 矩陣與 B 矩陣，透過不斷的迭代而得出來，如果要校正正交誤差就在加入 3 項 k 矩陣沒有對稱的變數即可，但實際效果如何我倒是沒有試過，有興趣可以自己試看看

$\large \begin{align*} X&= \begin{bmatrix} x_{1} & x_{2} & x_{3} & x_{4} & x_{5} & x_{6} & x_{7} & x_{8} & x_{9} \end{bmatrix}^{T}\\&= \begin{bmatrix} k_{11} & k_{12} & k_{13} & k_{22} & k_{23} & k_{33} & b_{a1} & b_{a2} & b_{a3} \end{bmatrix}^{T} \end{align*}$

2016/02/10 更正

之後想了一下，此方法主要以重力來校正，若裝置與重力有固定夾角的話，無法校正未對準或安裝的誤差

因為之前使用 Least Squares 在不同角度的重力值來做校正，所以可以校正安裝誤差，但缺點是需要外部的儀器精度才容易比較高，不過現在換成三軸輸出平方和為重力平方的條件後，因為沒有參考的角度，所以就無法校正與重力有夾角的裝置，但相對的，不需要外部儀器可以有一定的校正精度了。

MATLAB 的模擬結果

詳細的微分結果與程式都放在 Github 上

C 語言驗證版本 https://github.com/Hom-Wang/C-Language/tree/master/newtonMethod
MATLAB 驗證版本 https://github.com/Hom-Wang/MATLAB/tree/master/gauss-Newton_Methods

↑ 修正量，也就是 Jocabian 與 Hessian 反矩陣的相乘，最後卻趨近 0，表示收斂。
　
↑ 校正參數，在迭代的過程中不斷的更新，最後卻趨於穩定的一個值。
　
↑ 誤差平方和。
　
↑ 最後整裡的一張實際數值與迭代結果得比較表格，誤差大概都在 0.01% 以下。
　
　

磁力計（Magnetometer or Compass）校正

磁力計的校正之前是使用橢圓擬合，詳細可以參考Github: 推導_橢圓擬合，不過橢圓擬合僅適用於平面的校正，離開該平面，像是傾斜等情況，容易產生誤差，但理論上電子羅盤也可以用上述的高斯牛頓法來求得校正參數，把 g 改成 1 即可，實現歸一化的作用，因為最後在求航向角時，僅需要知道比例關係，不需要知道實際的磁場強度。

2016/06/29 補充

今天經過實際測試後，發現橢圓擬合方無法有效校正電子羅盤的資料，主要是因為實際應用是在三維空間中，橢圓擬合校準僅會校準二維平面上的偏移，之後再補上其他的電子羅盤校正方法

2016/07/30 新增磁力計校正方法 - 四參數校正模型

Freescale 的磁力計校準方法 (有另外的七參數和十參數校正)
https://github.com/memsindustrygroup/Open-Source-Sensor-Fusion/tree/master/docs

假設從磁力計中讀出，未經校正軟磁干擾 S 與硬磁干擾 H 的磁場為 h_m，已校正過的磁場為 h_c，地球磁場為 h_r，兩磁場間的轉換關係為 R，則其關係可表示如下：

$\begin{align*} &h_{c} = R h_{r},\ h_{m} = S h_{c} + H\\ &\Rightarrow h_{m} = S R h_{r} + H\\ &\Rightarrow h_{c} = S h_{r} = S^{-1} (h_{m} - H)\\ \end{align*}$

將以校正過的三軸磁力計任意旋轉而產生出來軌跡，會在的半徑為的球面上，其數值表示為地球磁場強度，關係如下：

$\begin{align*} \left \| h_{c} \right \|^{2} = h_{c}^{T} h_{c} = (R h_{r})^{T}R h_{r} = h_{r}^{T} R^{T} R h_{r} = B^{2} \end{align*}$

帶入可得到

$\begin{align*} &\left \| h_{c} \right \|^{2} = (S^{-1}(h_{m} - H))^{T} (S^{-1}(h_{m} - H)) = B^{2}\\ &\Rightarrow (h_{m} - H)^{T} (S^{-1})^{T} (S^{-1}) (h_{m} - H) = B^{2}\\ &\Rightarrow (h_{m} - H)^{T} A (h_{m} - H) = B^{2}\\ \\ & where A = (S^{-1})^{T} (S^{-1}) = A^{T} \end{align*}$

定義誤差函數

$\begin{align*} E = \left \| h_{c} \right \|^{2}-B^{2} \end{align*}$

．四參數校正模型 Four Parameter Magnetic Calibration Model

假設在無軟磁干擾，僅有硬磁干擾的情況下，原式可改寫如下

$\begin{align*} & h_{m} = h_{c} + H \\ & \Rightarrow h_{c} = h_{m} - H \\ & \Rightarrow \left \| h_{c} \right \|^{2} = h_{c}^{T} h_{c} = (h_{m} - H)^{T} (h_{m} - H) = h_{m}^{T} h_{m} -2 H^{T} h_{m} + H^{T} H \end{align*}$

並定義誤差函數

$\begin{align*} E = \left \| h_{c} \right \|^{2} - B^{2} = h_{m}^{T} h_{m} - 2 H^{T} h_{m} + H^{T} H - B^{2} \end{align*}$

將其展開與整理

$\begin{align*} E & = h_{mx}^{2} + h_{my}^{2} + h_{mz}^{2} - 2 H_{x} h_{mx} - 2 H_{y} h_{my} - 2 H_{z} h_{mz} + H_{x}^{2} + H_{y}^{2} + H_{z}^{2} - B^{2}\\ & = (h_{mx}^{2} + h_{my}^{2} + h_{mz}^{2}) - \begin{bmatrix} h_{mx}\\ h_{my}\\ h_{mz}\\ 1 \end{bmatrix}^{T} \begin{bmatrix} 2H_{x}\\ 2H_{y}\\ 2H_{z}\\ B^{2} - H_{x}^{2} - H_{y}^{2} - H_{z}^{2} \end{bmatrix} \\ & = (h_{mx}^{2} + h_{my}^{2} + h_{mz}^{2}) - \begin{bmatrix} h_{mx}\\ h_{my}\\ h_{mz}\\ 1 \end{bmatrix}^{T} \begin{bmatrix} \beta_{1}\\ \beta_{2}\\ \beta_{3}\\ \beta_{4} \end{bmatrix}\\ & = Y - X \beta \end{align*}$

當殘差為零時

$\begin{align*} & E = 0 \\ & \Rightarrow Y-X \beta = 0 \\ & \Rightarrow X \beta = Y \\ & \Rightarrow X^{T}X \beta = X^{T}Y \\ & \Rightarrow \beta = (X^{T}X )^{-1} X^{T}Y \\ \end{align*}$

擴展至 N 筆資料

$\begin{align*} Y-X \beta = \begin{bmatrix} h_{mx1}^{2} + h_{my1}^{2} + h_{my1}^{2}\\ h_{mx2}^{2} + h_{my2}^{2} + h_{my2}^{2}\\ \vdots \\ h_{mxN}^{2} + h_{myN}^{2} + h_{myN}^{2} \end{bmatrix} - \begin{bmatrix} h_{mx1} & h_{my1} & h_{mz1} & 1\\ h_{mx2} & h_{my2} & h_{mz2} & 1\\ \vdots & \vdots & \vdots & \vdots\\ h_{mxN} & h_{myN} & h_{mzN} & 1\\ \end{bmatrix} \begin{bmatrix} \beta_{1}\\ \beta_{2}\\ \beta_{3}\\ \beta_{4} \end{bmatrix} \end{align*}$

整理後

$\begin{align*} & \beta = (X^{T} X)^{-1} X^{T} Y \\ & \Rightarrow \begin{bmatrix} \beta_{1}\\ \beta_{2}\\ \beta_{3}\\ \beta_{4} \end{bmatrix} = \begin{pmatrix} \sum_{i=1}^{N} \begin{bmatrix} h_{mxi}^{2} & h_{mxi}h_{myi} & h_{mxi}h_{mzi} & h_{mxi}\\ h_{mxi}h_{myi} & h_{myi}^{2} & h_{myi}h_{mzi} & h_{myi}\\ h_{mxi}h_{mzi} & h_{myi}h_{mzi} & h_{mzi}^{2} & h_{mzi}\\ h_{mxi} & h_{myi} & h_{mzi} & 1 \end{bmatrix} \end{pmatrix}^{-1} \begin{bmatrix} h_{mx1} & h_{mx2} & \cdots & h_{mxN}\\ h_{my1} & h_{my2} & \cdots & h_{myN}\\ h_{mz1} & h_{mz2} & \cdots & h_{mzN}\\ 1 & 1 & \cdots & 1\\ \end{bmatrix} \begin{bmatrix} h_{mx1}^{2} + h_{my1}^{2} + h_{mz1}^{2}\\ h_{mx2}^{2} + h_{my2}^{2} + h_{mz2}^{2}\\ \vdots \\ h_{mxN}^{2} + h_{myN}^{2} + h_{mzN}^{2}\\ \end{bmatrix} \end{align*}$

$\begin{align*} &H = \begin{bmatrix} H_{x}\\ H_{y}\\ H_{z} \end{bmatrix} =\frac{1}{2} \begin{bmatrix} \beta_{1}\\ \beta_{2}\\ \beta_{3} \end{bmatrix}\\ & B=(\beta_{4} + H_{x}^{2} + H_{y}^{2} + H_{z}^{2})^{1/2} \end{align*}$

上一篇 → 從零開始做四軸 (五) - 感測器原理
下一篇 → 從零開始做四軸 (七) - 姿態計算

January 12, 2016

nRF52 DK 開箱

為了解決原本 nRF51-DK 運算量的問題，購買了新版本的 nRF52-DK，工作在 64MHz 下，CortexM4F 的藍牙開發板，與原本的 nRF51-DK 外型相似，主要差在多了 NFC 天線，而且顏色深了一些，目前還在熟悉一些基本的外設，官方一直修改 Library 真的很麻煩，有看到更新就會忍不去升級上去，一直在弄這些也頗累的...

Github 上也開始在新增 nRF52-DK 的範例了
https://github.com/Hom-Wang/NRF5x

January 12, 2016

NUCLEO-F303K8 開箱

實驗室最近從 MOUSER 購買的 NUCLEO-F303K8 開發板，幾個禮拜後要拿來教學用的，本來要用自己設計的 RdBeanSprout 來教學的，不過要焊這麼多開發板和燒錄器實在是很累，不敷成本阿，所以直接使用原廠的開發板來的方便多了。

這片開發板使用 STM32F303K8T6 的芯片，與 STM32F103 系列很相似，雖然具有相同的時脈，但具有 FPU 與更多新的功能，主要似乎都是集中在類比上面，像是較高性能的 ADC、比較器與放大器等，之後有空再試看看，會選擇這片開發板主要是因為體積較小（NUCLED-32），可以直接插在麵包板上，而且也跟 ST 以往的做法一樣，板子上仍有 ST-LINK & VCP 功能，省了很多的額外硬體，而且價格相當親民，MOUSER 上售價 380 台幣左右，相當的值得！唯一美中不足的是沒有外部的石英震盪器，頗可惜。

相關的測試程式放在 Github 上
https://github.com/Hom-Wang/NUCLEO-F303K8

體積上比自己設計的開發板大，但人家有燒錄器你沒有啊！！

January 8, 2016

STM32F469I-DISCO 開箱

最近需要用到遙控器，不過之前的 QCopterRC 遙控器都是自己重頭到尾設計和製作的，現在重做一次覺得覺得硬體有很大的進步空間，沒必要直接拿舊的來焊，但又懶得全部重新設計，所以就打算直接購買現有的 STM32F469I-DISCO，之後再搭配自己設計的簡單擴充底板來實現遙控器的功能，與之前設計的 QCopterRC 比較起來性能大增阿！而且有電容觸控的螢幕 + 麥克風，可以做的應用又變更多了。

此次是直接從 MOUSER 上購買的，台幣 2000 左右，因為滿 1400 所以不需要運費，算蠻划算的。

官網介紹：http://www.st.com/web/catalog/tools/FM116/CL1620/SC959/SS1532/LN1848/PF262395

IMUCube v1.0

SmartIMU v2.0

SmartBLE v1.2

MicroMultimeter v2.0

遙控器 QCopterRC-S v1.0

MiniMobileRobot v1.0

SmartIMU v2.0

2016/04/01 更新 － 預計販售版本，並新增電池底座 SmartIMU-BAT。

MicroMultimeter v2.0

測試版本的實體圖片

陀螺儀（Gyroscope or Gyro）校正

加速度計（Accelerometer or G-Sensor）校正

．加速度計的最小平方法校正

．加速度計的高斯牛頓法校正

MATLAB 的模擬結果

磁力計（Magnetometer or Compass）校正

．四參數校正模型 Four Parameter Magnetic Calibration Model

2016/04/01 更新－預計販售版本，並新增電池底座 SmartIMU-BAT。