UDP send added

Send UDP data for analysing via Matlab, and bug corrections as well.