Expanded menu items makes entitysubmenu go haywire

Problem/Motivation

We have menu items where the users can select "Expand" or not (Drupal Core behaviour).
When using these options entity submenu block printed out all "Expanded" menu items regardless of the current menu item and it's active trail.

For example:
Start
Questions
- Questions one
Services
- Service one
Here we place a entity submenu block.
This would even print: questions and questions one and start ....

Steps to reproduce

Make use of the "Expand" checkboxes on a menu item.

Proposed resolution

See attached patch.
Copied page of core code that is in "SystemMenuBlock.php"

Remaining tasks

Review