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.
Can someone help, please?
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]
Adding an observer it becomes:
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')
    s[1] = gyro.getRadSpeed();
    
    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

Thanks for the link...
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!