Fix some bugs.

Now the test.m can work.