Yoast Breadcrumbs add another level for the tag page

I have a page structure like this: Home Category Sub Category Tag, but when I visit the tag page, the breadcrumbs only shows: Home Category Sub Category where the tag is gone and the Sub Category isn't even linked.

How can I make sure that the Sub Category has a hyperlink and to add another level to the breadcrumbs, namely the current tag page?

I understand that I can override the yoast_breadcrumb() function in functions.php, but I do not understand how to add the link to the Sub Category or add the tag to the breadcrumbs.

I've tried with the code below, but $current_page only returns the sub category object, and not the tag object.

add_filter(wpseo_breadcrumb_links, wpse_100012_override_yoast_breadcrumb_trail);
function wpse_100012_override_yoast_breadcrumb_trail($links)
{
    $post_type = get_post_type();
    $current_page = sanitize_post($GLOBALS['wp_the_query']-get_queried_object());
    if (is_tag()) {
        $tag = get_queried_object();
            $breadcrumb[] = array(
                'url' = '/'.$current_page-slug,
                'text' = $current_page-slug
            );
        array_splice($links, 4, 0, $breadcrumb);
        }
    return $links;
}