WIP on the vector example
Working on a different version of the mk diag.

I use a pose diag function to make my tests.
Checkpoint: the first two tests are going through