**Filter**

The main purpose of a filter is to maintain object state (such as position, orientation and velocity), accept valid measurements (such as encoder and gyro readings) and reject measurements if they are erroneous. For example, if gyro was reporting 1 deg change every measurement interval and then suddenly reports 50 deg change, then there must be some sort of abnormal condition and filter will reject it.

Once filter rejects a measurement you may wish to do various things, depending on your experience with the specific sensor. For example, if you observe that every time your robot bumps into a pile of balls the gyro will jump and then continue tracking rotation rate correctly, you may want to reset gyro angle and start tracking its change after a few measurement cycles. However, if you notice that, after you collide with another robot or a wall, the reading out of gyro are total garbage you may want to start ignoring its readings until gyro goes through a 1.5 sec hardware reset, while the robot is not moving.

Filter will use accepted measurements to update the object state. As was already mentioned in another thread, filter uses weighted coefficients. Essentially, the filter keeps measurement statistics about each sensor to know how much it should be trusted. If one sensor says robot moved to the left and another says it moved to the right then filter will move internal state proportional to the trustworthiness of each sensor.

If you have a group of good sensors and one that keeps reporting erroneous results all the time, the filter will quickly figure out whom to trust and will start ignoring the outliers. However, in the real life it is more complicated. All sensors will have some noise and errors in their measurements, with some being less accurate than the others. As a result, the state out of the filter will be “jumpy”. To prevent that you need to “smooth” the measurements over several intervals and try to model physics more precisely.

For example, you know that your position cannot suddenly jump over the long distance if you are applying small motor power, unless you are being bumped by another robot or one of your wheels loses traction with the ground. In addition to filtering and smoothing of the observations good systems will model a lot of physics and many of the abnormal events. We will try to model, at least, the “normal” conditions and try to detect abnormal events. Then you could flash an indicator light to let operator know that we lost a lock on a good solutions and need some sort of a reset.

**Smoother**

Had it been senior college level project, I would say that everybody needs to learn math behind optimal Kalman Filter and implement Variable or Fixed Lag Smoother. But we are only in the middle school and this picture from the linked chapter should be enough:

Essentially, it illustrates that smoother defers judgment about trustworthiness of each single observation for several measurement intervals. While filter provides instant preliminary state, the smoother will have more refined state that lags in time, but provides more accurate estimates despite considerable noise in the input measurements.

Let’s get back to our problem:

Our state vector will have: position (x,y), orientation (heading angle), linear velocity (Vx,Vy) and rotational velocity (w) at any given moment of time. State vector contains parameters that we want to actively estimate. In addition to those six variables we will need to keep some information about their error bounds or how much we trust each of those numbers. In the formal filter theory we would need to keep 6x6 (or larger if we have more state variables) covariance matrix, but we will see if we could get away with something smaller and still get useful results.

Our sensors will include four quad encoders - one for each wheel, and a gyro. Potentially, you can include an accelerometer to detect abnormal events, line followers, to detect line crossings, or even additional gyros if you think it will somehow decrease your errors. But I think the latter is overkill - you should be able to do much better with relatively simple gyro error modelling.

For each sensor we will keep some measurement statistics, which lets us estimate how much we trust that particular sensor, how much error the sensor has accumulated so far, how much it drifts over time, and anything else we might need to model that sensor. If those numbers change over time we add them to our state and actively estimate, otherwise we collect calibration parameters for each sensor and then keep them as constants in our code. However, you may need to re-calibrate those sensors once in while.

Finally, in order to model your system you will need to input requested motor power, current battery voltage and approximate robot mass and inertia of the drive train components. Read down below and you will understand why.

**Prediction Step**

The way Filter-Smoother pair works is that at a given moment of time you know your current state and try to predict your next state at the next measurement time. For instance, if you know what power level was applied to each motor, you will know approximate force produced by each motor and, after considering drivetrain inertia and robot mass, you will know direction and magnitude of the acceleration vector. Acceleration is the rate of change of your velocity. Knowing current velocity and acceleration you can predict your velocity at the end of prediction time interval. You apply similar logic to both linear and rotational velocities and at the end could predict the new position and rotation angle (around center of mass) at the final time. Once you know that you can predict what reading each of the quad encoders and gyro will have at the next meas time.

If you have line follower sensors you can predict when line crossing event will occur and by the time difference of when it actually happens you can correct your state estimates.

**Residuals**

The difference between predicted and observer sensor value is a residual. If all residuals are zeros we know that we have perfectly modeled state. If residuals are non-zero we need to correct our state. We use our sensor trustworthiness estimation to know which sensors “pull more weight”.

Finally, after the state is corrected, we look back to see how accurate was each of the sensors and recompute their trustworthiness for the next step.

**State Vector Update**

The trick is to know how much each state parameter should change given that specific sensor produced certain residual. For example, how much (x) and (y) need to be corrected if quad encoder on the left-front wheel missed it’s predicted value by 3 ticks?

Those who’ve been to high school or college could sense that we are talking about Partial derivatives. For the rest of us, I just want to say that there will be relatively simple formulas for each drive train configuration, and knowing basic trigonometry should be sufficient to code them up.