Since you posted two blocks of code, it is not at all clear what "measurement" you are talking about. If it is the first program posted, post some examples of the output.
Basically, wherever you make a measurement, modify that measurement by including two lines similar to those in reply #1.