Example of the filter with scalar covariance values.
First of all, those who missed previous page in this thread: Collin has found an excellent filter tutorial for beginners.
Also, please, note that code below is not a Kalman Filter. Formal KF implementation requires processing of a square covariance matrix. We will implement only diagonals Pk] and will not do cross-correlations. I am hopeful that for our specific setup and the way how estimated states are mapped into physical robot’s sensors it wouldn’t even matter. We will see if this assumption holds.
Consider the same robot and filter setup described in post #57 of this thread. For each sensor we are going to add sensor noise (variance) R]. For each estimated state parameter Xk* we add a scalar covariance Pk*. Both R] and Pk] will start with some initial value determined experimentally. You may want to check posts on the previous page for some explanations.
long Tk1[3]; // timestamp of the sensor measurement (usec)
long Tpr[3]; // timestamp of the previous sensor measurement (usec)
short Zk[3]; // sensor measurements (internal units are sensor ticks)
float R[3]; // sensor variance (internal units are (sensor ticks)^2 )
// Zk[0] - gyro
// Zk[1] - left quad
// Zk[2] - right quad
long Tk; // time of the current state estimate (internal unit is usec)
float Xk[8]; // state estimates at a time Tk:
float Pk[8]; // Pk* is a covariance = uncertainty of value Xk*
// (internal unit for Pk* is whatever unit for Xk* is, but squared)
// Xk[0] - gyro bias (how much gyro had drifted from heading) (gyro ticks)
// Xk[1] - gyro drift rate (per unit of time) (gyro ticks/usec)
// Xk[2] - heading of the robot (gyro ticks)
// Xk[3] - angular velocity of robot body (gyro ticks/usec)
// Xk[4] - left wheel total travel (encoder ticks)
// Xk[5] - left wheel velocity (encoder ticks/usec)
// Xk[6] - right wheel total travel (encoder ticks)
// Xk[7] - right wheel velocity (encoder ticks/usec)
Predict step is the same as in previous example with addition of covariance:
float dt = 1.0 * ( Tk1 - Tk );
save Xk] to Xprev]
Xk[0] = Xk[0] + Xk[1] * dt; // gyroBias += drift * dt
Xk[2] = Xk[2] + Xk[3] * dt; // heading += angVel * dt
Xk[4] = Xk[4] + Xk[5] * dt; // leftDist += leftVel * dt
Xk[6] = Xk[6] + Xk[7] * dt; // rightDist += rightVel * dt
for each state i
{
float f = (Xprev*==0 /* or very small */ ? 1.0 : Xk*/Xprev* );
Pk* = f * Pk* * f + Q*;
}
Tk = Tk1;Where Q* is process noise for each estimated state. The meaning of the process noise is that it corresponds to the realistic uncertainty we expect to see from our filter.
For example, it would be equally unreasonable to expect our robot navigation to be either accurate to 10^-6 meters or only accurate to 0.5 meters. I would say ~1cm or 1/4 inch is a reasonable value to begin with.
Having good value for Q* ensures stable filter that both filters out sensor noise, but updates state estimates fast enough to keep up with the robot’s movement. As you calibrate the filter you will fine tune values for Q by trial and error.
Also, if you anticipate abnormal situations like bumping into another robot or a wall, you may want to program one time changes to Pk] and temporary changes to Q].
Code for the measurement updates will change a bit. If you take a look at EKF formulation*************, you will see that to map changes between sensor measurements and state values you need partial derivatives Hk]:
It is, actually, very easy to understand - it just sounds scary. Hk is, essentially, a measure of how much sensor reading (h) would change if the true value of what state (x) estimates was slightly different.
For example, if true value of Xk[5] (left wheel velocity) was larger by 1 tick/usec we would expect left quad reading to be larger by (1*dt) ticks after a period of time (dt).
Hk] for the left wheel encoder (from the previous example, and disregarding hardcoded constant 0.25) would be:
// Left quad partials (notice the inversion, because Hk is computed dZk/dXk)
float Hk[8] = { 0, 0, -1/Crot, -dt/Crot, 1, dt, 0, 0 };
// based on the code from the previous example:
//Xk[4] = Xk[4] + (0.25 ) * Yk; // left wheel
//Xk[5] = Xk[5] + (0.25/dt) * Yk; // left velocity
//Xk[2] = Xk[2] - Crot * (0.25 ) * Yk; // left wheel turns robot to the right
//Xk[3] = Xk[3] - Crot * (0.25/dt) * Yk; // dXk[3] = - Crot * dXk[5]
The complete pseudocode for measurement update would look like this:
for each sensor m
{
float dt = Tk1[m] - Tpr[m]; // time since previous Left quad meas update
float Yk, Hk[8]; // residual Yk = actual measurement - expected meas value
if( m==1 ) // left quad
{
Yk = Zk[m] - Xk[4];
Hk] = { 0, 0, -1/Crot, -dt/Crot, 1, dt, 0, 0 };
}
if( m==2 ) // right quad
{
Yk = Zk[m] - Xk[6];
Hk] = { 0, 0, -1/Crot, -dt/Crot, 0, 0, 1, dt };
}
if( m==0 ) // gyro
{
Yk = Zk - (Xk[2] + Xk[0]);
Hk] = { 1, dt, 1, dt, -2*Crot, -2*dt*Crot, 2*Crot, 2*dt*Crot };
}
for each i
{
float Sk = Hk**Pk**Hk* + R[m];
float Gk = Pk**Hk* / Sk;
float residRatio = Yk / sqrt(Sk);
if( residRatio < SomeSanityValue ) // accept residual and update state
{
Xk* = Xk* + Gk * Yk;
Pk* = Pk* - Gk*Hk**Pk*;
if( Pk*<0 ) // if Pk is negative - something went wrong
Pk*=0; // also print an error massage that it needs some debugging
}
else // reject residual
{
// in the physical world robot parameters can change only so much;
// for example, if gyro tells you that heading changed +60deg in 5 msec
// it is clearly wrong - you may want not to trust this gyro for a while
}
}
}
Similarly to process noise values Q], values for sensor noise (variance) R] may be temporarily changed if you expect bad readings from the sensor as a result of some abnormality.
Important disclaimers and limitations*************:
We still assume that robot doesn’t accelerate too fast and our predict step is linear over small time intervals.
In order to keep this example readable I decided not to estimate wheel slippage. As a result robot heading and wheel velocity change rates are locked by the formula Xk[3] = Crot * (Xk[7] - Xk[5]). Only two out of three states are independent.
With digital quad encoders and no slippage there is really no errors, except for time roundoffs when quad encoders tick. If this was the case in the real world, we wouldn’t need to use gyro and heading should not be in state vector at all. Instead, it should have been recomputed whenever wheel state change.
So look at this example only as an illustration of the principle, not as the ready-to-use code.
I will take a pause in posting further code to this thread and will actually try to implement the filter with direct gyro rate readings (QCC2), and modelling wheel slippage, to see if all of the assumptions are valid, and there is no other errors.
In the meantime, those who are trying to implement filters, please, post any questions or share your results - it should help move the thread along.*