diff --git a/examples-gallery/non_contiguous_parameter_identification_bias_solution.npy b/examples-gallery/non_contiguous_parameter_identification_bias_solution.npy new file mode 100644 index 0000000..01b1b8f Binary files /dev/null and b/examples-gallery/non_contiguous_parameter_identification_bias_solution.npy differ diff --git a/examples-gallery/plot_non_contiguous_parameter_identification_bias.py b/examples-gallery/plot_non_contiguous_parameter_identification_bias.py new file mode 100644 index 0000000..a442363 --- /dev/null +++ b/examples-gallery/plot_non_contiguous_parameter_identification_bias.py @@ -0,0 +1,291 @@ +# %% +""" +Parameter Identification from Noncontiguous Measurements with Bias +================================================================== + +In parameter estimation it is common to collect measurements of a system's +trajectories from distinct experiments. For example, if you are identifying the +parameters of a mass-spring-damper system, you may excite the system with +different initial conditions multiple times and measure the position of the +mass. The data cannot simply be stacked end-to-end in time and the +identification run because the measurement data would be discontinuous between +each measurement trial. + +A workaround in opty is to create a set of differential equations with unique +state variables for each measurement trial that all share the same constant +parameters. You can then identify the parameters from all measurement trials +simultaneously by passing the uncoupled differential equations to opty. + +If the measurements are biased in addition to being noisy, one way to handle +this is to double the set of differential equations created above, where the +second set is a shifted copy of the first set. + +Mass-spring-damper-friction Example +=================================== + +The position of a simple system consisting of a mass connected to a fixed point +by a spring and a damper and subject to friction is simulated and recorded as +noisy measurements with bias. The spring constant, the damping coefficient +and the coefficient of friction will be identified. + +**State Variables** + +- :math:`x_i`: position of the mass of the i-th measurement trial [m] +- :math:`u_i`: speed of the mass of the i-th measurement trial [m/s] + +**Parameters** + +- :math:`m`: mass [kg] +- :math:`c`: linear damping coefficient [Ns/m] +- :math:`k`: linear spring constant [N/m] +- :math:`l_0`: natural length of the spring [m] +- :math:`friction`: friction coefficient [N] + +""" +import sympy as sm +import sympy.physics.mechanics as me +import numpy as np +from scipy.integrate import solve_ivp +from opty import Problem +from opty.utils import parse_free +import matplotlib.pyplot as plt + +# %% +# Equations of Motion. +# -------------------- +# +# Set up as twice as many of equations of motion as there are measurements. +# The second part of these eoms are dependent on the first part by +# configuration constraints. +# +number_of_measurements = 6 +t0, tf = 0.0, 10.0 +num_nodes = 500 + +m, c, k, l0, friction = sm.symbols('m, c, k, l0, friction') +bias_ident = sm.symbols(f'bias:{number_of_measurements}') + +t = me.dynamicsymbols._t + +x = me.dynamicsymbols(f'x:{2*number_of_measurements}') +u = me.dynamicsymbols(f'u:{2*number_of_measurements}') + +N = me.ReferenceFrame('N') +O = me.Point('O') +O.set_vel(N, 0) + +Points = sm.symbols(f'P:{2*number_of_measurements}', cls=me.Point) +for i in range(2*number_of_measurements): + Points[i].set_pos(O, x[i]*N.x) + Points[i].set_vel(N, u[i]*N.x) +bodies = [me.Particle('part_' + str(i), Points[i], m) + for i in range(2*number_of_measurements)] +# %% +# :math:`\tanh(\alpha \cdot x) \approx sign(x)` for large :math:`\alpha`, and +# is differentiale everywhere. +forces = [(Points[i], -c*u[i]*N.x - k*(x[i]-l0)*N.x - + friction * sm.tanh(30*u[i])*N.x) for i in range(2*number_of_measurements)] + +q_ind = x[: number_of_measurements] +q_dep = x[number_of_measurements :] +u_ind = u[: number_of_measurements] +u_dep = u[number_of_measurements :] + +kd = sm.Matrix([u[i] - x[i].diff(t) for i in range(2*number_of_measurements)]) + +config_constr = sm.Matrix([x[i] - x[number_of_measurements + i] + 1*bias_ident[i] + for i in range(number_of_measurements)]) +speed_constr = config_constr.diff(t) + +KM = me.KanesMethod( + N, + q_ind=q_ind, + q_dependent=q_dep, + u_ind=u_ind, + u_dependent=u_dep, + kd_eqs=kd, + velocity_constraints=speed_constr, + configuration_constraints=config_constr, +) + +fr, frstar = KM.kanes_equations(bodies, forces) +eom =kd.col_join(fr+frstar) +eom = eom.col_join(config_constr) + +for i in range(2): + sm.pprint(eom[i]) +for i in range(2): + print('.........') +for i in range(2): + sm.pprint(eom[2*number_of_measurements+i]) +for i in range(2): + print('.........') +for i in range(2): + sm.pprint(eom[-(2-i)]) + + +# %% +# Generate Noisy Measurement Data with Bias +# ----------------------------------------- +# Create 'number_of_measurements' sets of measurements with different initial +# conditions. To get the measurements, the equations of motion are integrated, +# and then noise is added to each point in time of the solution. Finally a bias +# is added. +# +rhs = KM.rhs() +states = x + u +parameters = [m, c, k, l0, friction] + list(bias_ident) +par_vals = [1.0, 0.25, 2.0, 1.0, 0.5] + [0] * number_of_measurements + +eval_rhs = sm.lambdify(states + parameters, rhs) + +times = np.linspace(t0, tf, num=num_nodes) + +measurements = [] +np.random.seed(123) +# %% +# As the second half of x, u are a shifted copy of the first half, they must get +# identical initial conditions. +start1_list, start2_list = [], [] +for i in range(number_of_measurements): + start1 = 4.0*np.random.randn(number_of_measurements) + start2 = 4.0*np.random.randn(number_of_measurements) + + x0 = np.hstack((start1, start1, start2, start2)) + sol = solve_ivp(lambda t, x, p: eval_rhs(*x, *p).squeeze(), + (t0, tf), x0, t_eval=times, args=(par_vals,)) + + measurements.append(sol.y[1, :] + 1.0*np.random.randn(num_nodes)) +# %% +# Add bias to the measurements +bias = 4.0 *np.random.uniform(0., 1., size=number_of_measurements) +measurements = np.array(measurements) + bias[:, np.newaxis] + +print(measurements.shape) + +# %% +# Setup the Identification Problem +# -------------------------------- +# +# The goal is to identify the damping coefficient :math:`c`, the spring +# constant :math:`k` and the coefficient of friction :math:`friction`. +# The objective :math:`J` is to minimize the least square +# difference in the optimal simulation as compared to the measurements. If +# some measurement is considered more reliable, its weight :math:`w` may be +# increased relative to the other measurements. +# +# +# objective = :math:`\int_{t_0}^{t_f} \left[ \sum_{i=1}^{i = \text{number_of_measurements}} (w_i (x_i - x_i^m)^2 \right] \hspace{2pt} dt` +# +interval_value = (tf - t0) / (num_nodes - 1) + +w =[1 for _ in range(number_of_measurements)] + +nm = number_of_measurements +def obj(free): + return interval_value * np.sum([w[i] * np.sum( + (free[(nm+i)*num_nodes:(nm+i+1)*num_nodes] - measurements[i])**2) + for i in range(nm)] +) + +def obj_grad(free): + grad = np.zeros_like(free) + for i in range(nm): + grad[(nm+i)*num_nodes:(nm+i+1)*num_nodes] = 2*w[i]*interval_value*( + free[(nm+i)*num_nodes:(nm+i+1)*num_nodes] - measurements[i] +) + return grad + + +# %% +# By not including :math:`c`, :math:`k`, :math:`friction`, :math:`bias_i` +# in the parameter map, they will be treated as unknown parameters. +par_map = {m: par_vals[0], l0: par_vals[3]} + +# %% +bounds = { + c: (0.01, 2.0), + k: (0.1, 10.0), + friction: (0.1, 10.0), +} | {u[i]: (-10, 10) for i in range(2*number_of_measurements)} + +problem = Problem( + obj, + obj_grad, + eom, + states, + num_nodes, + interval_value, + known_parameter_map=par_map, + time_symbol=me.dynamicsymbols._t, + integration_method='midpoint', + bounds=bounds, +) +# %% +# This give the sequence of the unknown parameters at the tail of solution. +print(problem.collocator.unknown_parameters) +# %% +# Create an Initial Guess +# ----------------------- +# +# It is reasonable to use the measurements as initial guess for the states +# because they would be available. Here, only the measurements of the position +# are used and the speeds are set to zero. The last values are the guesses +# for :math:`bias_i`, :math:`c` and :math:`k`, respectively. +# +initial_guess = np.hstack((np.random.randn(number_of_measurements*num_nodes), #x2 + np.array(measurements).flatten(), # x1 + np.zeros(2*number_of_measurements*num_nodes), # u + [0.5 for _ in range(number_of_measurements)], # bias + [0.1, 3.0, 2.0], # c, k, friction +)) + + +# %% +# Solve the Optimization Problem +# ------------------------------ +# +# Here, the initial guess is the result of some previous run, stored in +# 'non_contiguous_parameter_identification_bias_solution.npy'. +initial_guess = np.load('non_contiguous_parameter_identification_bias_solution.npy') +solution, info = problem.solve(initial_guess) +# %% +# This is how the solution may be saved for a future initial guess +# ```np.save('non_contiguous_parameter_identification_bias_solution', solution)``` + +print(info['status_msg']) +print(f'final value of the objective function is {info['obj_val']:.2f}' ) + +# %% +problem.plot_objective_value() + +# %% +problem.plot_constraint_violations(solution) + + +# %% +# The identified parameters are: +# ------------------------------ +# +print(f'Estimate of damping coefficient is {solution[-3]: 1.2f}') +print(f'Estimate of the spring constant is {solution[-1]: 1.2f}') +print(f'Estimate of the friction coefficient is {solution[-2]: 1.2f}') + +# %% +# Plot the measurements and the trajectories calculated. +# +fig, ax = plt. subplots(number_of_measurements, 1, + figsize=(6.4, 1.25*number_of_measurements), sharex=True, constrained_layout=True) +for i in range(number_of_measurements): + ax[i].plot(times, solution[(nm+i)*num_nodes:(nm+i+1)*num_nodes], color='red') + ax[i].plot(times, measurements[i], color='blue', lw=0.5) + ax[i].set_ylabel(f'{states[nm+i]}') +ax[-1].set_xlabel('Time [s]') +ax[0].set_title('Trajectories') +prevent_output = True + +# %% +# +# sphinx_gallery_thumbnail_number = 3 + +plt.show()