add tests of constructing weight matrix in trajectrory gmm mapping and remove some asserts