void Window::paintNavTrace(QPainter& painter)
{
    painter.save();
    painter.setPen(QPen(Qt::green, 3, Qt::SolidLine));
    QPointF prev, next;
    double angle, stepLen;
    double deltaAngle = 0;//- 1.57 * 0.7;

    int n_last_steps = 150 ;
    double oldStepLen = 0.0;
    next = getScreenPoint(mClosestReferencePoints[mLocCounter].x,
                          mClosestReferencePoints[mLocCounter].y);

    for(int curLocCounter = mLocCounter; curLocCounter > 0 && curLocCounter > mLocCounter - n_last_steps; curLocCounter--) //
    {
        NavigationState curNavState = mNavigationStates[curLocCounter];
        stepLen = curNavState.getStepLen();
        if (curLocCounter > 0 && std::fabs(stepLen - oldStepLen) > 1e-6) {
            oldStepLen = stepLen;
            stepLen *= mScale;
            angle = curNavState.getHeading() + deltaAngle; //- firstStepAngle; //+ refAngle
            prev = QPointF(next.rx() - stepLen * std::cos(angle),
                           next.ry() + stepLen * std::sin(angle));
            painter.drawLine(prev.rx(), prev.ry(), next.rx(), next.ry());
            next = prev;
        }
    }
    painter.restore();
}