Created averaging function for poses. Will fix main code and ARTag code to use this correctly.