Add unit tests

- add unit test for cost
- add unit test for grad
- add unit test for numeric gradient