Fix error in Chain matrix update.

Rotations were calculated in the wrong order
Merging the Chain fix into branch garoe
In costFun in chainModel simplified penalty calculation