Skip to content

Commit

Permalink
switch from arrays to vectors for matrix implementation
Browse files Browse the repository at this point in the history
  • Loading branch information
LiamSmego committed Mar 23, 2021
1 parent 5235c4e commit 60e8bb8
Show file tree
Hide file tree
Showing 5 changed files with 63 additions and 104 deletions.
8 changes: 4 additions & 4 deletions main/main.ino
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ void send_tvc(Matrix u, Matrix * last_u, double yaw)

//rotate input are body z axis
float gamma=yaw+BETA;
float rotation_values [4]={cos(gamma), sin(gamma), -sin(gamma), cos(gamma)};
vector<float> rotation_values {cos(gamma), sin(gamma), -sin(gamma), cos(gamma)};
Matrix R=Matrix(2, 2, rotation_values);
u=R*u;

Expand Down Expand Up @@ -153,13 +153,13 @@ void loop()
else
{
ws.construct_y();
//ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt); //state estimation only using sensors
ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt); //state estimation only using sensors
}
break;
}
case(FINAL_COUNTDOWN):
{
if (change_mode_to_prep_tvc(millis()>ws.next_mode_time))
if (change_mode_to_prep_tvc(millis() > ws.next_mode_time))
{
transition_to_prep_tvc();
ws.next_mode_time=millis()+PREP_TVC_PERIOD*KILO_I;
Expand All @@ -168,7 +168,7 @@ void loop()
else
{
ws.construct_y();
//ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt); //state estimation only using sensors
ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt); //state estimation only using sensors
}
break;
}
Expand Down
116 changes: 37 additions & 79 deletions main/matrix.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,71 +1,32 @@
#include "matrix.hh"
#include <string>
#include <vector>

using namespace std;

//matrix subtraction helper function
float * m_sub_helper(float * A, float * B, int elements) {
float * C;
C = new float[elements];
for (int i = 0; i < elements; i++) {
C[i] = A[i] - B[i];
}
//delete C;
return C;
}

//scalar multiplication helper function
float * s_mult_helper(float * A, float rho, int elements) {
float * C;
C = new float[elements];
for (int i = 0; i < elements; i++) {
C[i] = A[i] * rho;
}
//delete C;
return C;
}

//matrix multiplication helper function
float * m_mult_helper(float * A, float * B, int m, int n, int p) {
float * C;
C = new float[m*p];
for (int i = 0; i < m; i++)
{
for (int j = 0; j < p; j++)
{
C[i*p + j] = 0;
for (int k = 0; k < n; k++)
{
C[i*p + j] += A[i*n + k] * B[j + p * k];
}
}
}
//delete C;
return C;
}

//matrix addition helper function
float * m_add_helper(float * A, float * B, int elements) {
float * C;
C = new float[elements];
for (int i = 0; i < elements; i++) {
C[i] = A[i] + B[i];
}
//delete C;
return C;
}

//matrix multiplication
Matrix Matrix::operator*(Matrix B)
{
const int m=rows;
const int p=B.columns;
const int n=columns;
float * new_values;
int m=rows;
int p=B.columns;
int n=columns;

vector<float> new_values (m*p, 0);

if (columns==B.rows)
{
new_values=m_mult_helper(values, B.values, m, n, p);
for (int i = 0; i < m; i++)
{
for (int j = 0; j < p; j++)
{
for (int k = 0; k < n; k++)
{
new_values[i*p + j] += values[i*n + k] * B.values[j + p * k];
}
}
}
}

return Matrix(m, p, new_values);
}

Expand All @@ -76,19 +37,26 @@ Matrix Matrix::operator+(Matrix B)
const int n_b=B.columns;
const int n_a=columns;
const int m_b=B.rows;
float * new_values;

vector<float> new_values (m_a*n_a, 0);

if ((m_a==m_b)&&(n_a==n_b))
{
new_values=m_add_helper(values, B.values, m_a*n_a);
for (int i = 0; i < m_a*n_a; i++) {
new_values[i] = values[i] + B.values[i];
}
}

return Matrix(m_a, n_a, new_values);
}

//scalar multiplication function
Matrix Matrix :: scale(float k)
{
float * new_values;
new_values=s_mult_helper(values, k, rows*columns);
vector<float> new_values (rows*columns, 0);
for (int i = 0; i < rows*columns; i++) {
new_values[i] = values[i] * k;
}
return Matrix(rows, columns, new_values);
}

Expand All @@ -99,30 +67,20 @@ Matrix Matrix::operator-(Matrix B)
const int n_b=B.columns;
const int n_a=columns;
const int m_b=B.rows;
float * new_values;
if ((m_a==m_b)&&(n_a==n_b))
{
new_values=m_sub_helper(values, B.values, m_a*n_a);
}
return Matrix(m_a, n_a, new_values);
}

string display_matrix(Matrix A)
{
std::string result;
for (int i=0; i<A.rows; i++)
vector<float> new_values (m_a*n_a, 0);

if ((m_a==m_b)&&(n_a==n_b))
{
for (int j = 0; j < A.columns; j++)
{
result+= A.select(i+1,j+1);
result+= " ";
for (int i = 0; i < m_a*n_a; i++) {
new_values[i] = values[i] - B.values[i];
}
result+="\n";
}
return result;

return Matrix(m_a, n_a, new_values);
}

void Matrix:: redefine(float* update_values)
void Matrix:: redefine(vector<float> update_values)
{
values=update_values;
}
Expand Down
8 changes: 3 additions & 5 deletions main/matrix.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,9 +12,9 @@ class Matrix
public:
int rows;
int columns;
float * values;
vector<float> values;

Matrix(int row_number, int column_number, float * mat_values)
Matrix(int row_number, int column_number, vector<float> mat_values)
{
rows=row_number;
columns=column_number;
Expand All @@ -26,13 +26,11 @@ class Matrix
return values[(row-1)*columns+(column-1)];
}

void redefine(float*);
void redefine(vector<float>);
Matrix operator+(Matrix);
Matrix operator-(Matrix);
Matrix scale (float);
Matrix operator*(Matrix);
};

string display_matrix(Matrix);

#endif
33 changes: 18 additions & 15 deletions main/mission_constants.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,9 @@
#define __MISSION_CONSTANTS_HH__

#include "matrix.hh"
#include <vector>

using namespace std;

//*****************************************************************************
// CONVERSION FACTORS
Expand Down Expand Up @@ -86,38 +89,38 @@ const float MOMENT_INERTIA_YY=1;//TODO: make exact
const float MASS=1;

//control constants TODO: fill these out
const float A_VALUES [36] = {0,THRUST/MASS,0,0,0,0,
0,0,1,0,0,0,
0,0,0,0,0,0,
0,0,0,-THRUST/MASS,0,0,
0,0,0,0,0,1,
0,0,0,0,0,0}; //dynamics matrix
const Matrix A=Matrix(6, 6, A_VALUES);
vector<float> A_VALUES {0,THRUST/MASS,0,0,0,0,
0,0,1,0,0,0,
0,0,0,0,0,0,
0,0,0,-THRUST/MASS,0,0,
0,0,0,0,0,1,
0,0,0,0,0,0}; //dynamics matrix
Matrix A=Matrix(6, 6, A_VALUES);

const float B_VALUES [12] = {THRUST/MASS, 0,
vector<float> B_VALUES {THRUST/MASS, 0,
0, 0,
-THRUST*MOMENT_ARM/MOMENT_INERTIA_YY, 0,
0, -THRUST/MASS,
0, 0,
0, -THRUST*MOMENT_ARM/MOMENT_INERTIA_XX}; //input matrix
const Matrix B=Matrix(6, 2, B_VALUES);
Matrix B=Matrix(6, 2, B_VALUES);

const float K_VALUES [12] = {0,0,0,0,0,0,
vector<float> K_VALUES {0,0,0,0,0,0,
0,0,0,0,0,0}; //controller gain
const Matrix KC=Matrix(2, 6, K_VALUES);
Matrix KC=Matrix(2, 6, K_VALUES);

const float C_VALUES [24] = {1,0,0,0,0,0,
vector<float> C_VALUES {1,0,0,0,0,0,
0,0,1,0,0,0,
0,0,0,1,0,0,
0,0,0,0,0,1}; //sensor matrix
const Matrix C=Matrix(4, 6, C_VALUES);
Matrix C=Matrix(4, 6, C_VALUES);

const float L_VALUES [24] = {0, 0, 0, 0,
vector<float> L_VALUES {0, 0, 0, 0,
0, 0, 0, 0,
0, 0, 0, 0,
0, 0, 0, 0,
0, 0, 0, 0,
0, 0, 0, 0}; //kalman gain
const Matrix L=Matrix(6, 4, L_VALUES);
Matrix L=Matrix(6, 4, L_VALUES);

#endif
2 changes: 1 addition & 1 deletion main/workspace.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,7 @@ class Workspace
Matrix y=Matrix(4, 1, initialize_4);
Matrix u=Matrix(2, 1, initialize_2);
Matrix last_u=Matrix(2, 1, initialize_2);
float y_values [4]= {0, 0, 0, 0};
vector<float> y_values {0, 0, 0, 0};
float yaw;

// state
Expand Down

0 comments on commit 60e8bb8

Please sign in to comment.