Матрица и пространство состояний Реализация Arduino

Я написал матричный класс Arduino. Затем я использовал этот класс для реализации пространства состояний на Arduino. Во время тестов я наткнулся на проблему. После запуска программы она в какой-то момент останавливается. Я понятия не имею, почему это происходит. Сообщений об ошибках нет. Не могли бы вы сказать мне, что я делаю не так?

Вот мой код:

//"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();
}

И этот скетч Arduino, который я использую для тестирования:

#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);
}

Я очень надеюсь, что вы сможете мне помочь