I had to change the order of declaration for PitchMatrix3D, YawMatrix3D and RollMatrix3D because you use this functions before to the definition.