PyNumDiff/pynumdiff/tests/test_optimize.py at 917382f9e4cd1c48c0c5975652c5bb150594e2c5 · florisvb/PyNumDiff · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
import numpy as np
from pytest import skip

from ..finite_difference import first_order as iterated_finite_difference
from ..smooth_finite_difference import mediandiff, meandiff, gaussiandiff, friedrichsdiff, butterdiff
from ..linear_model import spectraldiff
from ..polynomial_fit import polydiff, savgoldiff, splinediff
from ..total_variation_regularization import velocity, acceleration, iterative_velocity
from ..kalman_smooth import rtsdiff
from ..optimize import optimize
from ..utils.simulate import pi_cruise_control


# simulation
dt = 0.01
x, x_truth, dxdt_truth = pi_cruise_control(duration=2, noise_type='normal', noise_parameters=[0, 0.01], dt=dt)
cutoff_frequency = 10 # in Hz
log_gamma = -1.6 * np.log(cutoff_frequency) - 0.71 * np.log(dt) - 5.1
tvgamma = np.exp(log_gamma)


def test_finite_difference():
    params1, val1 = optimize(iterated_finite_difference, x, dt, dxdt_truth=dxdt_truth, padding='auto')
    params2, val2 = optimize(iterated_finite_difference, x, dt, tvgamma=tvgamma, dxdt_truth=None, padding='auto')
    assert params1['num_iterations'] == 5
    assert params2['num_iterations'] == 1

def test_mediandiff():
    params1, val1 = optimize(mediandiff, x, dt, dxdt_truth=dxdt_truth, search_space_updates={'num_iterations':1}, padding='auto')
    params2, val2 = optimize(mediandiff, x, dt, tvgamma=tvgamma, search_space_updates={'num_iterations':1}, padding='auto')
    assert params1['window_size'] == 5
    assert params2['window_size'] == 1

def test_meandiff():
    params1, val1 = optimize(meandiff, x, dt, dxdt_truth=dxdt_truth, search_space_updates={'num_iterations':1}, padding='auto')
    params2, val2 = optimize(meandiff, x, dt, tvgamma=tvgamma, search_space_updates={'num_iterations':1}, padding='auto')
    assert params1['window_size'] == 5
    assert params2['window_size'] == 1

def test_gaussiandiff():
    params1, val1 = optimize(gaussiandiff, x, dt, dxdt_truth=dxdt_truth, search_space_updates={'num_iterations':1}, padding='auto')
    params2, val2 = optimize(gaussiandiff, x, dt, tvgamma=tvgamma, search_space_updates={'num_iterations':1}, padding='auto')
    assert params1['window_size'] == 9
    assert params2['window_size'] == 1

def test_friedrichsdiff():
    params1, val1 = optimize(friedrichsdiff, x, dt, dxdt_truth=dxdt_truth, search_space_updates={'num_iterations':1}, padding='auto')
    params2, val2 = optimize(friedrichsdiff, x, dt, tvgamma=tvgamma, search_space_updates={'num_iterations':1}, padding='auto')
    assert params1['window_size'] == 9
    assert params2['window_size'] == 1

def test_iterative_velocity():
    params1, val1 = optimize(iterative_velocity, x, dt, dxdt_truth=dxdt_truth, search_space_updates={'num_iterations':1}, padding='auto')
    params2, val2 = optimize(iterative_velocity, x, dt, tvgamma=tvgamma, search_space_updates={'num_iterations':1}, padding='auto')

    np.testing.assert_almost_equal(params1['gamma'], 0.0001, decimal=4)
    np.testing.assert_almost_equal(params2['gamma'], 0.0001, decimal=4)

def test_velocity():
    try: import cvxpy
    except: skip("could not import cvxpy, skipping test_velocity")

    params1, val1 = optimize(velocity, x, dt, dxdt_truth=dxdt_truth, padding='auto', maxiter=20)
    params2, val2 = optimize(velocity, x, dt, tvgamma=tvgamma, padding='auto', maxiter=20)

    np.testing.assert_almost_equal(params1['gamma'], 0.0769, decimal=3)
    np.testing.assert_almost_equal(params2['gamma'], 0.010, decimal=3)

def test_acceleration():
    try: import cvxpy
    except: pytest.skip("could not import cvxpy, skipping test_acceleration")

    params1, val1 = optimize(acceleration, x, dt, dxdt_truth=dxdt_truth, padding='auto', maxiter=20)
    params2, val2 = optimize(acceleration, x, dt, tvgamma=tvgamma, padding='auto', maxiter=20)

    np.testing.assert_almost_equal(params1['gamma'], 0.147, decimal=3)
    np.testing.assert_almost_equal(params2['gamma'], 0.0046, decimal=4)

def test_savgoldiff():
    params1, val1 = optimize(savgoldiff, x, dt, dxdt_truth=dxdt_truth, padding='auto')
    params2, val2 = optimize(savgoldiff, x, dt, tvgamma=tvgamma, padding='auto')
    assert (params1['degree'], params1['window_size'], params1['smoothing_win']) == (7, 41, 3)
    assert (params2['degree'], params2['window_size'], params2['smoothing_win']) == (3, 3, 5)

def test_spectraldiff():
    params1, val1 = optimize(spectraldiff, x, dt, dxdt_truth=dxdt_truth, padding='auto')
    params2, val2 = optimize(spectraldiff, x, dt, tvgamma=tvgamma, padding='auto')
    np.testing.assert_almost_equal(params1['high_freq_cutoff'], 0.18, decimal=2)
    np.testing.assert_almost_equal(params2['high_freq_cutoff'], 0.45, decimal=2)

def test_polydiff():
    params1, val1 = optimize(polydiff, x, dt, dxdt_truth=dxdt_truth, search_space_updates={'step_size':1}, padding='auto')
    params2, val2 = optimize(polydiff, x, dt, tvgamma=tvgamma, search_space_updates={'step_size':1}, padding='auto')
    assert (params1['degree'], params1['window_size'], params1['kernel']) == (6, 50, 'friedrichs')
    assert (params2['degree'], params2['window_size'], params2['kernel']) == (3, 10, 'gaussian')

def test_rtsdiff_with_irregular_step():
    t = np.arange(len(x))*dt
    t_irreg = t + np.random.uniform(-dt/10, dt/10, *t.shape) # add jostle
    params1, val1 = optimize(rtsdiff, x, t, dxdt_truth=dxdt_truth)
    params2, val2 = optimize(rtsdiff, x, t_irreg, dxdt_truth=dxdt_truth)
    assert val2 < 1.1*val1 # optimization works and comes out similar, since jostle is small
    assert params1['qr_ratio']*0.9 < params2['qr_ratio'] < params1['qr_ratio']*1.1