Move matrix code into its own package.
Fix bug in when we add id to the id list.