Hi all:
Today my company had released my first fully developed project, an ultrasonic level controller.
While I was developing the firmware for this project (with an ATmega64), I was looking for a filter that could discriminate false echoes. First I thought to develop some kind of 'prediction' about where it should be, and if the measured echoes go out for a certain amount, then discard them.
Later, I found the Kalman filter, and I liked it, since it seems to lay about similar concepts: there is still no 'teletransportation', so anything that has some mass can move too far from it's later position. Also, under usual circumstances, it's speed also can change too much (rockets and crashes included), depending on sample rate.
For certain timing constraints I couldn't try Kalman filter, so I only implemented my original idea. But this week, I tried it, retrieving data at 2Hz rate from one of my prototypes in a noisy environment. It lays over my desk, looking towards the wall, aobut 4.90 m away, and with cold air from the air conditioner blowing trhough the 'ultrasonic beam'. This gives me a big amount of noise that dissapear if I shut off the air conditioner.
To simplify my first test, I modeled the 'process' (the wall position) as a constant one, giving me a pretty simple Kalman filter (black line). Also I use the same FIR filter (32 points low pass) that I use in the final product (red line), while the prototipe only gives me 'real readings' (blue line). Then I feed the output of the filter to itself, obtaining the 2* FIR filter (green line). Also I add a simple 5 points IIR filter (dark green) just to test.
Initialization of all filters is not well done, but results are evaluated much later.
At the bottom text boxes (all data is processed by a C# application) it writes the mean on the top one (except the true readings), and on the bottom one, it writtes the standard deviation of the last 128 points. FIR filters give a nice line, but its stadistical results are not the best, and also are slow for small movements.
Also I attached the C# code for this Kalman filters. Only 4 variables (can be reduced to 3), 3 adds, 2 substractions, 2 mult's and 1 division. Not too much job. :shock:
private float Kalamar(int val)
{
float aux;
K = P/(P + 64); //1 es sa variança estimada d'es renou de lectura
aux = Lestim + K*(val-Lestim); //correció 'meas update' a partir d'es nou valor
P = (1 - K)*P; //Correció d'es renou de predicció
Lestim = aux; //Predicció 'a priori estimate' per 'time update'
P = P + 0.1f; //Predicció d'es renou de procés. 25 es sa variança estimada de procés.
return aux;
}
Compare to the resources needed by a 32 points FIR filter. Seems that Kalman filter had passed this first round. Next one will be try to model a 'second order' one with speed in the state space to see if step response is faster. Right now, step response is fast only for steps within standar deviation, for steps bigger, response lays between 32 point FIR and 2* 32 point FIR.
If somebody has some experience, I will be glad to know.
Guillem.
