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

I would say the dynamic memory allocation is the culprit. Everytime you create a Matrix, you are allocating dynamic memory. And you are creating a lot of matrices. Even when deleting everthing correctly, you are making swiss cheese out of your memory, aka heap fragmentation. Rewrite your code to not use dynamic allocation — chrisl, Jun 2 '20 at 16:34
What the hell is this: delete this;? Are you trying to make recursion inside of destructor? Also forget this->, there is no need for it - it's not PHP — KIIV, Jun 3 '20 at 6:20
Btw, after longer look at that code - it's really bad. It could be much shorter, also bit faster as it's doing lots actions twice and even more. — KIIV, Jun 3 '20 at 6:35

Michel Keijzers · Accepted Answer · 2020-06-02 15:58:27Z

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

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).
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).

During PC debuging I noticed that it keeps using more and more memory. It probably doesnt destroy created objects. Could you please tell me how to fix this or at least give me some tips? — SanX, Jun 2 '20 at 15:23
I didn't look good enough, I see a lot of new statements which create dynamic variables. These are not counted. I don't have a quick fix, but for an Arduino with only 2 KB SRAM it's very unrecommendable to use dynamic memory (there are a few exceptions). Also when you assign or pass parameters, maybe those (copy?) constructors are used, creating a copy (maybe temporarily). But sometimes you might need a copy (if you want to keep the original). That's hard to say, since I don't know your requirements. — Michel Keijzers, Jun 2 '20 at 16:02
I added delete [] elems in operator=. It solved my problem. Thanks a lot for help. — SanX, Jun 2 '20 at 16:35
Great you found it (yourself) ... wonder why this didn't show up in Windows ... however, stay warned for memory issues. — Michel Keijzers, Jun 2 '20 at 17:24
@sanX you should read something about Rule of Three / Rule of Five. You'll find out adding delete into copy assigment operator and destructor is not enough. — KIIV, Jun 3 '20 at 6:16

Stack Exchange Network

current community

your communities

more stack exchange communities

Matrix and State Space Arduino Implementation

1 Answer 1

Your Answer

Not the answer you're looking for? Browse other questions tagged arduino-uno programming c++ library or ask your own question.

Hot Network Questions

Matrix and State Space Arduino Implementation

1 Answer 1

Your Answer

Sign up or log in

Post as a guest

Not the answer you're looking for? Browse other questions tagged arduino-uno programming c++ library or ask your own question.

Related

Hot Network Questions