added updateOrigSensorsVec() function to display correct PSO result