# How to apply Kalman filter in this case?

I have some straight and curve pieces with numbers, they are used to build tracks (of $5$ lanes) for my cars (figure $1$), I can send commands to the cars using an SDK on the Raspberry (set the speed for example ...), when a car is moving, a built-in downward facing camera scans the infrared markings on the track, a microcontroller inside the car decodes these markings to get the id of the position on the piece (see figure $2$) and the number of the piece, then a message PM=(piece number, position id, car linear velocity) is sent via Bluetooth low energy to a Raspberry. The car can send these messages AT LEAST every $0.2$ second. The problem is that the car can reach a linear velocity of $100cm/s$ and move quite a distance after it sends the PM message, so it will be a lag between the actual state of the car and the state contained in the received PM message. There is also the problem of delayed and lost packets containing PM messages because we use one Raspberry to communicate with $3$ cars via Bluetooth low energy (sometimes the Raspberry keeps receiving packets from one car even if the others try to send), which makes the lag larger. So, we cannot rely only on the received PM messages to determine the real-time position of the car.

Therefore I am trying to implement on the Raspberry a Kalman filter to estimate the position of the car on the track in real-time, but given the fact that I don’t have the $(x,y)$ coordinates of the car I don’t know how to apply the algorithm using (position id, number of the piece), also I want to know how to do so the update step can take into account the delayed and lost measurements.

When the car leaves a piece, it sends the wheels displacements but it is not sufficient I think, I have neither the heading nor the angular velocity of the car.

There is one thing that I think is very useful; if no commands are sent to the car, the microcontroller inside it ensures that the car will keep moving on the same lane and tries to maintain its linear velocity.

I would like to have your advice, opinions or ideas of another filter or algorithm, if you have any question, please ask me.

• One strong suggestion is to start with only one car on the track at a time. That way you may have fewer drop-outs from BT. On the other hand the Pi should be able to handle BT packets from 3 card at 200 Hz without too many problems. Of course that depends on what your program is doing to these packets. Does the computer running the game (not the microcontroller in the car) get more packets than you do? Could you bring your Pi closer to the game? – NomadMaker Jul 20 '18 at 19:03
• I have to use the three cars at the same time, yes The Pi can handle 3 connections but there are delays even if it's close to the cars (sometimes the Pi keeps receiving packets from only one car), if the speed is 100cm/s, in 0.2 seconds (5 Hz) the car can go 20 cm, so there will be always AT LEAST 20cm of error between the real position and the received position, and this is what I want to correct using a filter. – machine424 Jul 20 '18 at 19:19
• I'm sorry. I do agree that using a filter or some form of prediction is a good thing. I'm just curious as to how the computer running the game is getting more data from the cars than the Pi is. – NomadMaker Jul 20 '18 at 19:22
• I communicate with the cars using the Raspberry; receive messages or send commands (change speed ...) – machine424 Jul 20 '18 at 19:27
• One option is to time stamp the packets and discard any old ones. Alternately you could attempt to modify the old packets using knowldge of the track layout and making the assumption that the vehicle maintained constant velicity, although this would be wrong some if not all of the time. – Robert Sutton Jul 22 '18 at 23:07