x_offset = x_curr - PRIO*XREF*RMAX/r_ref          (5)

    x_diff   = x_curr - x_prev                        (6) 

                           delta    x_offset
    r_ref = r_ref - KAPPA*-------*------------*r_ref
                            TAU       TAU

                                x_diff
                  - KAPPA*ETA*---------*r_ref         (7)
                                 TAU