Matrix and State Space Arduino Implementation

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;

//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);

};
#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());
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);
}
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());
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 Jun 2, 2020 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, 2020 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, 2020 at 6:35

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).
• 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, 2020 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. Jun 2, 2020 at 16:02
• I added delete [] elems in operator=. It solved my problem. Thanks a lot for help.
– SanX
Jun 2, 2020 at 16:35
• Great you found it (yourself) ... wonder why this didn't show up in Windows ... however, stay warned for memory issues. Jun 2, 2020 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, 2020 at 6:16