====== 8. Strojové učení ======

===== Cíle cvičení =====

  * Seznámení výstupem z 6DOF IMU ICM-20648
  * Rozpoznání náklonu akcelerometru
  * Interpretace IMU dat
===== Další zdroje =====

  * [[https://pythonnumericalmethods.berkeley.edu/notebooks/chapter16.00-Least-Squares-Regression.html|Least Squares Regression]]
  * http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/
  * {{ :courses:b0b37nsi:tutorials:1704.06053.pdf |Using Inertial Sensors for Position and Orientation Estimation}}

===== Rozpoznání náklonu z akcelerometru =====

Cílem této úlohy je natrénovat SVM klasifikátor tak, aby rozpoznal přibližné naklonění akcelerometru. Abychom toho mohli dosáhnout, je třeba projít těmito kroky:
  - Získání trénovacích dat
  - Označení trénovacích dat (štítky)
  - Natrénování modelu
  - Predikce pomocí v reálném čase

==== Získání trénovacích dat ====

<note tip>

**Firmware pro Thunderboard**

  * proud dat: {{ :courses:b0b37nsi:tutorials:firmware-flow.zip |fw01.zip}}
  * data jako odpověď na libovolný byte: {{ :courses:b0b37nsi:tutorials:firmware-on-demand.zip |fw02.zip}}

</note>

=== Thunderboard ===

Program pro [[https://cw.fel.cvut.cz/wiki/courses/b0b37nsi/hw/01|Thunderboard]] čte data z IMU a posílá je do PC prostřednictvím UART rozhraní. Obslužný program je napsán v prostředí MBED. 

<code cpp>
#include <mbed.h>
#include "ICM20648.h"

typedef struct imu_data {
  float acc_x, acc_y, acc_z, gyr_x, gyr_y, gyr_z;
};

UnbufferedSerial port(USBTX, USBRX, 115200);
DigitalOut imu_en(PF8, 1);

int main() {

  imu_data a;

  ICM20648* imu = new ICM20648(PC0, PC1, PC2, PC3, PF12);
  if(!imu->open()) {
    imu_en = false;
  }

  while(1) {
    if(imu_en) {
      imu->get_accelerometer(&(a).acc_x, &(a).acc_y, &(a).acc_z);
      imu->get_gyroscope(&(a).gyr_x, &(a).gyr_y, &(a).gyr_z);
      port.write(&a, sizeof(imu_data));
    }
  }
}
</code>

Jedná se o data typu float, takže je nutné promyslet, jak data efektivně přenést. S ohledem na rychlost přenosu a potřebnou přesnost bude v tomto případě možná výhodné přenášet data binárně. V Pythonu pak lze pro dekódování dat ve struktuře využít knihovny [[https://docs.python.org/3/library/ctypes.html|ctypes]] nebo [[https://docs.python.org/3/library/struct.html|struct]].

<code python>
from ctypes import Structure, c_uint, c_int, c_float, sizeof

class IMUData(Structure):
    _pack_ = 1
    _fields_ = [
        ("acc_x", c_float),
        ("acc_y", c_float),
        ("acc_z", c_float),
        ("gyr_x", c_float),
        ("gyr_y", c_float),
        ("gyr_z", c_float)
    ]
#
# pokud existuje otevřený UART port popsaný objektem ser
byte = ser.read(sizeof(IMUData))
buffer = bytearray(byte)
IMU = IMUData.from_buffer(buffer)
# data ze stuktury jsou pak dostupná jako IMU.acc_x, ...
</code>  

=== PC - příjem a uložení ===

Na straně PC je třeba datat dekódovat a uložit do souboru s jednoduše editovatelným formátem, případně zobrazit. Lze vycházet např. z kódu na [[https://gitlab.fel.cvut.cz/viteks/nsi-codes/-/blob/main/tut-08-processing/imu-receive.py|gitlabu]], nebo použít následující:

<code python>
import struct
import numpy as np
#
points = []
# cteni ze serioveho portu
byte = ser.read(24)
points.append(struct.unpack('6f', byte))
</code> 

Pro offline analýzu uložte data do souboru ve formátu CSV. To lze udělat např. pomocí modulu [[https://docs.python.org/3/library/csv.html|csv]].

<code python>
import csv

header = ['acc_x', 'acc_y', 'acc_z', 'gyr_x', 'gyr_y', 'gyr_z']
data = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]


with open('data.csv', 'w', encoding='UTF8', newline='') as f:
    writer = csv.writer(f)

    # write the header
    writer.writerow(header)

    # write the data
    writer.writerow(data) 
</code>

==== Přiřazení štíků ====

Rozpoznání náklonu akcelerometru je velmi jednoduchá klasifikační úloha. Hledané třídě (náklonu) odpovídá kombinace diskrétní hodnot, získaných z IMU. Nasbíraná data rozdělíme do tříd tak, že do jednotlivých řádků doplníme jednoznačný štítek, např. takto
  * 0 - vodorovně
  * 1 - náklon dopředu
  * 2 - náklon dozadu

Štítky nemusí být číselné, může to být i textový řetězec. Příklad výsledného souboru:

<code>
acc_x,acc_y,acc_z,gyr_x,gyr_y,gyr_z,class
0.076416015625,-0.04150390625,1.00341796875,0.030517578125,6.591796875,0.9307861328125,0
0.1044921875,-0.04052734375,0.851318359375,-2.95257568359375,9.6435546875,-0.38909912109375,0
...
0.173583984375,0.426513671875,0.89892578125,2.99072265625,10.36834716796875,1.8768310546875,1
0.146728515625,0.327392578125,0.689697265625,3.692626953125,4.31060791015625,4.6844482421875,1
...
-0.076904296875,-0.660888671875,0.782958984375,5.767822265625,2.02178955078125,2.4566650390625,2
-0.1162109375,-0.67138671875,0.619384765625,6.44683837890625,-15.106201171875,-0.61798095703125,2
</code>

==== Trénování klasifikátoru ====

Uložená data použijeme jako trénovací (70% dat) a testovací (30% dat) množinu pro získání klasifikátoru.

<code python>import pandas as pd
import numpy as np
from sklearn.model_selection import train_test_split
from sklearn import svm
import pickle

dataset = pd.read_csv('data.csv')
X = dataset.iloc[:, [0, 1, 2, 3, 4, 5]].values
y = dataset.iloc[:, [6]].values

X_train, X_test, y_train, y_test = train_test_split(X, y, test_size = 0.30, random_state = 0)

clf = svm.SVC(kernel='linear')
clf.fit(X_train, np.ravel(y_train))
</code>

Klasifikátor je možné uložit pro pozdější použití např. pomocí modulu [[https://docs.python.org/3/library/pickle.html|pickle]].

<code python>
filename = 'model.sav'
pickle.dump(clf, open(filename, 'wb'))
</code>

==== Predikce ====

Pokud máme k dispozici model, je možné použít předikci z dat, které v reálném čase získáváme z Thunderboardu:

<code python>
clf = pickle.load(open('model.sav', 'rb'))
# ziskani dat
D = [[data.acc_x, data.acc_y, data.acc_z, data.gyr_x, data.gyr_y, data.gyr_z]]
# predikce
print(clf.predict(D))
</code>
===== IMU interpretace =====

{{:courses:b0b37nsi:tutorials:7e35672ed3e05191cb6aea0d69cf6ebb98bcb88a.png?400|}}

<code>
pitch = 180 * atan2(acc_x, sqrt(acc_y*acc_y + acc_z*acc_z))/PI;
roll = 180 * atan2(acc_y, sqrt(acc_x*acc_x + acc_z*acc_z))/PI;
</code>

Vyzkoušejte vypočítat hodnoty a zamyslet se nad tím, jaké výsledky získáváte.

Další linky:

https://silo.tips/download/application-note-imu-visualization-software

https://github.com/jarzebski/Arduino-ADXL345

http://davidegironi.blogspot.com/2013/02/avr-atmega-mpu6050-gyroscope-and.html#.YnpJnZYzXrc

https://www.instructables.com/Arduino-MPU6050-GY521-6-Axis-Accelerometer-Gyro-3D/

https://www.slideshare.net/dibaowang/imu-general-introduction


===== Kód pro získání dat z gyroskopu =====

Kód pro získání dat z gyroskopu a vytisknutí těchto dat do konzole ve vhodné formě (např. pro nakopírování do CSV souboru). Za ''COMXXX'' dejte název vašeho komunikačního rozhraní, hodnota proměnné třída rozlišuje třídu pro klasifikaci.  

<code python>
from ctypes import Structure, c_uint, c_int, c_float, sizeof
import serial

class IMUData(Structure):
    _pack_ = 1
    _fields_ = [
        ("acc_x", c_float),
        ("acc_y", c_float),
        ("acc_z", c_float),
        ("gyr_x", c_float),
        ("gyr_y", c_float),
        ("gyr_z", c_float)
    ]

trida = 0

try:
    ser = serial.Serial('COMXXX', 115200)
    ser.flushInput()
    for i in range(1, 20):
        byte = ser.read(sizeof(IMUData))
        buffer = bytearray(byte)
        IMU = IMUData.from_buffer(buffer)
        print('{},{},{},{}'.format(IMU.gyr_x, IMU.gyr_y, IMU.gyr_z, trida))
except:
    print("unable to open COM port")
</code>

===== Kód pro vizualizaci dat z gyroskopu =====

Předpokládejme, že ve třech prvních sloupcích CSV souboru jsou data přečtená z gyroskopu. Jejich vizualizaci  můžeme provést např. takto:

<code python>
from mpl_toolkits import mplot3d
import pandas as pd
import matplotlib.pyplot as plt

data = pd.read_csv('data1.csv')

ax = plt.axes(projection='3d')

x = data.iloc[:,0]
y = data.iloc[:,1]
z = data.iloc[:,2]

ax.scatter3D(x, y, z, color='green')
plt.show()
</code>