Use Scalar type of matrix for assertions s.t. this works with Ceres Jet
Merge pull request #15 from ethz-asl/fix/scalar_type_in_assertions

Use Scalar type of matrix for assertions s.t. this works with Ceres Jet