Implementierung eines Zustandreglers mit Beobachter

[Buell24 (Gast)]

Ich habe in Matlab-Simulink einen Zustandregler mit Beobachter für ein 
inverses Pendel ausgelegt und diskretisiert.
http://www.library.cmu.edu/ctms/ctms/state/obs.gif
Ich bin dabei analog zu:
http://www.library.cmu.edu/ctms/ctms/examples/pend/diginvss.htm
vorgegangen.

Nun möchte ich diesen Zustandsregler auf einem Mikrocontroller (Arduino 
Mega) implementieren.
Folgende Gleichungen habe ich aufgestellt:

x[n + 1] = Ax[n] + Bu[n]
wobei u[n] = - Kx[n]  (negativ rückgekoppelt)
x[n + 1] = Ax[n] - BKx[n] = (A-BK)x[n]
Mit dem Beobachter sieht das ganze so aus:
x^[n + 1] = (A-BK)x^[n] - L(y^-y)
x^ = predicted state, y^ = predicted output, y= measured outputs

Implementiert habe ich das ganze volgendermassen:

   .
   .
   .
   .

  //matrix A-B*K
const double matr_A_BK[4][4] =       {
  1.0013,    0.0112,   -0.0061,   -0.0020,
  0.2633,    1.2461,   -1.2260,   -0.4024,
  0.0012,    0.0012,    0.9947,    0.0081,
  0.2489,    0.2326,   -1.0661,    0.6201 };

const double matr_L[2][4] =       {
  2.6319,     -0.0108,
  173.1476,     -1.3975,
  -0.0086,      2.6289,
  -1.1521,    172.8517   };

const double matr_K[4] = {
-65.9759,  -61.7565,  314.0219,  100.8610 };

double state[4] = {
  0, 0, 0, 0};   //state vector

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

   .
   .
   .
   .

    p[0] = state[0];  //two predicted states (x^ and theta^)
    p[1] = state[2];
    read_current_distance_m1();
    s[1] = (distanceM1-initDistanceM1)/6573.75; //two sensors (x and 
theta)
    s[2] = (analogRead(10)-513.0);

    for(int i = 0; i < 4; i++) {
      for(int j = 0; j < 4; j++) {   // (A-BK)*state
        nstate[i] = nstate[i] + matr_A_BK[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] );
      }
    }

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

   .
   .
   .
   .


Leider muss aber ein Fehler im Code vorhanden sein, die Variable 
nstate[i] ist nämlich immer 0.00. Könnt Ihr mir sagen, ob dieser Ansatz 
der Implementierung überhaupt richtig ist?

[Buell24 (Gast)]

Kann mir niemand einen Rat dazu geben?
Ich wäre schon um einen Literaturverweis für die Implementierung eines 
Zustandsreglers dankbar...

[dunno.. (Gast)]

dein code sieht aber nicht so aus, oder...?

poste mal alles, und benutze die code- einbettungsfunktion des forums..

oder wenns zuviel wird, häng die datei an..

[Buell24 (Gast)]

Der Code wurde für einen Arduino Mega geschrieben und besteht aus zwei 
Dateien:

StateSpaceControl.ino:
Beinhaltet den Regel Algorithmus

RoboClaw.ino:
Beinhaltet die Kommunikation zum Motor Controller.

StateSpaceControl.ino:

#define SERIAL_BAUD 57600
#define INPUT_COUNT 5     //number of analog inputs
#define VDD 5000.0f       //Analog reference voltage in mV
#define PI 3.14159265358979f

//DIGITAL OUTPUT
int ledGREENpin = 2;
int ledREDpin = 3;
int oscilloscopePin = 8;  //oscilloscope pin to work out the program loop time
boolean oscilloscopeState = false;

//Analog Input
float poti8 = 0;
float poti9 = 0;


//Global variables Motor Controller-----------------------------------------------------------------------------------------------
float level = 0;
int speedMotor1;
long distanceM1=0;
long initDistanceM1=0;
  
  //matrix A-B*K
const double matr_A_BK[4][4] =       { 
  1.0013,    0.0112,   -0.0061,   -0.0020,
  0.2633,    1.2461,   -1.2260,   -0.4024,  
  0.0012,    0.0012,    0.9947,    0.0081,  
  0.2489,    0.2326,   -1.0661,    0.6201 };

const double matr_L[2][4] =       {    
  2.6319,     -0.0108,
  173.1476,     -1.3975,
  -0.0086,      2.6289,
  -1.1521,    172.8517   };

const double matr_K[4] = {      
-65.9759,  -61.7565,  314.0219,  100.8610 };

double state[4] = {
  0, 0, 0, 0};   //state vector

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


void setup() {
  Serial.begin(SERIAL_BAUD);
  setupRoboClaw();

  //digital outputs
  pinMode(ledREDpin, OUTPUT);
  pinMode(ledGREENpin, OUTPUT);
  pinMode(oscilloscopePin, OUTPUT);

  digitalWrite(ledREDpin, HIGH);
  digitalWrite(ledGREENpin, LOW);
  Serial.println("start");
  read_current_distance_m1();
  initDistanceM1=distanceM1;
}

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 = millis();
  long last = millis();


  while(1) {     //-------------CYCLE
    digitalWrite(ledREDpin, LOW);
    digitalWrite(ledGREENpin, HIGH);
    now = millis();

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

    // -L*pred + L*measures
    p[0] = state[0];  //two predicted states (x^ and theta^)
    p[1] = state[2];
    read_current_distance_m1();
    s[0] = (distanceM1-initDistanceM1)/6573.75;     //two sensors (x and theta)
    s[1] = (analogRead(10)-511.0)*0.00405890523719604651162790697674;


    for(int i = 0; i < 4; i++) {
      for(int j = 0; j < 4; j++) {   // _________(A-BK)*state
        nstate[i] = nstate[i] + matr_A_BK[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] );
      }
      Serial.print(" nstate[");
      Serial.print(i);
      Serial.print("]= ");
      printDouble(nstate[i],10);
    }
    Serial.println(" end 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]);
    }
    level = force * 6666.6;
    set_motor();

  }//end of while(1)
} 

void printDouble( double val, byte precision){
  // prints val with number of decimal places determine by precision
  // precision is a number from 0 to 6 indicating the desired decimial places
  // example: printDouble( 3.1415, 2); // prints 3.14 (two decimal places)

  Serial.print (int(val));  //prints the int part
  if( precision > 0) {
    Serial.print("."); // print the decimal point
    unsigned long frac;
    unsigned long mult = 1;
    byte padding = precision -1;
    while(precision--)
   mult *=10;

    if(val >= 0)
  frac = (val - int(val)) * mult;
    else
  frac = (int(val)- val ) * mult;
    unsigned long frac1 = frac;
    while( frac1 /= 10 )
  padding--;
    while(  padding--)
  Serial.print("0");
    Serial.print(frac,DEC) ;
  }
}

RoboClaw.ino:

#define address 128
#define ROBOCLAW_BAUD 38400

signed char Motor1percent = 0;
signed char Motor2percent = 0;
int incomingByte= 0;

void setupRoboClaw()
{
  Serial1.begin(ROBOCLAW_BAUD);
  delay(100);
  set_min_main_voltage_value(15);   //9V   formula: (voltage-6)*5
  set_max_main_voltage_value(82);   //16V  formula: voltage*5.25
  set_min_logic_voltage_value(15);  //9V   formula: (voltage-6)*5
  set_max_logic_voltage_value(82);  //16V  formula: voltage*5.25
  delay(100);
  set_speed_m1_m2(0,0);

}

/*
** 2 - Set Minimum Main Voltage
 ** Sets main battery (B- / B+) minimum voltage level. If the battery voltages drops below the set
 ** voltage level Robo Claw will shut down. The value is cleared at start up and must set after each
 ** power up. The voltage is set in .2 volt increments. A value of 0 sets the minimum value allowed which
 ** is 6V. The valid data range is 0 - 120 (6V - 30V). The formula for calculating the voltage is: (Desired
 ** Volts - 6) x 5 = Value. Examples of valid values are 6V = 0, 8V = 10 and 11V = 25. Example with
 ** Robo Claw address set to 128:
 **
 ** Serout P15, i19200, [128, 2, 25, (165 & 0X7F)]
 */

void set_min_main_voltage_value(byte min_main_battery_voltage)
{
  byte serial_data[4];
  serial_data[0]=address;
  serial_data[1]=2;
  serial_data[2]=min_main_battery_voltage;
  serial_data[3]=(serial_data[0] + serial_data[1] + serial_data[2]) & 0x7F;

  Serial1.write(serial_data, sizeof(serial_data));
}

/*
** 3 - Set Maximum Main Voltage
 ** Sets main battery (B- / B+) maximum voltage level. The valid data range is 0 - 154 (0V - 30V). If
 ** you are using a battery of any type you can ignore this setting. During regenerative breaking a back
 ** voltage is applied to charge the battery. When using an ATX type power supply if it senses anything
 ** over 16V it will shut down. By setting the maximum voltage level, Robo Claw before exceeding it will
 ** go into hard breaking mode until the voltage drops below the maximum value set. The formula for
 ** calculating the voltage is: Desired Volts x 5.12 = Value. Examples of valid values are 12V = 62, 16V
 ** = 82 and 24V = 123. Example with Robo Claw address set to 128:
 **
 ** Serout P15, i19200, [128, 3, 82, (213 & 0X7F)]
 */

void set_max_main_voltage_value(byte max_main_battery_voltage)
{

  byte serial_data[4];
  serial_data[0]=address;
  serial_data[1]=3;
  serial_data[2]=max_main_battery_voltage;
  serial_data[3]=(serial_data[0] + serial_data[1] + serial_data[2]) & 0x7F;

  Serial1.write(serial_data, sizeof(serial_data));
}


void set_min_logic_voltage_value(byte min_logic_battery_voltage)
{
  byte serial_data[4];
  serial_data[0]=address;
  serial_data[1]=26;
  serial_data[2]=min_logic_battery_voltage;
  serial_data[3]=(serial_data[0] + serial_data[1] + serial_data[2]) & 0x7F;

  Serial1.write(serial_data, sizeof(serial_data));
}

void set_max_logic_voltage_value(byte max_logic_battery_voltage)
{
  byte serial_data[4];
  serial_data[0]=address;
  serial_data[1]=27;
  serial_data[2]=max_logic_battery_voltage;
  serial_data[3]=(serial_data[0] + serial_data[1] + serial_data[2]) & 0x7F;

  Serial1.write(serial_data, sizeof(serial_data));
}

void set_speed_m1_m2(long speedM1, long speedM2) {
  byte ab[11];
  ab[0] = address;
  ab[1] = 37;
  ab[2] = (speedM1 >> 24) & 0xff;
  ab[3] = (speedM1 >> 16) & 0xff;
  ab[4] = (speedM1 >> 8) & 0xff;
  ab[5] = (speedM1 >> 0) & 0xff;
  ab[6] = (speedM2 >> 24) & 0xff;
  ab[7] = (speedM2 >> 16) & 0xff;
  ab[8] = (speedM2 >> 8) & 0xff;
  ab[9] = (speedM2 >> 0) & 0xff;
  ab[10] = (ab[0]+ab[1]+ab[2]+ab[3]+ab[4]+ab[5]+ab[6]+ab[7]+ab[8]+ab[9]) & 0x7f;
  Serial1.write(ab, sizeof(ab));
}

void read_current_distance_m1()
{
  byte ab[2];
  ab[0] = address;
  ab[1] = 16;
  Serial1.write(ab, sizeof(ab));
  delay(10);
  
  byte distanceM1_byte[4];
    // send data only when you receive data:
  int i=0;
  while (Serial1.available() > 0) {
    if (i<4){
      distanceM1_byte[i] = Serial1.read();
    }
    i++;
  }
    distanceM1 = ((long )distanceM1_byte[0]) << 24;
    distanceM1 |= ((long )distanceM1_byte[1]) << 16;
    distanceM1 |= ((long )distanceM1_byte[2]) << 8;
    distanceM1 |= distanceM1_byte[3];
}

Der debug output sieht wie folgt aus:

start
 nstate[0]= 0.0003028782 nstate[1]= 0.0737256000 nstate[2]= -12.0398380160 nstate[3]= 0.0000000000 end for
 nstate[0]= 0.0072283688 nstate[1]= 48.0991425344 nstate[2]= -787.0603735872 nstate[3]= 13.0303843360 end for
 nstate[0]= -3.0112114600 nstate[1]= 3091.0715704576 nstate[2]= 16104.0000000000 nstate[3]= 858.1392766592 end for
 nstate[0]= -194.0648187712 nstate[1]= -2546.0000000000 nstate[2]= -21931.0000000000 nstate[3]= -11586.0000000000 end for
 nstate[0]= -12197.0159733968 nstate[1]= -11260.0000000000 nstate[2]= -32240.0000000000 nstate[3]= -22153.0000000000 end for
 nstate[0]= 20980.0000000000 nstate[1]= -21632.0000000000 nstate[2]= 0.0000000000 nstate[3]= 3568.0000000000 end for
 nstate[0]= 1824.0000000000 nstate[1]= 0.0000000000 nstate[2]= 0.0000000000 nstate[3]= 0.0000000000 end for
 nstate[0]= 0.0000000000 nstate[1]= 0.0000000000 nstate[2]= 0.0000000000 nstate[3]= 0.0000000000 end for
 nstate[0]= 0.0000000000 nstate[1]= 0.0000000000 nstate[2]= 0.0000000000 nstate[3]= 0.0000000000 end for
 nstate[0]= 0.0000000000 nstate[1]= 0.0000000000 nstate[2]= 0.0000000000 nstate[3]= 0.0000000000 end for
 nstate[0]= 0.0000000000 nstate[1]= 0.0000000000 nstate[2]= 0.0000000000 nstate[3]= 0.0000000000 end for
 nstate[0]= 0.0000000000 nstate[1]= 0.0000000000 nstate[2]= 0.0000000000 nstate[3]= 0.0000000000 end for
 nstate[0]= 0.0000000000 nstate[1]= 0.0000000000 nstate[2]= 0.0000000000 nstate[3]= 0.0000000000 end for
 nstate[0]= 0.0000000000 nstate[1]= 0.0000000000 nstate[2]= 0.0000000000 nstate[3]= 0.0000000000 end for
 nstate[0]= 0.0000000000 nstate[1]= 0.0000000000 nstate[2]= 0.0000000000 nstate[3]= 0.0000000000 end for
 nstate[0]= 0.0000000000 nstate[1]= 0.0000000000 nstate[2]= 0.0000000000 nstate[3]= 0.0000000000 end for
 nstate[0]= 0.0000000000 nstate[1]= 0.0000000000 nstate[2]= 0.0000000000 nstate[3]= 0.0000000000 end for
 nstate[0]= 0.0000000000 nstate[1]= 0.0000000000 nstate[2]= 0.0000000000 nstate[3]= 0.0000000000 end for

[Buell24 (Gast)]

Kann mir damit niemand helfen? Ich denke der Fehler wird irgendein 
Overflow sein. Ich kann ihn nur nicht finden!!

[Oliver (Gast)]

Pack dein Programm in den Debugger, und steppe da Schritt für Schritt 
durch. Da musst du selber durch, die Arbeit nimmt die keiner ab.

Oliver