add color comparison in to cost function & Trj source seletion(from predict or measurement)