# Implementing state space equations on Arduino

Hello!

I'm trying to implement discrete state space equations (like this one [1]) on my Arduino in order to obtain something like this http://www.engin.umich.edu/group/ctm/state/obs.gif.
My aim is to control an inverse pendulum.

Unfortunately the calculated force (last lines of [2]) which has to be applied to the pendulum is really close to 0 and I don't know why: sensors give right values and force shouldn't be 0.
I think it must be related with my poor knowledge of C/C++.

This is the code [2]; matrixes come from Matlab.
...Thanks for any hint!

[1]
x[n + 1] = Ax[n] + Bu[n]
where u[n] = - Kx[n] (negative feedback)
x[n + 1] = Ax[n] - BKx[n] = (A-BK)x[n]
x[1] = (A-BK)x[2] - L(y^-y)
x^ = predicted state, y^ = predicted output, y= measured outputs

[2]

``````#include ...
#define MULT 1000

//components (globals)
some definitions... m = motor

//matrixes
const double matr_A[ROW_A][COL_A] = { 1.0001,    0.0010,    0.0001,    0.0000,
0.1728,    1.0970,    0.1864,    0.0164,
-0.0010,   -0.0005,    0.9990,    0.0009,
-1.9200,   -1.0773,   -1.9625,    0.8178 };
const double matr_L[ROW_L][COL_L] = {    233.1,    -51.2,
7769,   -2082.1,
-22.9,    119.7,
-79.1,    466.2   };
const double matr_K[ROW_A] = {      -0.0492,   -0.0276,   -0.0541,   -0.0047 };

double state[4] = {0, 0, 0, 0};   //state vector
double nstate[4] = {0, 0, 0, 0};  //temporarly stores new state

void setup() {
...
}

void loop() {
double p[2] = {0,0}; //two predicted state
double s[2] = {0,0}; //two outputs
double force = 0;     //force that must be applied to motor
long now = 0;
long last = 0;

while(1) {                        //-------------CYCLE
now = millis();

while(last == now) {   //wait until 1 microsecond elapses
// sample time = 1 ms
delayMicroseconds(10);
now = millis();
}
last = now;

// -L*pred + L*measures
p[0] = state[0];  //two predicted states (x^ and theta^')
p[1] = state[3];
gyro.updateAngSpeed();
s[0] = m.getMetres();     //two sensors (x and theta')

for(int i = 0; i < 4; i++) {

for(int j = 0; j < 4; j++) {   // _________(A-BK)*state
nstate[i] = nstate[i] + matr_A[i][j]*state[j];
}

for(int j = 0; j < 2; j++) { //-L*pred + L*measures
nstate[i] = nstate[i] + matr_L[i][j]*( s[j] - p[j] );
}
}//____________________________________________end of for

//update states
for(int i = 0; i<4; i++) {
state[i] = nstate[i];
nstate[i] = 0; //init for following cycle
}

// controller action
force = 0;
for(int i = 0; i < 4; i++) { //controller action
force =  force + (state[i]*matr_K[i]);
}
force = (-1)*force*MOTORGAIN; //negative feedback
m.setSpeed((int) force);

}//end of while(1)
}
``````

1. n + 1 ↩︎

2. n ↩︎

I don't know if this is the problem but bear in mind that the compiler used by the arduino treats a double as a float (both are 32 bits). So your results will not be as precise as they would if calculated using 64 bit doubles

I don't know if this is the problem but bear in mind that the compiler used by the arduino treats a double as a float (both are 32 bits). So your results will not be as precise as they would if calculated using 64 bit doubles

Thanks, I converted all the doubles into floats and luckly the measures are still fine.

One problem I have is with conversions: how does the following code behave?

``````float f = 0.12345;
int i = 100;
int ris = f * i;

ris = 12  or  ris = 0?
``````

Does it first convert the float into an int?
What is the result?

Thank you.

C will convert the int to a float, do the math, and convert the result back to an int.
ris will equal 12

there is a brief overview here: C Class - Promotion, Conversion and Casts

C will convert the int to a float, do the math, and convert the result back to an int.
ris will equal 12

there is a brief overview here: C Class - Promotion, Conversion and Casts

Unfortunately I still don't understand where my mistake is...

I suggest you come up with some dummy sensor values and carry out the calculation by hand. Then use those same dummy sensor values in your program and print out intermediate values throughout the calculation to see if you can narrow down where things are going wrong.

• Ben

I posted a routine to print floats/doubles here: http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1207226548/6#6

I suggest you come up with some dummy sensor values and carry out the calculation by hand. Then use those same dummy sensor values in your program and print out intermediate values throughout the calculation to see if you can narrow down where things are going wrong.

• Ben

Thanks for the suggestion.
I'm still debugging: if I solve the problem I will let you know.

I posted a routine to print floats/doubles here: http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1207226548/6#6

Very useful, thanks!