KalmanFilter ausreichend für Regelung, bzw. PD-Regelung falsch?

[Florian K. (f-kae)]

Hallo zusammen und wieder einmal habe ich eine Frage,

ich habe einen Kalmanfilter zur Sensordatenfilterung zusammengestellt.
Die Daten kommen von folgenden Sensoren:

-> Gyroskop: L3G4200D
-> Accelerometer: BMA180
-> Magnetometer: HMC5883

Die Kurven auf dem Bild(siehe Anhang) entsprechen folgendermaßen den 
Drehmomenten(Mx,My,Mz) - bzw. den Winkeln:

Rot -> Mx (Angabe des Winkels(in Grad) zwischen +Z-Achse und Y-Achse)
Blau -> My (Angabe des Winkels(in Grad) zwischen +Z-Achse und X-Achse)
Grün -> Mz (Angabe des Winkels(in Grad) zwischen NORDEN und X-Achse)

(Rot und Blau sind anfangs auf 0Grad weil ich in horizontaler Stellung 
auf Null sein möchte. Ich habe dafür einfach 90Grad auf die Achsen 
addiert.)

Kann man anhand dieses Bildes erkennen, ob die gefilterten Daten zur 
Regelung eines Multicopters ausreichen?

Leider gibt es immer wieder ein Rucken an den Motoren und allgemein 
wirkt die Ansprache der Motoren nicht sauber und ich weiß nicht ob meine 
PD-Regelung das Problem ist oder die gefilterten Daten.

Mein versuche einen PD-Regler zu programmieren sieht folgendermaßen aus:

float errorX = -anglesOutput.y/90; //anglesOutput.y = Winkel(inGrad) zwischen DownVektor und y-Achse ->RollWert
  float errorY = anglesOutput.x/90; //anglesOutput.x = Winkel(inGrad) zwischen DownVektor und x-Achse ->PitchWert
  float errorZ = (anglesOutput.z-globalstartingZ)/90; //anglesOutput.z = Winkel(inGrad) zwischen NorthVektor und rotem Ausleger(x-Achse) ->YawWert

  float DerrorX = (errorX - lasterrorX)/0.003;
  float DerrorY = (errorY - lasterrorY)/0.003;
  float DerrorZ = (errorZ - lasterrorZ)/0.003;

  float pParamXY = remoteData.ctrl6 * 10; //ctrl6 Poti
  float pParamZ = remoteData.ctrl8; //ctrl8 Poti
  float dParamXY = remoteData.ctrl7 * -2.5; //ctrl7 Poti
  float dParamZ = -0.1;

  if (remoteData.isValid && remoteData.g > 0.2){
    float x = remoteData.r; //zwischen -1 und 1
    float y = remoteData.p; //zwischen -1 und 1
    float z = remoteData.y; //zwischen -1 und 1
    errorX += x*0.2;
    errorY += y*0.2;
    errorZ += z*0.4;


    float uF = (remoteData.g - 0.2)/(1-0.2)*40;
    float uX = pParamXY * errorX + dParamXY*DerrorX;
    float uY = pParamXY * errorY + dParamXY*DerrorY;
    float uZ = pParamZ  * errorZ  + dParamZ*DerrorZ;

    actualMoments.uF = uF;
    actualMoments.uX = uX;
    actualMoments.uY = uY;
    actualMoments.uZ = uZ;

    // now calculate motor values from u (see limitMotorValue for limits)
    MotorValues motorValues(Motors::calculateMotorValues(actualMoments));
    actualMotorValues = motorValues;
  }
  else {
    actualMoments.uF = 0;
    actualMoments.uX = 0;
    actualMoments.uY = 0;
    actualMoments.uZ = 0;
    MotorValues motorValues = motorvalues_ZERO;
    actualMotorValues = motorValues;
  }

  lasterrorX = errorX;
  lasterrorY = errorY;
  lasterrorZ = errorZ;

Wäre Klasse wenn mir jemand weiterhelfen könnte, ich weiß nicht so recht 
wo ich nun Probleme ausfindigmachen könnte...