Before advancing in Kalman developments, as incorporating the control input signals, I wanted to cleanup the code, make the tracking more robust and improve power efficiency of the robots (after I fully discharged a LiPo battery =/ ).
I am using a white background now over the floor, that results in a better tracking and also keep my robots clean, besides having quite good friction. The tracking code now uses two modes for identifying a robot, one for new robots and other that gives a more steady response after one robot has been identified.
Now I will go into the codes of the robots themselves to make them more power effient, entering in stand by mode if no messages are being sent.
Next steps are to model the response of the vehicle to speed input commands and use them into the Kalman filter, for optimal tracking.
