Started to write sigma points class for unscented filter