Update to new rebased (from upstream) Stonesoup repo