Hackster is hosting Hackster Holidays, Ep. 6: Livestream & Giveaway Drawing. Watch now!Tune in to Hackster Holidays, Ep. 6 now!
Hans-Günther Nusseck
Published © MIT

Hotel Room Finder: WiFi Based Location System to Find Home

An indoor positioning system based on WiFi RSSI data to return to the right room of a hotel corridor. Maybe useless, but it works just fine!

AdvancedFull instructions provided4 hours2,593

Things used in this project

Hardware components

ESP32 GREY Development Kit with 9Axis Sensor
M5Stack ESP32 GREY Development Kit with 9Axis Sensor
×1
Flash Memory Card, SD Card
Flash Memory Card, SD Card
×1

Software apps and online services

PlatformIO IDE
PlatformIO IDE

Story

Read more

Code

main.cpp

C/C++
The main code for the M5Stack
/**************************************************************************
 * WiFi based location system to find home.
 * 
 * A Indoor Positioning System based on WiFi RSSI data to return to the 
 * right room of a hotel corridor. Maybe useless, but it works just fine!
 * 
 * Hague Nusseck @ electricidea
 * v1.1 05.June.2020
 * https://github.com/electricidea/M5Stack-Hotel-room-finder
 * 
 * Changelog:
 * v1.1 = - first published version
 * 
 * Distributed as-is; no warranty is given.
 * 
 ***************************************************************************/
#include <Arduino.h>

#include <M5Stack.h>
// install the library:
// pio lib install "M5Stack"

#include "WiFi.h"

// Free Fonts for nice looking fonts on the screen
#include "Free_Fonts.h"

// library for liniear and nonlinear fits
#include "curve_fit.h"

// maximum number of access points = maximum number of fits
const int max_fits = 40;
// array of of a number of fits 
curve_fit fits[max_fits] = curve_fit();

// position on the floor
double min_pos = 99999;
double max_pos = -99999;
// array for the calculated x positions along the floor
double *newx_array;
int n_newx = 0;

// the inverse intensity lookup table Map
double *IILTM;
int n_usable_APs = 0;

// the BSSID lookup table
typedef char cstring[100];  
cstring *BSSIDLT;
// the array for the square sums
double *square_sum_array;

// state machine index to switch between the menu states
int menu_state = 0;

#define STATE_START 1
#define STATE_MEASURE 2
#define STATE_GET_DATA 3
#define STATE_DATA 4
#define STATE_RUN 5

// value for the measurement along the floor
int measure_position = 0;

//==============================================================
// function forward declaration
uint16_t RGB2Color(uint8_t r, uint8_t g, uint8_t b);
void writeFile(fs::FS &fs, const char * path, const char * message);
void Clear_Screen();
void print_menu(int menu_index);
String split(String source, char delimiter, int location);
uint8_t collect_WiFi_data(String filename, bool append = true);
bool load_measurement(String filename);
bool load_floor_data();
String calculate_position();
bool analyze_measurements();


void setup() {
    // initialize the M5Stack object
    M5.begin();
    // configure the Lcd display
    M5.Lcd.setBrightness(100); //Brightness (0: Off - 255: Full)
    M5.Lcd.setTextColor(TFT_WHITE);
    M5.Lcd.setTextSize(1);
    Clear_Screen();
    // configure centered String output
    M5.Lcd.setTextDatum(CC_DATUM);
    M5.Lcd.setFreeFont(FF2);
    M5.Lcd.drawString("Hotel room Finder", (int)(M5.Lcd.width()/2), (int)(M5.Lcd.height()/2), 1);
    M5.Lcd.setFreeFont(FF1);
    M5.Lcd.setTextColor(TFT_RED);
    M5.Lcd.drawString("Version 1.1 | 05.06.2020", (int)(M5.Lcd.width()/2), (int)(M5.Lcd.height()/2)+50, 1);
    // configure Top-Left oriented String output
    M5.Lcd.setTextDatum(TL_DATUM);
    M5.Lcd.setTextColor(TFT_WHITE);
    // Start Menu
    menu_state = 1;
    print_menu(menu_state);
}

void loop() {
  M5.update();

  // left Button
  if (M5.BtnA.wasPressed()){
    switch (menu_state) {
        case STATE_START: {   //  START -> MEASURE 
            Clear_Screen();
            menu_state = STATE_MEASURE;
            print_menu(menu_state);
            break;       
        }
        case STATE_MEASURE: {   //  MEASURE -> ADD
            Clear_Screen();
            M5.Lcd.println("Stand in front of the door\nand face the door.\n");
            M5.Lcd.println("got to the LEFT and press (<)");
            M5.Lcd.println("or to the RIGHT and press (>)");
            measure_position = 0;
            menu_state = STATE_GET_DATA;
            print_menu(menu_state);
            break;       
        }
        case STATE_GET_DATA: {   //  NEW -> < (-)
            Clear_Screen();
            ++measure_position;
            M5.Lcd.printf("measure %i steps away\n", measure_position);
            int n_WiFi_networks = collect_WiFi_data("/WiFi_data.txt");
            if(n_WiFi_networks > 0)
                M5.Lcd.printf("[OK] %i Networks found\n", n_WiFi_networks);
            else
                M5.Lcd.println("[ERR] unable to scan WiFi");
            print_menu(menu_state);
            break;       
        }
        case STATE_DATA: {   //  DATA -> DELETE
            Clear_Screen();
            M5.Lcd.println("Delete all measured data...");
            if(SD.remove("/WiFi_data.txt"))
              M5.Lcd.println("[OK] data deleted");
            else
              M5.Lcd.println("\n\n[ERR] unable to deleted data");
            writeFile(SD, "/WiFi_data.txt","pos;n;name;id;RSSI");
            measure_position = 0;
            n_usable_APs = 0;
            n_newx = 0;
            min_pos = 99999;
            max_pos = -99999;
            print_menu(menu_state);
            break;       
        }
        case STATE_RUN: {   //  RUN -> CHECK
            // check my position
            Clear_Screen();
            M5.Lcd.setTextDatum(CC_DATUM);
            M5.Lcd.setFreeFont(FF3); 
            M5.Lcd.drawString("Let me check...", (int)(M5.Lcd.width()/2), (int)(M5.Lcd.height()/2), 1);
            String pos_result = calculate_position();
            Clear_Screen();
            M5.Lcd.setTextDatum(CC_DATUM);
            M5.Lcd.setFreeFont(FF4); 
            M5.Lcd.drawString(pos_result.c_str(), (int)(M5.Lcd.width()/2), (int)(M5.Lcd.height()/2), 1);
            print_menu(menu_state);
            break;       
        }
    } 
  }


  // center Button
  if (M5.BtnB.wasPressed()){
    switch (menu_state) {
        case STATE_START: {   //  START -> RUN 
            Clear_Screen();
            M5.Lcd.setTextDatum(TL_DATUM);
            M5.Lcd.setFreeFont(FF1);
            // load floor data from SD card
            if(load_floor_data()) {
                Clear_Screen();
                M5.Lcd.println("\n\n      OK, ready to run");
                menu_state = STATE_RUN;
            } else
                M5.Lcd.println("Failed to load data");   
            print_menu(menu_state);
            break;       
        }
        case STATE_MEASURE: {   //  MEASURE -> BACK
            Clear_Screen();
            menu_state = STATE_START;
            print_menu(menu_state);
            break;       
        }
        case STATE_GET_DATA: {   //  MEASURE -> DONE
            Clear_Screen();
            // analyze measured data
            M5.Lcd.println("let's analyze the data");
            if(analyze_measurements())
                M5.Lcd.println("\n\n         OK, Success!");
            else
                M5.Lcd.println("\nSorry\nSomething went wrong.. :-(");
            menu_state = STATE_START;
            print_menu(menu_state);
            break;       
        }
        case STATE_DATA: {   //  DATA -> BACK
            Clear_Screen();
            menu_state = STATE_START;
            print_menu(menu_state);
            break;       
        }
        case STATE_RUN: {   //  RUN -> DONE
            Clear_Screen();
            menu_state = STATE_START;
            print_menu(menu_state);
            break;       
        }
    } 
  }

  
  // right Button
  if (M5.BtnC.wasPressed()){
    switch (menu_state) {
        case 1: {   //  START -> DATA 
            menu_state = STATE_DATA;
            print_menu(menu_state);
            break;       
        }
        case STATE_MEASURE: {   //  MEASURE -> NEW
            Clear_Screen();
            M5.Lcd.println("Delete all measured data...");
            if(!SD.remove("/WiFi_data.txt"))
              M5.Lcd.println("[ERR] unable to deleted data");
            writeFile(SD, "/WiFi_data.txt","pos;n;name;id;RSSI");
            M5.Lcd.println("\nReady for new measurements");
            M5.Lcd.println("\nStand in front of the door\nand face the door.\n");
            M5.Lcd.println("got to the LEFT and press (<)");
            M5.Lcd.println("or to the RIGHT and press (>)");
            measure_position = 0;
            menu_state = STATE_GET_DATA;
            print_menu(menu_state);
            break;       
        }
        case STATE_GET_DATA: {   //  NEW ->  > (+)
            Clear_Screen();
            --measure_position;
            M5.Lcd.printf("measure %i steps away\n", measure_position);
            int n_WiFi_networks = collect_WiFi_data("/WiFi_data.txt");
            if(n_WiFi_networks > 0)
              M5.Lcd.printf("[OK] %i Networks found\n", n_WiFi_networks);
            else
              M5.Lcd.println("\n\n[ERR] unable to scan WiFi");
            print_menu(menu_state);
            break;       
        }
        case STATE_DATA: {   //  DATA -> INFO
            Clear_Screen();
            M5.Lcd.printf("Data Info:\n\n");
            M5.Lcd.printf("usable APs: %i\n", n_usable_APs);
            M5.Lcd.printf("min x pos: %.1f\n", min_pos);
            M5.Lcd.printf("max x pos: %.1f\n", max_pos);
            print_menu(menu_state);
            break;       
        }
        case STATE_RUN: {   //  RUN -> Nothing
            break;       
        }
    } 
  }

  delay(50);
}

//==============================================================
// convert a RGB color into a LCD color value
uint16_t RGB2Color(uint8_t r, uint8_t g, uint8_t b) {
  return ((r / 8) << 11) | ((g / 4) << 5) | (b / 8);
}

//==============================================================
// split() returns a substring out of a source string
// defined by a delimiter and a location
// inspired by:
// http://wp.scalesoft.de/arduino-split/
String split(String source, char delimiter, int location) {
  String result = "";
  int locationCount = 0;
  int FromIndex = 0, ToIndex = -1;
  while (location >= locationCount) {
    FromIndex = ToIndex + 1;
    ToIndex = source.indexOf(delimiter,FromIndex);
    // if there is no delimiter anymore within the source string
    if(ToIndex == -1){
      // return the end of the string starting from the last delimiter
      // if the actual location is the target location
      if (location == locationCount) {
        result = source.substring(FromIndex);
      } else {
        // if not, return a empty string
        return "";
      }
    } else {
      // if it is not the last delimiter, but the actual
      // loacation is the target location, return the sub String
      if (location == locationCount)
        result = source.substring(FromIndex,ToIndex);
    }
    ++locationCount;
  }
  return result;
}

//==============================================================
// Scan for WiFi networks and save the SSID, BSSID and RSSI
// into a file on the SD card
// fix file name: "pos_data.txt"
uint8_t collect_WiFi_data(String filename, bool append){
    File file;
    if(append)
      file = SD.open(filename.c_str(), FILE_APPEND);
    else
      file = SD.open(filename.c_str(), FILE_WRITE);
    int n = WiFi.scanNetworks();
    if (n == 0) {
        M5.Lcd.println("[ERR] no networks found");
    } else {
        for (int i = 0; i < n; ++i) {
            // Print SSID, BSSID and RSSI for each network found
            file.printf("%i;%i;%s;%s;%i\n",measure_position , i+1, WiFi.SSID(i).c_str(), WiFi.BSSIDstr(i).c_str(), WiFi.RSSI(i));
            delay(10);
        }
    }
    file.close();
    return n;
}

//==============================================================
// Write Text into a file
void writeFile(fs::FS &fs, const char * path, const char * message){
    File file = fs.open(path, FILE_APPEND);
    if(!file){
        M5.Lcd.println("\n\n[ERR] Failed to open file");
        return;
    } else {
        if(!file.println(message))
            M5.Lcd.println("\n\n[ERR] Write failed");
        file.close();
    }
} 


//==============================================================
// Print a small menu at the bottom of the display above the buttons
void print_menu(int menu_index){
    M5.Lcd.fillRect(0, M5.Lcd.height()-25, M5.Lcd.width(), 25, RGB2Color(50,50,50));
    M5.Lcd.setCursor(0, 230);    
    M5.Lcd.setFreeFont(FF1);
    M5.Lcd.setTextColor(TFT_WHITE);
    switch (menu_index) {
      case 0: { // never used 
        M5.Lcd.print("      -       -        - ");
        break;       
      }
      case STATE_START: { // start menu
        M5.Lcd.print("   MEASURE   RUN     DATA"); 
        break;
      }
      case STATE_MEASURE: { // Submenu 1 for new Data collection  
        M5.Lcd.print("     ADD    BACK      NEW ");
        break;
      }
      case STATE_GET_DATA: { // Submenu 2 for new Data collection 
        M5.Lcd.print("      <     DONE       > ");
        break;
      }
      case STATE_DATA: { // DATA Submenu 
        M5.Lcd.print("   DELETE    BACK     INFO"); 
        break;
      }
      case STATE_RUN: { // RUN Submenu 
        M5.Lcd.print("    CHECK   DONE         "); 
        break;
      }
      default: { // should never been called
        M5.Lcd.print("      -       -        - ");
        break;
      }
    }
}


//==============================================================
// Clear the entire screen and add one row
// The added row is important. Otherwise the first row is not visible
void Clear_Screen(){
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setCursor(0, 0);
  M5.Lcd.println("");
}


//==============================================================
// loads a stored measurement of positions and BSSID, RSSI data
// the data is used to learn the fits for each WiFi access point
// fix file name: "WiFi_data.txt"
bool load_measurement(String filename){
  // reset min and max for new measurements
  min_pos = 99999;
  max_pos = -99999;
  // reset all fits
  // and set the tag to -1 = not learned
  for(int i = 0; i < max_fits; ++i){
    // init as fith order polynomial
    fits[i].init(5);
    fits[i].reset();
    fits[i].tag = -1;
  }
  File file = SD.open("/WiFi_data.txt");
  if(!file){
      M5.Lcd.println("Failed to open file");
  } else {
      String line = "";
      while(file.available()){
        // file format:
        // pos;n;name;id;RSSI
        char chread = file.read();
        // ASCII printable characters (character code 32-127)
        if((chread >= 32) && (chread <= 127))
          line = line + String(chread);
        // CRLF = \r\n = ASCII code 13 and then ASCII code 10
        if((chread == '\r' || chread == '\n') && (line.length() > 0)){
          // get the BSSID at index 3
          String BSSID = split(line, ';', 3);
          // skip the header line
          if(BSSID != "id"){
            // get the position at index 0
            double pos = split(line, ';', 0).toDouble();
            if(pos > max_pos)
              max_pos = pos;
            if(pos < min_pos)
              min_pos = pos;
            // get the RSSI at index 4
            double RSSI = split(line, ';', 4).toDouble();
            int i = 0;
            bool OK_Flag = false;
            // check all fits if the BSSID exist
            // then let this fit learn the new data pair
            // otherwise, use the next unused fit for this BSSID
            do {
              if(fits[i].name == BSSID){
                fits[i].learn(pos, RSSI);
                OK_Flag = true;
              }
              if(fits[i].tag == -1){
                fits[i].tag = i;
                fits[i].name = BSSID;
                Serial.print(i);
                Serial.print(": ");
                Serial.println(BSSID);
                fits[i].learn(pos, RSSI);
                OK_Flag = true;
              }
              if(++i == max_fits){
                OK_Flag = true;
              }
            } while (!OK_Flag);
          }
          line = "";
        }
    }
    file.close();
    return true;
  }
  return false;
}


//==============================================================
// loads a stored floor data from SD card
// the data is used to find the room
// fix file name: "floor_data.txt"
bool load_floor_data(){
// load floor data from file
    M5.Lcd.printf("loading from file:\n  -->  /floor_data.txt\n");
    File file = SD.open("/floor_data.txt");
    if(!file){
      return false;
    } else {
      // File format:
      // n_newx
      // n_usable_APs
      // newx_array[0] ... newx_array[n_newx-1]
      // BSSIDLT[0] ... BSSIDLT[n_usable_APs-1]
      // IILTM[0] ... IILTM[((n_usable_APs-1)*n_newx)+(n_newx-1)]
      String line = "";
      int File_Block_index = 0;
      // 0 = header information with array dimensions
      // 1 = newx_array
      // 2 = BSSIDLT
      // 3 = IILTM
      int line_count = 0;
      while(file.available()){
          char chread = file.read();
          if(chread != '\n'){
            line = line + String(chread);
          } else {
            switch (File_Block_index) {
            // 0 = header information with array dimensions
            case 0:
              n_newx = split(line, ';', 0).toInt();
              n_usable_APs = split(line, ';', 1).toInt();     
              // free all daynamic arrays           
              free(newx_array);
              free(square_sum_array);
              free(BSSIDLT);
              free(IILTM);
              M5.Lcd.printf("new_x array size: %i \n", n_newx);
              M5.Lcd.printf("n_usable_APs: %i \n", n_usable_APs);
              Serial.printf("new_x array size: %i \n", n_newx);
              Serial.printf("n_usable_APs: %i \n", n_usable_APs);
              // allocate all dynamic arrays with the dimensions from the file
              newx_array = (double*) malloc(n_newx*sizeof(double));
              square_sum_array = (double*) malloc(n_newx*sizeof(double));
              BSSIDLT = (cstring*) malloc(n_usable_APs*sizeof(cstring));
              IILTM = (double*) malloc(n_newx*n_usable_APs*sizeof(double));
              // stop, if allocation fails
              if(!newx_array || !square_sum_array || !IILTM || !BSSIDLT){
                M5.Lcd.printf("[ERR] unable to allocate memory\n");
                delay(5000);
                // stop processing readed data from file
                File_Block_index = -1;
                file.close();
                return false;
              } else{
                Serial.println("done: header!");
                Serial.println("read newx_array data...");
                File_Block_index = 1;
              }
              break;
            
            // 1 = newx_array data
            case 1:
              if(line != ""){
                // read the new-x values line by line
                newx_array[line_count++] = split(line, ';', 0).toDouble();
                if(line_count == n_newx){
                  Serial.println("done: read newx_array!");
                  Serial.println("read BSSIDLT data...");
                  File_Block_index = 2;
                  line_count = 0;
                }
              }
              break;
              
            // 2 = BSSIDLT data
            case 2:
              if(line != ""){
                // read the BSSID values line by line
                strcpy(BSSIDLT[line_count++], split(line, ';', 0).c_str());
                if(line_count == n_usable_APs){
                  Serial.println("done: read BSSIDLT!");
                  Serial.println("read IILTM data...");
                  File_Block_index = 3;
                  line_count = 0;
                }
              }
              break;
              
            // 3 = IILTM data
            case 3:
              if(line != ""){
                // read the IILTM data
                for(int i=0; i < n_usable_APs; ++i){
                  IILTM[(i*n_newx)+line_count] = split(line, ';', i).toDouble();
                }
                ++line_count;
                if(line_count == n_newx){
                  Serial.println("done: read IILTM!");
                  File_Block_index = -1;
                  line_count = 0;
                }
              }
              break;
            
            default:
              break;
            }
            line = "";
          }
      }
      file.close();
    } 
    return true; 
}

//==============================================================
// scan fout times the available APS
// Calculate the best fitting positon based on the IILTM
// Return the number as text, or text if the position can't be calculated
String calculate_position(){
  String result = "...";
  // scan APs and save to File "pos_data.txt"
  // four times...
  int n_WiFi_networks = collect_WiFi_data("/pos_data.txt", false);
  collect_WiFi_data("/pos_data.txt");
  collect_WiFi_data("/pos_data.txt");
  collect_WiFi_data("/pos_data.txt");

  // open the file and go throw all APs...
  File file = SD.open("/pos_data.txt");
  if(!file || n_newx == 0 || n_usable_APs == 0 || n_WiFi_networks == 0){
    // without a file or without any APs, we are unable to find the room
    return "No idea :-(";
  } else {
    // Because we scanned four times, we have to average the RSSI data
    // This can be done with the fit-class ()
    // reset all fits
    // and set the tag to -1 = not learned
    for(int i = 0; i < max_fits; ++i){
      fits[i].init(0);
      fits[i].reset();
      fits[i].tag = -1;
    }
    String line = "";
    while(file.available()){
      char chread = file.read();
      // ASCII printable characters (character code 32-127)
      if((chread >= 32) && (chread <= 127))
        line = line + String(chread);
      // CRLF = \r\n = ASCII code 13 and then ASCII code 10
      if((chread == '\r' || chread == '\n') && (line.length() > 0)){
        // file format:
        // pos;n;name;BSSID;RSSI
        String BSSID = split(line, ';', 3);
        // skip the header line
        if(BSSID != "id"){
          double RSSI = split(line, ';', 4).toDouble();
          // find AP in BSSIDLT
          int AP_index = -1;
          for(int i = 0; i < n_usable_APs; ++i){
            if(strcmp(BSSIDLT[i], BSSID.c_str()) == 0)
              AP_index = i;
          }
          if(AP_index > -1){
            fits[AP_index].learn(0.0, RSSI);
            fits[AP_index].tag = AP_index;
          }
        }
        line = "";
      }
    }
    file.close();
    // Now, the fits are filled with the average RSSI data from the APs
    // Time to calculate the square sum array:
    for(int x = 0; x < n_newx; ++x)
      square_sum_array[x] = 0.0;
    for(int AP_index = 0; AP_index < n_usable_APs; ++AP_index){
      if(fits[AP_index].tag > -1){
        for(int x = 0; x < n_newx; ++x){
          // predict(0.0) will return the average value because 
          // the fit is initialized with degree = 0
          double RSSI_diff = (fits[AP_index].predict(0.0) - IILTM[(AP_index*n_newx)+x]);
          square_sum_array[x] += (RSSI_diff*RSSI_diff);
        }
      }
    }
    // Finally find the minimum of the square sums:
    double best_pos = newx_array[0];
    double min_sum = square_sum_array[0];
    for(int x = 0; x < n_newx; ++x){
      if(square_sum_array[x] < min_sum){
        min_sum = square_sum_array[x];
        best_pos = newx_array[x];
      }
    }
    // If best pos is the first or the last position of the new x array
    // then we can say that we are far away, because we might don't know 
    // the right value of the distance
    if(best_pos == newx_array[0] || best_pos == newx_array[n_newx-1])
      result = "far away...";
    else
      result = String(best_pos).c_str();
  }
  return result;
}

//==============================================================
// load the measurements from SD card (file: /WiFi_data.txt)
// build the new-x array, the BSSIDLT and the IILTM
// return true if the procedure was succesfull
bool analyze_measurements(){
  bool result = false;
  M5.Lcd.printf("Reading file:\n --> /WiFi_data.txt\n");
  // load measured data from file and let the fits learn...
  if(load_measurement("/WiFi_data.txt")){
    M5.Lcd.printf("Analyze AP data\n");
    // build new_x array....
    // free the daynamic arrays
    free(newx_array);
    free(square_sum_array);
    // get the position range out of the data
    int x_range = round(max_pos - min_pos);
    n_newx = x_range *2;
    // allocate the memory for the array with the new size
    newx_array = (double*) malloc(n_newx*sizeof(double));
    square_sum_array = (double*) malloc(n_newx*sizeof(double));
    // if the memory allocation failed
    if(!newx_array || !square_sum_array)
      return false;
    else {
      // fill the newx_array with the fine position steps
      for(int i=0; i < n_newx; ++i){
        newx_array[i] = min_pos + (i*((double)x_range / (double)n_newx));
      }
      // check for usable APs out of the fits
      // criteria:
      // at least 6 valid data points
      //    --> fith order polynome should have at least 6 values
      // estimated min and max y values should not be out of bounds [-25 .. -95]
      // a minimum of 15dBm amplitude over the data range is required
      n_usable_APs = 0;
      for(int i = 0; i < max_fits; ++i){
        // check all fits for criteria
        double min_y = fits[i].estimate_min_y();
        double max_y = fits[i].estimate_max_y();
        if(fits[i].tag > -1){
          if((fits[i].count() < 6) ||               
              (min_y < -95.0) || (max_y > -25.0) ||  
              (fabs(max_y - min_y) < 15)) {          
                fits[i].reset();
                fits[i].name = "";
                fits[i].tag = -1;
          }
        }
        if(fits[i].tag > -1){
          Serial.printf("%i: N: %i min: %.2f max: %.2f\n", i, fits[i].count(), fits[i].estimate_min_y(), fits[i].estimate_max_y());
          ++n_usable_APs;
        }
      }

      // build Inverse Intensity Lookup Table Map (IILTM)
      // ....
      M5.Lcd.printf("Build IILTM and BSSIDLT\n");
      Serial.println("Build the IILTM and the BSSIDLT:");
      if(n_usable_APs > 0){
        Serial.printf("number of usable APs: %i \n", n_usable_APs);
        // free the daynamic arrays
        free(IILTM);
        free(BSSIDLT);
        // allocate the memory for the array with the new size
        BSSIDLT = (cstring*) malloc(n_usable_APs*sizeof(cstring));
        IILTM = (double*) malloc(n_newx*n_usable_APs*sizeof(double));
        // if the memory allocation failed
        if(!IILTM || !BSSIDLT) {
          Serial.println("[ERR] malloc failed");
          return false;
        } else {
          int AP_count = 0;
          for(int i = 0; i < max_fits; ++i){
            //Serial.printf("check: %i: \n", i);
            if(fits[i].tag > -1){
              //Serial.printf("tag > -1 --> %i: %s\n", AP_count, fits[i].name.c_str());
              strcpy(BSSIDLT[AP_count], fits[i].name.c_str());
              for(int x = 0; x < n_newx; ++x){
                // -95dBm for x values outside the learned range
                IILTM[(AP_count*n_newx)+x] = fits[i].predict(newx_array[x], -95.0);
              }
              ++AP_count;
            }
          }
        }
      } else {
        M5.Lcd.printf("no usable APs found!\n");
        Serial.println("no usable APs found!");
        return false;
      }

      Serial.println("the BSSIDLT:");
      for(int i = 0; i < n_usable_APs; ++i){
        Serial.printf("%i: %s\n", i, BSSIDLT[i]);
      }

      Serial.println("the IILTM:");
      for(int x = 0; x < n_newx; ++x){
        Serial.printf("\n%.2f", newx_array[x]);
        for(int i = 0; i < n_usable_APs; ++i){
          Serial.printf(" %.2f", IILTM[(i*n_newx)+x]);
        }
      }
      // save floor data to file
      M5.Lcd.printf("Writing to file:\n --> /floor_data.txt\n");
      File file = SD.open("/floor_data.txt", FILE_WRITE);
      if(!file){
          M5.Lcd.println("Failed to open file");
          return false;
      } else {
        // File format:
        // n_newx
        // n_usable_APs
        // newx_array[0] ... newx_array[n_newx-1]
        // BSSIDLT[0] ... BSSIDLT[n_usable_APs-1]
        // IILTM[0] ... IILTM[((n_usable_APs-1)*n_newx)+(n_newx-1)]
        // header with dimensions
        file.printf("%i;%i\n",n_newx, n_usable_APs);
        // save newx_array
        for(int x=0; x < n_newx; ++x){
          file.printf("%.6f\n",newx_array[x]);
        }
        // save BSSIDLT array
        for(int i=0; i < n_usable_APs; ++i){
          file.printf("%s\n",BSSIDLT[i]);
        }
        // save IILTM array
        for(int x=0; x < n_newx; ++x){
          file.printf("\n%.6f",IILTM[(0*n_newx)+x]);
        for(int i=1; i < n_usable_APs; ++i){
            file.printf(";%.6f",IILTM[(i*n_newx)+x]);
          }
        }
        file.printf("\n");
        file.close();
      }

      M5.Lcd.println("done..");
      Serial.println("");
      Serial.println("done!");
    }
    result = true;
  }
  return result;
}

curve_fit.h

C/C++
Header file for the fit class
/***************************************************
 * 
 * Fitting of a Polynomial using Cramer Rule
 * 
 * Hague Nusseck @ electricidea
 * 
 * --> see curve_fit.cpp for more details on how it 
 * works and how to use it.
 * 
 * Distributed as-is; no warranty is given.
 * 
 * ************************************************/

// class definition
class curve_fit {
    public:
        curve_fit(uint8_t degree=2);
        bool init(uint8_t degree);
        void learn(double x, double y);
        double predict(double x);
        double predict(double x, double outside_value);
        void get_coefficients(double values[]);
        String get_formula(uint8_t decimals = 6);
        uint32_t get_order();
        void reset();
        double max_x() const { return max_x_; }
        double min_x() const { return min_x_; }
        int count() const { return N; }
        double estimate_max_y(uint32_t steps = 100);
        double estimate_min_y(uint32_t steps = 100);
        int tag;
        String name;
    private:
        // Mindex generates the index for adressing the matrices
        uint32_t Mindex(uint32_t i, uint32_t j);
        double determinant(double *mainmatrix);
        int order = -1;
        double *M;
        double *b;
        // y = a[n]*x^n + ... + a[1]*x + a[0]
        double *a;
        // Number of learned x, y pairs
        uint32_t N = 0;
        double max_x_, min_x_;
};

curve_fit.cpp

C/C++
fit class for linear and nonlinear curve fitting
/**************************************************************************
 * Library to calculate polynomial regression in Arduino.
 * 
 * Simple, efficient and memory efficient method to 
 * calculate mean values, linear regressions or 
 * parabolas from pairs of values.
 * 
 * Based on Cramers rule  for fitting of a polynomial 
 * regression model using least square method.
 * For a very good explanation, please see:
 * https://neutrium.net/mathematics/least-squares-fitting-of-a-polynomial/
 * 
 * and thanks to Cubiwan:
 * https://github.com/cubiwan/LinearRegressino
 * and Chandu yadav:
 * https://www.tutorialspoint.com/cplusplus-program-to-compute-determinant-of-a-matrix
 * for the inspiration on how to implement it
 * and Martin R.
 * https://codereview.stackexchange.com/questions/204135/determinant-using-gauss-elimination
 * for the tip about partial pivoting...
 * 
 * 
 * Hague Nusseck @ electricidea
 * v1.4 24.May.2020
 * https://github.com/electricidea/xxxx
 * 
 * Changelog:
 * v1.2 = - first version ready with "Laplace expansion" Method to calculate determinants
 * v1.3 = - Gaussian triangular method for the determination of determinants (faster!)
 *        - added name and tag
 *        - added max_x and min_x
 *        - established aij Matrix notation
 *        - String output for formular
 * v1.4 = - dynamic matrix sizes according to initialized order
 *        - functions to estimate max and min y values over the known range of x
 *        - add function count() to return N
 * 
 * 
 * Distributed as-is; no warranty is given.
 * 
 **************************************************************************
 * 
 * ==== How to use it: ====
 * 
 * 1.) intitialize the fitting object:
 * 
 *          curve_fit fit_1 = curve_fit(1);
 * NOTE:
 * degree = 0 --> A constant (or: The Average of y values)
 * degree = 1 --> A first order polynomial (or: linear regression)
 * degree = 2 --> A second order polynomial (or: parabolic curve)
 * degree = 3 --> A third order polynomial
 * Note: 
 * Generally the degree is not limited, but from the 6th or 7th 
 * degree the floating-point arithmetic reaches its limits and 
 * there may be inaccuracies.
 * 
 * In principle, there is no limit for the degree of polynomials, 
 * but from the 6th or 7th degree on, first calculation errors appear
 * due to the inaccurate floating-point arithmetic. 
 * 
 * 2.) Let the function solve the polynomial regression model 
 *     by adding pairs of x and y values:
 * 
 *          fit_1.learn(0,5);
 *          fit_1.learn(1,18.65);
 *              ...
 *          fit_1.learn(10,52.4);
 * 
 * During each of these steps the polynomial representation is calculated. 
 * The value pairs are not stored. As a result, the computing time is 
 * higher, but the class can be used more flexible.
 * 
 * 3.) print out the calculated coefficients:
 * 
 *          double coeff_values[2];
 *          fit_1.get_coefficients(coeff_values);
 *          Serial.printf("y= a1*x + a0\n\na1= %.2f\na0= %.2f", 
 *                        coeff_values[1], coeff_values[0]);
 * 
 * 4.) Calculate the y values for a new x value:
 * 
 *          new_y = fit_1.predict(new_x);
 * 
 * Some NOTES:
 * At any time additional pairs of values can be added to improve 
 * the calculation / allow further learning.....
 * The value pairs are not stored. They cannot be read out again 
 * or changed afterwards.
 * 
 * 
 * ==== How the Math works: ==== 
 * 
 * Representation of a kth order polynomial
 * y = ak*x^k +⋯+ a1*x + a0
 * 
 * with
 * a	:	Polynomial coefficient
 * k	:	The degree of the polynomial
 * N	:	The number of points to be regressed
 *  
 * The coefficients of the polynomial regression model (ak,ak−1,⋯,a1) may be 
 * determined by solving the following system of linear equations:
 * 
 * |                                             |   |    |   |              |
 * |    N          SUM(xi)     ...    SUM(xi^k)  |   | a0 |   |    SUM(yi)   |
 * |  SUM(xi)     SUM(xi^2 )   ...   SUM(xi^k+1) |   | a1 |   |  SUM(xi*yi)  |
 * |    ...         ...        ...       ...     | * | .. | = |     ...      |
 * | SUM(xi^k)  SUM(xi^k+1)    ...    SUM(xi^2k) |   | ak |   | SUM(xi^k*yi) |
 * |                                             |   |    |   |              |
 * 
 * Cramer's rule allows you to solve the linear system of equations to find the 
 * regression coefficients using the determinants of the square matrix M. Each 
 * of the coefficients ak may be determined using the following equation:
 * 
 * ak = det(Mi) / det(M)
 * 
 * Where Mi is the matrix M with the ith column replaced with the column 
 * vector b (remembering the system is presented in the form Ma=b). 
 * For example M0 could be calculated as follows:
 * 
 * 
 *      |                                                |  
 *      |     SUM(yi)     SUM(xi)      ...    SUM(xi^k)  | 
 *      |   SUM(xi*yi)   SUM(xi^2 )    ...   SUM(xi^k+1) | 
 * M0 = |      ...          ...        ...       ...     | 
 *      | SUM(xi^k*yi)   SUM(xi^k+1)   ...    SUM(xi^2k) |  
 *      |                                                |  
 * 
 * Example for an linear regression (polynominal 1th order)
 * 
 *  y = a1*x + a0
 * 
 * |                     |   |    |   |            |
 * |   N       SUM(xi)   |   | a0 |   |   SUM(yi)  |
 * | SUM(xi)  SUM(xi^2)  | * | a1 | = | SUM(xi*yi) |
 * |                     |   |    |   |            | 
 * 
 * 
 *      |                     | 
 *      |   N       SUM(xi)   | 
 *  M = | SUM(xi)  SUM(xi^2)  | 
 *      |                     |  
 * 
 *      |                        | 
 *      |   SUM(yi)    SUM(xi)   | 
 * M0 = | SUM(xi*yi)  SUM(xi^2)  | 
 *      |                        |  
 * 
 *      |                      | 
 *      |   N       SUM(yi)    | 
 * M1 = | SUM(xi)  SUM(xi*yi)  | 
 *      |                      |  
 * 
 * a0 = det(M0) / det(M)
 * a1 = det(M1) / det(M)
 * 
 * How to calculate the determinants of a 2x2 matrix M:
 * 
 *     |         |
 *     | a11 a12 |
 * det | a21 a22 | = a11*a22 – a12*a21
 *     |         |  
 * 
 ***************************************************************************/

#include "Arduino.h"
#include "curve_fit.h"

//==============================================================
// the constructor
curve_fit::curve_fit(uint8_t degree) {
    tag = 0;
    name = "";
    curve_fit::init(degree);
}

//==============================================================
// Initialization of the fit object
// degree = 0 --> A constant (or: The Average of y values)
// degree = 1 --> A first order polynomial (or: linear regression)
// degree = 2 --> A second order polynomial (or: parabolic curve)
// degree = 3 --> A third order polynomial
// Note: 
// Generally the degree is not limited, but from the 6th or 7th 
// degree the floating-point arithmetic reaches its limits and 
// there may be inaccuracies.
// this function can be used to change the degree at runtime
bool curve_fit::init(uint8_t degree) {
    order = degree;
    free(a);
    free(b);
    free(M);
    // the matrix dimension need to order + 1 for the calculations
    a = (double*) malloc((order+1)*sizeof(double));
    b = (double*) malloc((order+1)*sizeof(double));
    M = (double*) malloc((order+1)*(order+1)*sizeof(double));
    // if malloc fails, set order to -1
    // and return false
    if(!a || !b || !M) {
		order = -1;
        N = 0;
        return false;
	}
    curve_fit::reset();
    return true;
}

//==============================================================
// clear all calculated coefficients and buffered values 
// set all values back to 0.0 to start a new calculation
void curve_fit::reset() {
    // initialize the matrices with 0.0
    for(int i = 0; i <= order; i++) {
        for(int j = 0; j <= order; j++)
            M[Mindex(i,j)] = 0.0;
        a[i] = 0.0;
        b[i] = 0.0;
    }
    // reset the number of used x, y pairs
    N = 0;
    max_x_ = 0.0;
    min_x_ = 0.0;
}

//==============================================================
// calculate the determinant by following Gaussian Method:
// Use elementary row operations to bring the matrix into 
// a row echelon form, to be able to calculated the determinant 
// by using the diagonal values.
double curve_fit::determinant( double *mainmatrix) {   
    double det = 1;
    // order < 2 = matrix dimention up to 2x2
    // for these matrix sizes, the determinants can be calculate directly
    if (order < 2) {
        if(order == -1)
            // A matrix without dimension?
            // would say the deteminat is zero....
            det = 0;
        if(order == 0)
            // The determinant of a 1x1 matrix is the element of the matrix itself
            det = mainmatrix[0];
        if(order == 1)
            // a 2x2 matrix can be calculated directly:
            //       |         |                       |            |
            //       | a11 a12 |                       | M[0]  M[1] |
            //   det | a21 a22 | = a11*a22 – a12*a21 = | M[2]  M[3] | = M[0]*M[3] - M[1]*M[2]
            //       |         |                       |            | 
            det = ((mainmatrix[0] * mainmatrix[3]) - (mainmatrix[1] * mainmatrix[2]));
    } else {   
        // otherwise, elementary row operations are required to bring the 
        // matrix into a row echelon form.
        // First, create a work-matrix with the same dimensions
        // and fill it with the data from the main matrix
        double workmatrix[(order+1)*(order+1)];
        for(int i = 0; i < order+1; ++i)
            for(int j = 0; j < order+1; ++j)
                workmatrix[Mindex(i,j)] = mainmatrix[Mindex(i,j)];
        // here are some hints about Gaussian Elimination with Partial Pivoting:
        // https://youtu.be/euIXYdyjlqo
        // https://www.youtube.com/watch?v=SQ98-eImrgA
        // Now, go throw each column
        for (int i = 0; i < order+1; ++i) {
            // find the largest element for partial pivoting
            double pivotElement = workmatrix[Mindex(i,i)];
            int pivotRow = i;
            for (int row = i + 1; row < order+1; ++row) {
                if (fabs(workmatrix[Mindex(row,i)]) > fabs(pivotElement)) {
                    pivotElement = workmatrix[Mindex(row,i)];
                    pivotRow = row;
                }
            }
            // important:
            // there is no solution, if the pivot element is zero!
            if (pivotElement == 0.0) {
                return 0.0;
            }
            // if the found pivot element is not in the actual row,
            // we need to swap the rows..
            if (pivotRow != i) {
                // swapping
                for (int k = 0; k < order+1; ++k) { 
                    double t = workmatrix[Mindex(i,k)]; 
                    workmatrix[Mindex(i,k)] = workmatrix[Mindex(pivotRow,k)];
                    workmatrix[Mindex(pivotRow,k)] = t;
                }
                // change the sign of the determinant after swapping a line!
                det *= -1.0;
            }
            // The determinant is calculated by a multiplication of the diagonal
            // values of the upper triangle matrix part.
            // in this case, the pivot element of each column
            det *= pivotElement;
            // Gaussian triangular steps to reach a row echelon form of the matrix
            for (int row = i + 1; row < order+1; ++row) {
                for (int col = i + 1; col < order+1; ++col) {
                    workmatrix[Mindex(row,col)] -= workmatrix[Mindex(row,i)] * workmatrix[Mindex(i,col)] / pivotElement;
                }
            }
        }

    }
    return det;
}

//==============================================================
// learn:
// Adding a new pair of x and y values and
// solving the polynomial regression model with Cramers rule
void curve_fit::learn(double x, double y) {
    ++N;
    // find min and max values for x
    if(N == 1) {
        max_x_ = x;
        min_x_ = x;
    } else {
        if(x < min_x_)
            min_x_ = x;
        if(x > max_x_)
            max_x_ = x;
    }
    // order is -1 when the matrices are not allocated (undefines)
    if(order > -1) {
        // Example for matrix values
        // A second-order polynomial (or: parabolic curve)
        //      |                                 |  
        //      |     N       SUM(xi)   SUM(xi^2) |    M[0] M[1] M[2] 
        //  M = |  SUM(xi)   SUM(xi^2)  SUM(xi^3) |  = M[3] M[4] M[5] 
        //      | SUM(xi^2)  SUM(xi^3)  SUM(xi^4) |    M[6] M[7] M[8] 
        //      |                                 |  
        // Matrix notation:
        // aij for each element of the matrix:
        //       |                         |   |             |   |                |
        //       | a_i1_j1 a_i1_j2 a_i1_j3 |   | a11 a12 a13 |   | M[0] M[1] M[2] |
        //   M = | a_i2_j1 a_i2_j2 a_i2_j3 | = | a21 a22 a23 | = | M[3] M[4] M[5] |
        //       | a_i3_j1 a_i3_j2 a_i3_j3 |   | a31 a32 a33 |   | M[6] M[7] M[8] |
        //       |                         |   |             |   |                |  
        // to calculate the matrix index based on i and j, the function Mindex(i, j)
        // can be used..
        // 
        // First, add the new x values to the existing values of the matrix M[]:
        for(int j = 0; j < order+1; j++) {
            for(int i = 0; i < order+1; i++) {
                // fill the first column with pow() for each element
                if(j==0) {
                    M[Mindex(i, j)] += pow(x, i);
                } else {
                    if(i < order){
                        // fill with already calculated values from 
                        // the previous column to save calulation power
                        M[Mindex(i, j)] = M[Mindex(i+1, j-1)];
                    } else {
                        // calculate only last element of the column with pow()
                        M[Mindex(i, j)] += pow(x, i+j);
                    }
                }
            }
        }
        // fill the first element with the number of samples
        M[0] = N;

        // Secondly add the new y value to the vector b[]
        //      |              |
        //      |    SUM(yi)   |   b[0]
        //  b = |  SUM(xi*yi)  | = b[1]
        //      | SUM(xi^2*yi) |   b[2]
        //      |              | 
        for(int n = 0; n < order+1; n++) {
            b[n] += (pow(x, n)*y);
        }

        // Thirdly calculate the coefficients by dividing the determinants
        //  a0=det(M0)/det(M)
        //  a1=det(M1)/det(M)
        //  ...
        //  an=det(Mn)/det(M)
    
        double det_M = determinant(M);
        // create a work-matrix to calculate M0, M1, ... , Mn
        // according to Cramer's rule
        double Mn[(order+1)*(order+1)];
        for(int n = 0; n < order+1; n++) {
            for(int j = 0; j < order+1; j++) {
                for(int i = 0; i < order+1; i++) {
                    if(j == n)
                        Mn[Mindex(i, j)] = b[i];
                    else
                        Mn[Mindex(i, j)] = M[Mindex(i, j)];
                }
            }
            // calculate the coefficients by dividing the determinants
            double det_Mn = determinant(Mn);
            a[n] = det_Mn / det_M;
        }
    }
}

//==============================================================
// predict:
// returning the predicted y values of a given x values
// based on the calculated (learned) polynomial regression model
double curve_fit::predict(double x) {
    double y = 0.0;
    for(int i = 0; i <= order; i++)
        y = y + pow(x, i) * a[i];
    return y;
}

//==============================================================
// predict:
// returning the predicted y values of a given x values
// based on the calculated (learned) polynomial regression model
// if x is outside the learned range, y is replaced with outside_value
double curve_fit::predict(double x, double outside_value) {
    double y =0.0;
    if(x >  max_x_ || x < min_x_){
        y = outside_value;
    } else {
        y = predict(x);
    }
    return y;
}

//==============================================================
// Fills the given field with the current coefficients. 
// the values field need to have the right size!
void curve_fit::get_coefficients(double values[]) {
    // order 0 -->  y = a[0]
    // order 1 -->  y = a[1]*x + a[0]
    // order 2 -->  y = a[2]*x^2 + a[1]*x + a[0]
    for(int i = 0; i <= order; ++i)
        values[i] = a[i];
}

//==============================================================
// Return the formula as String with all coefficients. 
// The number of decimal places can be set the optional parameter.
String curve_fit::get_formula(uint8_t decimals) {
    // order n -->  y = a[n]*x^n + ... + a[1]*x + a[0]
    String formula = "("+String(N)+") y= ";
    for(int i = order; i >= 0; --i) {
        // at a '+' if the value is positiv
        if(a[i] > 0 && i < order)
            formula += "+";
        if(i > 1) {
            formula += String(a[i],decimals)+"x^"+i+" ";
        } else {
            if(i== 1)
                formula += String(a[i],decimals)+"x ";
            else
                formula += String(a[i],decimals);
        }
    }
    return formula;
}

//==============================================================
// return the actual order. 
uint32_t curve_fit::get_order() {
   return order; 
}

//==============================================================
// return the matrix index based on i and j
// M[0][0] = M[0]
// M[0][3] = M[3]
// M[i][j] = M[(i*(order+1))+j]
uint32_t curve_fit::Mindex(uint32_t i, uint32_t j) {
    return (i*(order+1))+j;
}

//==============================================================
// return an estimation of the max y value over the existing x 
// range (min_x .. max_x)
// The optional parameter "steps" can be used to define the
// number of steps between min_x and max_x (default = 100)
double curve_fit::estimate_max_y(uint32_t steps) {
    if(steps == 0)
        steps = 1;
    double stepwidth = (max_x_ - min_x_) / steps;
    double max_y_ = predict(min_x_);
    for(int i = 1; i <= steps; ++i) {
        if(predict(min_x_ + (i*stepwidth)) > max_y_)
            max_y_ = predict(min_x_ + (i*stepwidth));
    }
    return max_y_;
}

//==============================================================
// return an estimation of the min y value over the existing x 
// range (min_x .. max_x)
// The optional parameter "steps" can be used to define the
// number of steps between min_x and max_x (default = 100)
double curve_fit::estimate_min_y(uint32_t steps) {
    if(steps == 0)
        steps = 1;
    double stepwidth = (max_x_ - min_x_) / steps;
    double min_y_ = predict(min_x_);
    for(int i = 1; i <= steps; ++i) {
        if(predict(min_x_ + (i*stepwidth)) < min_y_)
            min_y_ = predict(min_x_ + (i*stepwidth));
    }
    return min_y_;
}

github repository

complete code

Credits

Hans-Günther Nusseck

Hans-Günther Nusseck

17 projects • 55 followers
Just a guy who can't pass by any hardware without wondering how it works. Managing robot based industrial automation projects for living.

Comments