So over the past few weeks I've been stuck on what seems to be a very difficult problem. I have a quadrotor I'm working on and I have implemented a Kalman Filter on it. Now, the Kalman Filter is fairly simple and really the same as many of the ones posted here. Here's the hard part, reducing lag and minimizing noise.
Many of you here have posted on the forums and from the multiple examples I've found have chosen Q_angle = 0.001, Q_gyro = 0.003, and R_angle = 0.03
seem to be the usually constants for the Kalman Filter.
However, I found these constants to cause my Kalman Filter to fall into a completely unstable oscillation eventually getting so noisy that the estimated angle is jumping from -500deg to +500deg within milliseconds.
So before I continue I would like to note that my Kalman Filter is running at 1800Hz (many I have found run around 100Hz), so I'm not exactly sure how those constants would change based on sample rate. I ended up tuning the Q and R constants until I found a stable zone:
Q_angle = 0.001, Q_gyro = 0.012, and R_angle = 0.1
These would work, and when the motors turn on, the angle noise jumps from +-0.05deg to about +-1.00deg. Which is reasonable. Even though there great smooth lines there is a horrible lag and overshoot!!!
So I thought, "Ok, since the Q constant chooses how much you trust the raw values and R is the acceptable noise and convergence then I should tune the R." I ended up finding the following values to work:
Q_angle = 0.001, Q_gyro = 1.0, and R_angle = 0.0059
These values would work fairly well, and all the lag and overshoot was reduced, however as soon as I turn on the motors, it goes from +-0.75deg to +-5deg. So now I have too much noise being let through!
It's become very irritating tuning these values and I'm really stuck. It's become a battle between noise and lag. You either get a smooth but laggy signal or a noisy yet fast signal...
I've attached some pictures showing the red lines as the actual final angle the quad was tilted to, but you can see that in one the settle time is much larger but it's a clean signal (R=0.1) then in the other it's a noisy signal that's really quick to respond and settle with small overshoots (R=0.0059)
Any advice or ideas out there as to how I can get a smooth and fast signal without overshoot??
Here's the code:
const float Q_angle = 0.001f;
const float Q_gyro = 1.0f;
const float R_angle = 0.0059f;
float y;
float S;
float K[2];
float P[2][2] = ;
P[0][0] -= dt * (P[1][0] + P[0][1]) + Q_angle * dt;
P[0][1] -= dt * P[1][1];
P[1][0] -= dt * P[1][1];
P[1][1] += Q_gyro * dt;
angle[0] += (x_gyro_rad_sec[0] - bias[0])*dt;
y = acc_rad[0] - angle[0];
S = P[0][0] + R_angle;
K[0] = P[0][0] / S;
K[1] = P[1][0] / S;
angle[0] += K[0] * y;
bias[0] += K[1] * y;
P[0][0] -= K[0] * P[0][0];
P[0][1] -= K[0] * P[0][1];
P[1][0] -= K[1] * P[0][0];
P[1][1] -= K[1] * P[0][1];
Replies