「変身ベルト」を機械学習で作ってみた

#include <Arduino.h>
#include <M5Core2.h>
#include "filter.h"
#include <map>
#include <string>
#include <cstdio>
#include <ctime>
using namespace std;

// MPU
float accX, accY, accZ;
float filtered_accX, filtered_accY, filtered_accZ;
float gyroX, gyroY, gyroZ;
float pitch, roll, yaw;
float temp;

// Timer
volatile int interruptCounter;
hw_timer_t * timer = NULL;
portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;
void IRAM_ATTR onTimer() {
  portENTER_CRITICAL_ISR(&timerMux);
  interruptCounter++;
  portEXIT_CRITICAL_ISR(&timerMux);
}

// Low-pass Filter
float cutoff   = 0.3;  // Hz
float sampling = 0.02; // Sec
Filter fx(cutoff, sampling);
Filter fy(cutoff, sampling);
Filter fz(cutoff, sampling);

// File
File output_file;
bool isLogging = true;

// Activities
int activity = 0; // 0: 'WALKING', 1: 'SITTING', 2: 'STANDING', 3: 'HENSHING'
std::map<int, std::string> activityName;

// RTC
RTC_DateTypeDef RTC_DateStruct; // Data
RTC_TimeTypeDef RTC_TimeStruct; // Time

String zeroPadding(int num,int cnt){
  char tmp[256];
  char prm[5] = {'%','0',(char)(cnt+48),'d','\0'};
  sprintf(tmp,prm,num);
  return tmp;
}

// Setup
void setup(){
  // Activity Name
  activityName[0] = " WALKING ";
  activityName[1] = " SITTING ";
  activityName[2] = " STANDING";
  activityName[3] = " HENSHING";
  
  // M5 Init
  M5.begin();
  M5.IMU.Init();
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextColor(GREEN , BLACK);
  M5.Lcd.setTextSize(2);

//  時計合わせが必要なときに利用
//  RTC_TimeStruct.Hours   = 23;
//  RTC_TimeStruct.Minutes = 59;
//  RTC_TimeStruct.Seconds = 59;
//  M5.Rtc.SetTime(&RTC_TimeStruct);
//  RTC_DateStruct.WeekDay = 6;
//  RTC_DateStruct.Month = 8;
//  RTC_DateStruct.Date = 1;
//  RTC_DateStruct.Year = 2021;
//  M5.Rtc.SetDate(&RTC_DateStruct);

  // Timer
  timer = timerBegin(0, 80, true);
  timerAttachInterrupt(timer, &onTimer, true);
  timerAlarmWrite(timer, 20000, true); // 20ms  
  timerAlarmEnable(timer);

  // File
  if (!SD.begin()) {
    M5.Lcd.println("ERROR: SD CARD");
    while (1) ;
  }
  M5.Rtc.GetDate(&RTC_DateStruct);
  M5.Rtc.GetTime(&RTC_TimeStruct);
  String datetime = zeroPadding(RTC_DateStruct.Year, 4) 
                  + zeroPadding(RTC_DateStruct.Month, 2) 
                  + zeroPadding(RTC_DateStruct.Date, 2) 
                  + "-"
                  + zeroPadding(RTC_TimeStruct.Hours, 2)
                  + zeroPadding(RTC_TimeStruct.Minutes, 2)
                  + zeroPadding(RTC_TimeStruct.Seconds, 2);
  String filepath = "/imu_data_"  + datetime + ".csv";
  output_file = SD.open(filepath.c_str(), FILE_WRITE);
  if (!output_file) {
      M5.Lcd.println("ERROR: OPEN FILE");
      while (1) ;
  }
  output_file.println("activity,accX,accY,accZ,filtered_accX,filtered_accY,filtered_accZ,gyroX,gyroY,gyroZ"); // Header
  M5.Lcd.setCursor(0,221);
  M5.Lcd.printf("%s",activityName[activity].c_str());
}

// Loop
void loop(){
  M5.update();
  if (interruptCounter > 0) {
    portENTER_CRITICAL(&timerMux);
    interruptCounter--;
    portEXIT_CRITICAL(&timerMux); 

    M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
    M5.IMU.getAccelData(&accX,&accY,&accZ);
    M5.IMU.getAhrsData(&pitch,&roll,&yaw);
    M5.IMU.getTempData(&temp);
    M5.Lcd.setCursor(0, 0);    
    M5.Rtc.GetDate(&RTC_DateStruct);
    M5.Rtc.GetTime(&RTC_TimeStruct);
    M5.Lcd.printf("%04d.%02d.%02d  %02d:%02d:%02d", RTC_DateStruct.Year, RTC_DateStruct.Month, RTC_DateStruct.Date, RTC_TimeStruct.Hours, RTC_TimeStruct.Minutes, RTC_TimeStruct.Seconds);
    M5.Lcd.setCursor(0, 20);
    M5.Lcd.printf("%6.2f  %6.2f  %6.2f      ", gyroX, gyroY, gyroZ);
    M5.Lcd.setCursor(220, 42);
    M5.Lcd.print(" o/s");
    M5.Lcd.setCursor(0, 65);
    M5.Lcd.printf(" %5.2f   %5.2f   %5.2f   ", accX, accY, accZ);
    M5.Lcd.setCursor(0, 87);
    M5.Lcd.printf(" %5.2f   %5.2f   %5.2f   ", filtered_accX=fx.filter(accX), filtered_accY=fy.filter(accY), filtered_accZ=fz.filter(accZ));
    M5.Lcd.setCursor(220, 109);
    M5.Lcd.print(" G");
    M5.Lcd.setCursor(0, 132);
    M5.Lcd.printf(" %5.2f   %5.2f   %5.2f   ", pitch, roll, yaw);
    M5.Lcd.setCursor(220, 154);
    M5.Lcd.print(" degree");
    M5.Lcd.setCursor(0, 177);
    M5.Lcd.printf("Temperature : %.2f C", temp);

    // Logging
    if(isLogging){
      output_file.printf("%d,%.7e,%.7e,%.7e,%.7e,%.7e,%.7e,%.7e,%.7e,%.7e", activity, accX, accY, accZ, filtered_accX, filtered_accY, filtered_accZ, gyroX, gyroY, gyroZ);
      output_file.println();
    }
  }

  // Button A, change activities
  if (M5.BtnA.wasPressed()) {
    if(activity < activityName.size() - 1){
      activity++;
    } else {
      activity=0;
    }
    M5.Lcd.setCursor(0,221);
    M5.Lcd.printf("%s",activityName[activity].c_str());
   }

  // Button B
  if (M5.BtnB.wasPressed()) {
  }

  // Button C, stop logging
  if (M5.BtnC.wasPressed()) {
    if (isLogging) {
      isLogging=false;
      output_file.close();
      M5.Lcd.setCursor(180,221);
      M5.Lcd.printf("LOG STOPPED");
    }
  }
}

import csv

input = open('imu_data_yyyyMMddhhmmss.csv', 'r')
reader = csv.reader(input)
header = next(reader) # skip

output_acc_x = open('test/body_acc_x.txt', 'w')
writer_acc_x = csv.writer(output_acc_x, lineterminator='\n', delimiter=' ')
output_acc_y = open('test/body_acc_y.txt', 'w')
writer_acc_y = csv.writer(output_acc_y, lineterminator='\n', delimiter=' ')
output_acc_z = open('test/body_acc_z.txt', 'w')
writer_acc_z = csv.writer(output_acc_z, lineterminator='\n', delimiter=' ')
output_total_acc_x = open('test/total_acc_x.txt', 'w')
writer_total_acc_x = csv.writer(output_total_acc_x, lineterminator='\n', delimiter=' ')
output_total_acc_y = open('test/total_acc_y.txt', 'w')
writer_total_acc_y = csv.writer(output_total_acc_y, lineterminator='\n', delimiter=' ')
output_total_acc_z = open('test/total_acc_z.txt', 'w')
writer_total_acc_z = csv.writer(output_total_acc_z, lineterminator='\n', delimiter=' ')
output_gyro_x = open('test/body_gyro_x.txt', 'w')
writer_gyro_x = csv.writer(output_gyro_x, lineterminator='\n', delimiter=' ')
output_gyro_y = open('test/body_gyro_y.txt', 'w')
writer_gyro_y = csv.writer(output_gyro_y, lineterminator='\n', delimiter=' ')
output_gyro_z = open('test/body_gyro_z.txt', 'w')
writer_gyro_z = csv.writer(output_gyro_z, lineterminator='\n', delimiter=' ')
output_activity = open('test/activity.txt', 'w')
writer_activity = csv.writer(output_activity, lineterminator='\n', delimiter=' ')

accX1 = []
accX2 = []
accY1 = []
accY2 = []
accZ1 = []
accZ2 = []
totalAccX1 = []
totalAccX2 = []
totalAccY1 = []
totalAccY2 = []
totalAccZ1 = []
totalAccZ2 = []
gyroX1 = []
gyroX2 = []
gyroY1 = []
gyroY2 = []
gyroZ1 = []
gyroZ2 = []
activity1 = []
activity2 = []

def is_all_same(es):
    return all([e == es[0] for e in es[1:]]) if es else False

counter = 0
for row in reader:
    accX1.append(row[1])
    accY1.append(row[2])
    accZ1.append(row[3])
    totalAccX1.append(row[4])
    totalAccY1.append(row[5])
    totalAccZ1.append(row[6])
    gyroX1.append(row[7])
    gyroY1.append(row[8])
    gyroZ1.append(row[9])
    activity1.append(row[0])

    if counter >= 64:
        accX2.append(row[1])
        accY2.append(row[2])
        accZ2.append(row[3])
        totalAccX2.append(row[4])
        totalAccY2.append(row[5])
        totalAccZ2.append(row[6])
        gyroX2.append(row[7])
        gyroY2.append(row[8])
        gyroZ2.append(row[9])
        activity2.append(row[0])

    if len(accX1) == 128:
        if is_all_same(activity1):
            writer_acc_x.writerow(accX1)
            writer_acc_y.writerow(accY1)
            writer_acc_z.writerow(accZ1)
            writer_total_acc_x.writerow(totalAccX1)
            writer_total_acc_y.writerow(totalAccY1)
            writer_total_acc_z.writerow(totalAccZ1)
            writer_gyro_x.writerow(gyroX1)
            writer_gyro_y.writerow(gyroY1)
            writer_gyro_z.writerow(gyroZ1)
            writer_activity.writerow(activity1[0])
        accX1 = []
        accY1 = []
        accZ1 = []
        totalAccX1 = []
        totalAccY1 = []
        totalAccZ1 = []
        gyroX1 = []
        gyroY1 = []
        gyroZ1 = []
        activity1 = []

    if len(accX2) == 128:
        if is_all_same(activity2): 
            writer_acc_x.writerow(accX2)
            writer_acc_y.writerow(accY2)
            writer_acc_z.writerow(accZ2)
            writer_total_acc_x.writerow(totalAccX2)
            writer_total_acc_y.writerow(totalAccY2)
            writer_total_acc_z.writerow(totalAccZ2)
            writer_gyro_x.writerow(gyroX2)
            writer_gyro_y.writerow(gyroY2)
            writer_gyro_z.writerow(gyroZ2)
            writer_activity.writerow(activity2[0])
        accX2 = []
        accY2 = []
        accZ2 = []
        totalAccX2 = []
        totalAccY2 = []
        totalAccZ2 = []
        gyroX2 = []
        gyroY2 = []
        gyroZ2 = []

    counter = counter + 1

input.close()
output_acc_x.close()
...

import numpy as np 
import pandas as pd
import tensorflow as tf
from keras.models import Sequential
from keras.models import load_model
from keras.layers import LSTM
from keras.layers.core import Dense, Dropout
from util import load_data
from util import confusion_matrix

# Load Dataset
records_train, records_test, labels_train, labels_test = load_data()
timesteps = len(records_train[0])
input_dim = len(records_train[0][0])
n_classes = len(set([tuple(category) for category in labels_train]))
print(timesteps, input_dim, n_classes)

# Create Model / LSTM
model = tf.keras.Sequential([
    tf.keras.layers.Bidirectional(
        tf.keras.layers.LSTM(32),
        input_shape=(timesteps, input_dim)),
    tf.keras.layers.Dense(n_classes, activation="sigmoid")
])
model.compile(
    loss='categorical_crossentropy', 
    optimizer='rmsprop', 
    metrics=['accuracy'])

# Train
model.fit(
    records_train,
    labels_train,
    batch_size=16,
    validation_data=(records_test, labels_test),
    epochs=200
)

# Save Model
model.save('henshing_model.h5')

# Result
print(confusion_matrix(labels_test, model.predict(records_test)))

import pandas as pd
import numpy as np

#DATADIR = 'UCI HAR Dataset'
DATADIR = 'M5Stack Dataset'

AXES = [
    "body_acc_x",
    "body_acc_y",
    "body_acc_z",
    "body_gyro_x",
    "body_gyro_y",
    "body_gyro_z",
    "total_acc_x",
    "total_acc_y",
    "total_acc_z"
]

# UCI HAR Dataset を読み込むときは適宜修正
LABELS = {
    0: 'WALKING',
    1: 'SITTING',
    2: 'STANDING',
    3: 'HENSHING',
}

def get_records(mode):
    records = []
    for ax in AXES:
        records.append(
            pd.read_csv(f'{DATADIR}/{mode}/Inertial Signals/{ax}_{mode}.txt', delim_whitespace=True, header=None).values
        ) 
    return np.transpose(records, (1, 2, 0))

def get_labels(mode):
    label = pd.read_csv(f'{DATADIR}/{mode}/y_{mode}.txt', delim_whitespace=True, header=None)[0]
    return pd.get_dummies(label).values

def load_data():
    return get_records('train'), get_records('test'), get_labels('train'), get_labels('test')

def confusion_matrix(true, pred):
    true = pd.Series([LABELS[i] for i in np.argmax(true, axis=1)])
    pred = pd.Series([LABELS[i] for i in np.argmax(pred, axis=1)])
    return pd.crosstab(true, pred, rownames=['True'], colnames=['Pred'])

model = load_model("henshing_model.h5")
converter = tf.lite.TFLiteConverter.from_keras_model(model)
tflite_model = converter.convert()
open("henshing_model.tflite", "wb").write(tflite_model)

$ xxd -i henshing_model.tflite > henshing_model.cc 

#include "henshing_model.h"
alignas(8) const unsigned char g_model[] = {
  0x1c, 0x00, 0x00, 0x00, 0x54, 0x46, 0x4c, 0x33, 0x14, 0x00, 0x20, 0x00,
  0x04, 0x00, 0x08, 0x00, 0x0c, 0x00, 0x10, 0x00, 0x14, 0x00, 0x00, 0x00,
  0x18, 0x00, 0x1c, 0x00, 0x14, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00,
  ...
}
const int g_model_len = 1186508;

#ifndef TENSORFLOW_LITE_MICRO_HENSHING_MODEL_H_
#define TENSORFLOW_LITE_MICRO_HENSHING_MODEL_H_
extern const unsigned char g_model[];
extern const int g_model_len;
#endif

$ sudo make -f tensorflow/lite/micro/tools/make/Makefile TARGET=esp generate_hello_world_esp_project

#include <math.h>
#include "tensorflow/lite/micro/all_ops_resolver.h"
#include "tensorflow/lite/micro/micro_error_reporter.h"
#include "tensorflow/lite/micro/micro_interpreter.h"
#include "henshing_model.h"
#include <M5Core2.h>
#include <cstdio>
#include <ctime>
#include "filter.h"
#include "FastLED.h"

#define LEDS_PIN 25
#define LEDS_NUM 10
#define LEDSTAPE_PIN 32
#define LEDSTAPE_NUM 72
CRGB ledsBuff[LEDS_NUM];
CRGB ledsTapeBuff[LEDSTAPE_NUM];

// MPU
float accX, accY, accZ;
float filtered_accX, filtered_accY, filtered_accZ;
float gyroX, gyroY, gyroZ;

// Filter
const float cutoff   = 0.3;
const float sampling = 0.02;
Filter fx(cutoff, sampling);
Filter fy(cutoff, sampling);
Filter fz(cutoff, sampling);

// Acc data
std::vector<std::vector<float>> input_tensor; 

// Timer
volatile int interruptCounter;
hw_timer_t * timer = NULL;
portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;
void IRAM_ATTR onTimer() {
  portENTER_CRITICAL_ISR(&timerMux);
  interruptCounter++;
  portEXIT_CRITICAL_ISR(&timerMux);
}
volatile int invoke_counter;

constexpr int tensor_pool_size = 60 * 1024;
uint8_t tensor_pool[tensor_pool_size];
const tflite::Model* model;
tflite::MicroInterpreter* interpreter;
TfLiteTensor* input;
TfLiteTensor* output;

// Setup
void setup() {
  M5.begin();
  M5.IMU.Init();
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextColor(GREEN , BLACK);
  M5.Lcd.setTextSize(2);
  M5.Lcd.println("Start Now");

  // Timer
  timer = timerBegin(0, 80, true);
  timerAttachInterrupt(timer, &onTimer, true);
  timerAlarmWrite(timer, 20000, true); // 20ms
  timerAlarmEnable(timer);

  // LED
  FastLED.addLeds<SK6812, LEDSTAPE_PIN>(ledsTapeBuff, LEDSTAPE_NUM);
  for (int i = 0; i < LEDSTAPE_NUM; i++){
      ledsTapeBuff[i].setRGB(0, 0, 0);
  }
  FastLED.show();

  // Load Model
  model = tflite::GetModel(g_model);
  if (model->version() != TFLITE_SCHEMA_VERSION) {
    M5.Lcd.println("Model Error");
  }else{
    M5.Lcd.println("Model Loaded");
  }

  // MicroInterpreter
  static tflite::AllOpsResolver resolver;
  static tflite::ErrorReporter* error_reporter;
  static tflite::MicroErrorReporter micro_error;
  error_reporter = &micro_error;
  static tflite::MicroInterpreter static_interpreter(
      model, resolver, tensor_pool, tensor_pool_size, error_reporter
  );
  interpreter = &static_interpreter;
  TfLiteStatus allocState = interpreter->AllocateTensors();
  if (allocState != kTfLiteOk) {
      M5.Lcd.println("AllocateTensors Error");
      return;
  }
  input = interpreter->input(0);
  output = interpreter->output(0);
}

// Loop
void loop() {
  M5.update();
  if (interruptCounter > 0) {
    portENTER_CRITICAL(&timerMux);
    interruptCounter--;
    portEXIT_CRITICAL(&timerMux); 

    M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
    M5.IMU.getAccelData(&accX,&accY,&accZ);
    filtered_accX=fx.filter(accX);
    filtered_accY=fy.filter(accY);
    filtered_accZ=fz.filter(accZ);

    std::vector<float> temp_vectore{ accX, accY, accZ, filtered_accX, filtered_accY, filtered_accZ, gyroX, gyroY, gyroZ};
    input_tensor.push_back(temp_vectore);
    M5.Lcd.setCursor(0, 0);    
    M5.Lcd.println(accX);      

    if (input_tensor.size() == 128) {
      // Fill input
      for(int h = 0; h < 128; ++h) {
        for(int w = 0; w < 9; ++w) {
            auto i = h * 9  + w;
            input->data.f[i] = input_tensor[h][w];
        }
      }

      // Invoke
      if (kTfLiteOk != interpreter->Invoke()) {
        M5.Lcd.println("Invoke Error");
      }else{
        invoke_counter = invoke_counter + 1;
        M5.Lcd.setCursor(0, 60);    
        M5.Lcd.println("Invoke OK");
        M5.Lcd.println("++++++++++++");
        M5.Lcd.println(invoke_counter);      
        M5.Lcd.printf("WALKING:  %5.2f", output->data.f[0]); //WALKING
        M5.Lcd.println();
        M5.Lcd.printf("SITTING:  %5.2f", output->data.f[1]); //SITTING
        M5.Lcd.println();
        M5.Lcd.printf("STANDING: %5.2f", output->data.f[2]); //STANDING
        M5.Lcd.println();
        M5.Lcd.printf("HENSHING: %5.2f", output->data.f[3]); //HENSHIN

        // HENSHING モーション判定でLEDを点灯
        if(output->data.f[3] > output->data.f[0] 
        && output->data.f[3] > output->data.f[1] 
        && output->data.f[3] > output->data.f[2]){
          for (int k = 0; k < 30; k++){
            for (int i = 0; i < LEDSTAPE_NUM/4; i++){
              ledsTapeBuff[i*4+0].setRGB(0, 20, 0);
              ledsTapeBuff[i*4+1].setRGB(0, 20, 0);
              ledsTapeBuff[i*4+2].setRGB(0, 0, 0);
              ledsTapeBuff[i*4+3].setRGB(0, 0, 0);
            }
            FastLED.show(); 
            delay(100);
            for (int i = 0; i < LEDSTAPE_NUM/4; i++){
              ledsTapeBuff[i*4+0].setRGB(0, 0, 0);
              ledsTapeBuff[i*4+1].setRGB(0, 20, 0);
              ledsTapeBuff[i*4+2].setRGB(0, 20, 0);
              ledsTapeBuff[i*4+3].setRGB(0, 0, 0);
            }
            FastLED.show(); 
            delay(100);
            for (int i = 0; i < LEDSTAPE_NUM/4; i++){
              ledsTapeBuff[i*4+0].setRGB(0, 0, 0);
              ledsTapeBuff[i*4+1].setRGB(0, 0, 0);
              ledsTapeBuff[i*4+2].setRGB(0, 20, 0);
              ledsTapeBuff[i*4+3].setRGB(0, 20, 0);
            }
            FastLED.show(); 
            delay(100);
          } 
          for (int i = 0; i < LEDSTAPE_NUM; i++){
              ledsTapeBuff[i].setRGB(0, 0, 0);
          }
          FastLED.show();
        }
      }
      delay(1000);
      input_tensor.clear();
    }
  }
}

import numpy as np 
import pandas as pd
import tensorflow as tf
from keras.models import Sequential
from keras.models import load_model
from keras.layers import LSTM
from keras.layers.core import Dense, Dropout
from util import load_data
from util import confusion_matrix

# Load Dataset
records_train, records_test, labels_train, labels_test = load_data()
timesteps = len(records_train[0])
input_dim = len(records_train[0][0])
n_classes = len(set([tuple(category) for category in labels_train]))
print(timesteps, input_dim, n_classes)

def reshape_function(data):
  reshaped_data = tf.reshape(data, [-1, 3, 1])
  return reshaped_data

# Create Model / CNN
model = tf.keras.Sequential([
      tf.keras.layers.Conv2D(8, (4, 3), padding="same", activation="relu", input_shape=(timesteps, input_dim, 1)),
      tf.keras.layers.MaxPool2D((3, 3)), 
      tf.keras.layers.Dropout(0.1),  
      tf.keras.layers.Conv2D(16, (4, 1), padding="same", activation="relu"),  
      tf.keras.layers.MaxPool2D((3, 1), padding="same"),  
      tf.keras.layers.Dropout(0.1), 
      tf.keras.layers.Flatten(),
      tf.keras.layers.Dense(16, activation="relu"),
      tf.keras.layers.Dropout(0.1),
      tf.keras.layers.Dense(n_classes, activation="softmax")
])
records_train = records_train.reshape(402,128, 9, 1)
records_test = records_test.reshape(152, 128, 9, 1)
model.compile(
    loss='categorical_crossentropy', 
    optimizer='rmsprop', 
    metrics=['accuracy'])

# Train
model.fit(
    records_train,
    labels_train,
    batch_size=16,
    validation_data=(records_test, labels_test),
    epochs=200
)

# Save
model.save('henshing_model.h5')

# Result
print(confusion_matrix(labels_test, model.predict(records_test.reshape(152, 128, 9, 1))))