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^[n + 1] = (A-BK)x^[n] - 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)
}