Investigate higher order FD scheme in TVR

[pavelkomarov]

Right now the TVR is just using first order FD. We can definitely fit more accurate derivatives by summing the CVXPY variables in such a way that it mimics a higher-order scheme.

[pavelkomarov]

There are also all of these lines at the end of _total_variation_regularized_derivative

dxdt_hat = (dxdt_hat[0:-1] + dxdt_hat[1:])/2 # take first order FD to smooth a touch
ddxdt_hat_f = dxdt_hat[-1] - dxdt_hat[-2]
dxdt_hat_f = dxdt_hat[-1] + ddxdt_hat_f # What is this doing? Could we use a 2nd order FD above natively?
dxdt_hat = np.hstack((dxdt_hat, dxdt_hat_f))

# fix first point
d = dxdt_hat[2] - dxdt_hat[1]
dxdt_hat[0] = dxdt_hat[1] - d

which appear to be doing smoothing and final adjustments to endpoints. I believe these kinds of tweaks should be unnecessary, but I'm finding their presence does help jerk at least, in the current code. Would a higher-order scheme do even better?

[pavelkomarov]

It totally works.

def _total_variation_regularized_derivative(x, dt, order, gamma, solver=None):
    mean = np.mean(x)
    std = np.std(x)
    if std == 0: std = 1 # safety guard
    x = (x-mean)/std

    x_vars = cvxpy.Variable(len(x))

    # calculate variables for the order^th derivative
    dx = x_vars
    for i in range(order):
        mid = dx[2:] - dx[:-2] # second order finite difference routine
        first = -3*dx[0] + 4*dx[1] - dx[2]
        last  = 3*dx[-1] - 4*dx[-2] + dx[-3]
        dx = cvxpy.hstack([first, mid, last]) / (2 * dt)
        if i == 0: # save the first expression for recovering derivative values
            dxdt_vars = dx

    prob = cvxpy.Problem(cvxpy.Minimize(
        cvxpy.sum_squares(x - x_vars) + gamma*cvxpy.sum(cvxpy.tv(dx)) ))
    prob.solve(solver=solver)

    x_hat = x_vars.value
    dxdt_hat = dxdt_vars.value

    return x_hat*std+mean, dxdt_hat*std

[pavelkomarov]

Compare to answers with the first-order integration-based method, and the plots don't look that different.

The first-order method has a big advantage in that it's faster, about 20x faster, 10ms instead of 200+. I tried a first-order differencing method in the forward direction in place of cumsum and find it's about 4x slower than the integration-based way. So we'll keep integrating.

[pavelkomarov]

I've also just tried using a trapezoidal integration rule in the TVR, rather than cumulative sum:

T = np.zeros((len(x), len(x))) # cumulative_trapezoid matrix
for i in range(1, len(x)):
    T[i, 0] = 0.5
    for j in range(1, i):
        T[i, j] = 1
    T[i, i] = 0.5

# Recursively integrate the highest order derivative to get back to the position. This is a first-
# order scheme, but it's very fast and tends to not do markedly worse than 2nd order. See #116
y = deriv_values
for i in range(order):
    #y = cvxpy.cumsum(y) + integration_constants[I]
    y = T @ y + integration_constants[I]

It doesn't help: It takes longer when run in the optimizer and fails with the default solver in some cases. Not great. I document this here for posterity, and I'll leave a comment in the code so no one is ever tempted to wonder whether more sophistication might be better.

I believe the reason behind these kinds of manipulations not doing better is that the TV regularization is merely trying to keep values on whatever level of derivative close to each other, and whether we calculate those derivative values super carefully or pretty granularly doesn't make a ton of difference in the cost function. Ultimately we're trying to get $x$ close to $y$, and some derivative layers down we're looking for flattening.