Matrix and State Space Arduino Implementation

Question

I wrote an Arduino Matrix class. Then I used this class to implement State Space on Arduino. During tests I stomped upon a problem. After starting the program it stops at some point. I have no idea why this is happening. There are no error messages. Can you please tell me what im doing wrong?

Here is my code:

//"Matrix.h"
#ifndef _Matrix_h
#define _Matrix_h

#if defined(ARDUINO) && ARDUINO >= 100
    #include "arduino.h"
#else
    #include "WProgram.h"
#endif
class Matrix
{
private:
    unsigned row_number = 0;
    unsigned column_number = 0;
    double* elems;

public:

    //Constructors + Destructor
    Matrix();
    Matrix(double, unsigned row_number = 1, unsigned column_number = 1);
    Matrix(double*, unsigned row_number = 1, unsigned column_number = 1);
    Matrix(const char*);
    Matrix(const Matrix&);
    void operator=(Matrix);
    ~Matrix();

    //Comunication Methods
    unsigned get_rows() const { return row_number; };
    unsigned get_columns() const { return column_number; };
    double& operator()(const unsigned row, const unsigned column);
    double valueAt(unsigned row, unsigned column) const ;
    void print() const;
    bool mRead2();

    //Matrix Operations
    Matrix operator+(Matrix);
    Matrix operator-(Matrix);
    Matrix operator*(Matrix);
    Matrix transpose();


    //Scalar Operations
    Matrix operator+(double);
    Matrix operator-(double);
    Matrix operator*(double);
    Matrix operator/(double);

};
Matrix mRead(unsigned row,unsigned column);
#endif

//"Matrix.cpp"

#include "Matrix.h"
Matrix::Matrix() {
  row_number = 0;
  column_number = 0;
  elems = new double[0];
}
Matrix::Matrix(double val, unsigned row_number, unsigned column_number) {
  this->row_number = row_number;
  this->column_number = column_number;
  elems = new double[row_number * column_number + 1];
  for (unsigned i = 1; i < row_number * column_number + 1; i++)
    elems[i] = val;

}

Matrix::Matrix(double* elems, unsigned row_number, unsigned column_number)
{
  this->column_number = column_number;
  this->row_number = row_number;
  this->elems = new double[row_number * column_number + 1];
  for (unsigned i = 1; i < row_number * column_number + 1; i++)
    this->elems[i] = elems[i - 1];
}


Matrix::Matrix(const char*)
{

}


void Matrix::operator=(Matrix A){
  this->column_number = A.get_columns();
  this->row_number = A.get_rows();
  this->elems = new double[row_number * column_number + 1];
  for (unsigned i = 1; i <= row_number; i++)
    for (unsigned j = 1; j <= column_number; j++)
      this->elems[(i - 1) * (column_number) + j] =  A.valueAt(i, j);

}

Matrix::Matrix(const Matrix& A) {
  this->column_number = A.get_columns();
  this->row_number = A.get_rows();
  elems = new double[row_number * column_number + 1];
  for (unsigned i = 1; i <= row_number; i++)
    for (unsigned j = 1; j <= column_number; j++)
      this->elems[(i - 1) * (column_number) + j] =  A.valueAt(i, j);

}


Matrix::~Matrix()
{
  delete[] elems;
}

double& Matrix::operator()(const unsigned row, const unsigned column)
{

    return this->elems[(row - 1) * (column_number) + column ];


}

double Matrix::valueAt(unsigned row, unsigned column) const
{

    return this->elems[(row - 1) * (column_number) + column ];

}

void Matrix::print() const
{
  for (unsigned i = 1; i <= get_rows(); i++) {
    for (unsigned j = 1; j <= get_columns(); j++) {
      Serial.print(this->valueAt(i, j));
      Serial.print(" ");
      Serial.flush();
    }
    Serial.print("\n");
  }
}
Matrix mRead(unsigned row, unsigned column) {
  int sizeM = row * column;
  double m[sizeM];
  char buffor[64];
  for (int i = 0; i < 64; i++) {
    buffor[i] = 0;
  }
  for (int i = 0; i < sizeM; i++) {m[i]=0;}
  for (int i = 0; i < sizeM; i++) {
    while (!Serial.available()) {
      Serial.write(32);
    }
    int j = 0;
    while (Serial.available() > 0) {
      Serial.println(Serial.available());
      buffor[j] = Serial.read();
      j++;
      Serial.flush();
    }
    m[i] = (double)atof(buffor);
    Serial.flush();
    //for (int i = 0; i < 64; i++) {
    //  buffor[i] = 0;
    //}
  }
  return Matrix(m, row, column);
}
bool Matrix::mRead2() {
  int sizeM = row_number * column_number;
  double m[sizeM];
  char buffor[64];
  for (int i = 0; i < 64; i++) {
    buffor[i] = 0;
  }
  for (int i = 0; i < sizeM; i++) {m[i]=0;}
  for (int i = 0; i < sizeM; i++) {
    while (!Serial.available()) {
      Serial.write(32);
    }
    int j = 0;
    while (Serial.available() > 0) {
      Serial.println(Serial.available());
      buffor[j] = Serial.read();
      j++;
      Serial.flush();
    }
    m[i] = (double)atof(buffor);
    Serial.flush();
    for (int i = 0; i < 64; i++) {
      buffor[i] = 0;
    }
  }
  *this=Matrix(m,row_number,column_number);
  return true;
}
Matrix Matrix::operator+(Matrix A)
{
    Matrix sum = Matrix(0.0, row_number, column_number);
    for (unsigned i = 1; i <= row_number; i++)
      for (unsigned j = 1; j <= column_number; j++)
        sum(i, j) = A.valueAt(i, j) + this->valueAt(i, j);

    return sum;
}

Matrix Matrix::operator-(Matrix A)
{
    Matrix diff = Matrix(0.0, row_number, column_number);
    for (unsigned i = 1; i <= row_number; i++)
      for (unsigned j = 1; j <= column_number; j++)
        diff(i, j) = A.valueAt(i, j) + this->valueAt(i, j);
    return diff;

}

Matrix Matrix::operator*(Matrix A)
{
  Matrix mult = Matrix(0.0, row_number, A.get_columns());
    double temp;
    for (unsigned i = 1; i <= row_number; i++) {
      for (unsigned j = 1; j <= A.get_columns(); j++) {
        temp = 0.0;
        for (unsigned k = 1; k <= column_number; k++)
          temp += this->valueAt(i, k) * A(k, j);
        mult(i, j) = temp;
      }
    }

    return mult;
}

Matrix Matrix::transpose() {
  Matrix transpose = Matrix(0.0, row_number, column_number);
  for (unsigned i = 1; i <= row_number; i++) {
    for (unsigned j = 1; j <= column_number; j++) {
      transpose(i, j) = this->valueAt(j, i);
    }
  }
  return transpose;
}

Matrix Matrix::operator+(double x)
{
  Matrix A = Matrix(x, row_number, column_number);
  return *this + A;
}

Matrix Matrix::operator-(double x)
{
  Matrix A = Matrix((-1) * x, row_number, column_number);
  return *this + A;
}

Matrix Matrix::operator*(double x)
{
  Matrix A = Matrix(*this);
  for (unsigned i = 1; i <= row_number; i++)
    for (unsigned j = 1; j <= column_number; j++)
      A(i, j) = this->valueAt(i, j) * x;
  return A;

}

Matrix Matrix::operator/(double x)
{
  if (x != 0) {
    Matrix A = Matrix(*this);
    for (unsigned i = 1; i <= row_number; i++)
      for (unsigned j = 1; j <= column_number; j++)
        A(i, j) = this->valueAt(i, j) / x;
    return A;
  }
  else {} //throw "Dividing by zero";
}

//"StateSpace.h"

#ifndef _StateSpace_h
#define _StateSpace_h
#include "C:\Users\Wojciech Trybulec\Desktop\Test\Matrix.h"
class StateSpace
{
private:
    Matrix State;
    Matrix A;
    Matrix B;
    Matrix C;
    Matrix D;
    Matrix Out;
public:

    //Constructors + Destructor
    StateSpace();
    StateSpace(Matrix A, Matrix B, Matrix C, Matrix D);
    StateSpace(Matrix A, Matrix B, Matrix C, Matrix D,Matrix InitState);
    StateSpace(const StateSpace&);
    StateSpace operator=(StateSpace);
    ~StateSpace();

    //Comunication Methods
    Matrix get_states() const;
    Matrix get_output() const;
    Matrix* get_all() const;
    void calculate(Matrix Signal);
    void print_states() const;
    void print_output() const;
    void print_matrices() const;

};
#endif

//"StateSpace.cpp"
#include "C:\Users\Wojciech Trybulec\Desktop\Test\Matrix.h"
#include "StateSpace.h"
StateSpace::StateSpace() {
    A = Matrix();
    B = Matrix();
    C = Matrix();
    D = Matrix();
    State = Matrix();
    Out = Matrix();
}
StateSpace::StateSpace(Matrix A, Matrix B, Matrix C, Matrix D) {
    this->A = A;
    this->B = B;
    this->C = C;
    this->D = D;
    this->State = Matrix(0.0,A.get_columns(),1);
    this->Out = Matrix(0.0, C.get_rows(), 1);
}
StateSpace::StateSpace(Matrix A,Matrix B, Matrix C, Matrix D,Matrix InitState){
    this->A = A;
    this->B = B;
    this->C = C;
    this->D = D;
    this->State = InitState;
    this->Out = Matrix(0.0, C.get_rows(), 1);
}
StateSpace::StateSpace(const StateSpace& ss) {
    this->A = ss.get_all()[0];
    this->B = ss.get_all()[1];
    this->C = ss.get_all()[2];
    this->D = ss.get_all()[3];
    this->State = ss.get_all()[4];
    this->Out = ss.get_all()[5];
}
StateSpace StateSpace::operator=(StateSpace ss) {
    this->A = ss.get_all()[0];
    this->B = ss.get_all()[1];
    this->C = ss.get_all()[2];
    this->D = ss.get_all()[3];
    this->State = ss.get_all()[4];
    this->Out = ss.get_all()[5];
}
StateSpace::~StateSpace() {
    delete this;
    //A.~Matrix;
    //B.~Matrix;
    //C.~Matrix;
    //D.~Matrix;
    //State.~Matrix;
    //Out.~Matrix;
}

Matrix StateSpace::get_states() const {
    return this->State;
}
Matrix StateSpace::get_output() const{
    return this->Out;
}
Matrix* StateSpace::get_all() const {
    Matrix all[6];
    all[0] = A;
    all[1] = B;
    all[2] = C;
    all[3] = D;
    all[4] = State;
    all[5] = Out;
    return all;

}
void StateSpace::calculate(Matrix Signal) {
    State = ((A * State) + (B * Signal));
    Out = ((C * State) + (D * Signal));
}
void StateSpace::print_states() const {
    State.print();
}
void StateSpace::print_output() const {
    Out.print();
}
void StateSpace::print_matrices() const {
    A.print();
    B.print();
    C.print();
    D.print();
}

And this Arduino sketch that im using for testing:

#include "Matrix.h"
#include "StateSpace.h"
double a[4]={-0.5,0,0,0.7};
double b[2]={1,2};
double c[4]={1,0,0,1};
double d = 0;
Matrix A=Matrix(a,2,2);
Matrix B=Matrix(b,2,1);
Matrix C=Matrix(c,2,2);
Matrix D=Matrix(d,1,1);
Matrix x=Matrix(0.0,2,1);
Matrix U=Matrix(1.0,1,1);
StateSpace ss=StateSpace(A,B,C,D);
void setup()
{
  Serial.begin(9600);  
}
void loop() {
  ss.calculate(U);
  x=ss.get_states();
  Serial.print(x.valueAt(1,1));
  Serial.print("\t");
  Serial.println(x.valueAt(2,1));

  delay(100);
}

I really hope that you can help me

Answer 1

This is not a solution (meaning to pinpoint the exact problem), but to find out yourself. There are mainly two ways to debug this:

1. On Arduino: Try placing Serial.print statements to see where it stops (like putting them on the beginning of every function, or on other 'strategic' places. When you found which function stops, try putting print statements within. So within two 'runs' you can find which line is causing the problem. What I normally do is just number the print statements, e.g. function 1 you print values 100..200, next function 200.300 etc. So you can easily leave them for later reusage (just comment them out).

2. Unit test on PC: If you didn't already do, test your class on a PC... this will give you much better debugging tools than an Arduino. When you know the class itself work, create a small test sketch on the Arduino (but high likely it will work, unless you made a mistake in the test sketch). For a PC, consider writing a so-called unit-test ... so even later when you change something, you can test you didn't break anything else.

In both cases (if you start on PC also do this), check for memory usage. The Arduino has only 2 KB SRAM, on a PC you can already check how much is used. The SRAM usage shown during compiling only takes into account global variables. It does not take into account:

• Local variables
• Dynamic variables (made with new, malloc)
• Stack (passed parameters).