MPU causing jitters in kitty

Often I have seen that the mpu adjustment returned from the adjust() function cause the kitty to jitter continuously, particularly when in a stationary position. I.E. the adjustment appears to be in harmonic sync with the mechanical motion of the kitty.

To correct this, I have added a Kalman filter on the value returned by the adjust() funtion. For those of you unfamiliar with the Kalman filter, it is a relatively simple mathematical technique to converge on the true value when the measured values fluctuates. Some IMU devices even have a kalman filter embedded in the device. I am using it to correct for harmonic vibrations.

Warning: the Kalman filter is dependent on two constant values: R and Q. R is the expected measurement error. Smaller values will cause the return value to closely follow the measured value. Q is the "transition error". Large values will cause it to track the measured value. So, you may need to play with these values.

The code changes (in OpenCat.h):

added:

// Kalman filter. x is the state, p is the error (covariance). z is the most recent measurement.

// Constants which affect it's behavior: q (transition error): large values make it track the

// measurement values, r (measurement error): small values cause it to track the measurement values.

float prevKalman[16]= {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};

float prevError[16]={1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0};

#define Q .005 // works for me. depends on how often you call the calibratePWM() funtion

#define R .003 // same as Q.

void kalman(float &xPrev, float &pPrev, float z)

{

float x = xPrev;

float p = pPrev + Q;

// update

float y = z - x; // difference between measurement and prediction

float kg = p * 1.0 / (p + R) ; // Kalman Gain

x = x + kg * y;

pPrev = (1 - kg) * p;

}

.

,

,

in the adjust() function: replace this

return (

#ifdef POSTURE_WALKING_FACTOR

(i > 3 ? postureOrWalkingFactor : 1) *

#endif

rollAdj + RollPitchDeviation[1] * adaptiveCoefficient(i, 1) );

}

with this:

float totalAdj =

#ifdef POSTURE_WALKING_FACTOR

(i > 3 ? postureOrWalkingFactor : 1) *

#endif

rollAdj + RollPitchDeviation[1] * adaptiveCoefficient(i, 1) ;

kalman(prevKalman[i],prevError[i],totalAdj);

return prevKalman[i];

}

What is going on: totalAdj is the sum of the roll adjustment plus the pitch adjustment. Instead of returning this value, it is sent to the Kalman filter. This changes the prevKalman[i] value to the predicted value for the current time, and adjust() returns this value.

This requires two float arrays: prevKalman[16], and prevError[16]. These hold the previously returned values for the 16 motor correction factors, and the 16 error values (calculated by the Kalman filter) for those motors. If you have only the 8 motors (like me), you can save some space by making their dimensions 8, and subtracting 8 from the index i in the yellow lines.

I haven't played extensively with this, so be aware: the Q and R values may be sub-optimal (or entirely wrong) for you.

In my version of openCat, I can change the speed of motion. So, the kitty can walk much faster than the released code, or much slower. This will affect the harmonic motion of kitty, and perhaps require the values of R and Q to change as the speed changes. I have not done that analysis yet. I'll post that when I've got results. I.E. R and Q may need to be variable, instead of constants.

Mark