def plot_truth_vel(_ds, filename=None): fig = jpu.prepare_fig(window_title='Truth') ax = plt.subplot(2, 1, 1) #plt.plot(_ds.truth_stamp, _ds.truth_norm_vel, '.') plt.plot(_ds.truth_vel_stamp, _ds.truth_lvel_body[:, 0], '.', label='x') plt.plot(_ds.truth_vel_stamp, _ds.truth_lvel_body[:, 1], '.', label='y') plt.plot(_ds.truth_vel_stamp, _ds.truth_lvel_body[:, 2], '.', label='z') jpu.decorate(ax, title='Truth velocity', xlab='time in s', ylab='m/s', legend=True, xlim=None, ylim=None) ax = plt.subplot(2, 1, 2) plt.plot(_ds.truth_vel_stamp, _ds.truth_rvel[:, 0], '.', label='p') plt.plot(_ds.truth_vel_stamp, _ds.truth_rvel[:, 1], '.', label='q') plt.plot(_ds.truth_vel_stamp, _ds.truth_rvel[:, 2], '.', label='r') jpu.decorate(ax, title='Truth rotational velocity', xlab='time in s', ylab='rad/s', legend=True, xlim=None, ylim=[-2, 2]) jpu.savefig(filename)
def compute_derivatives(self): self.enc_vel_lw = self.data['lw_angle'][1:] - self.data['lw_angle'][:-1] #self.enc_vel_rw = self.enc_rw[1:] - self.enc_rw[:-1] self.enc_dt = self.data['stamp'][1:] - self.data['stamp'][:-1] self.enc_vel_lw /= self.enc_dt #self.enc_vel_rw /= self.enc_dt self.enc_vel_stamp = (self.data['stamp'][:-1] + self.data['stamp'][1:]) / 2 fs, cutoff = 100., 2 #3.2 # sampling freq and desired cutoff (Hz) nyq = 0.5 * fs normal_cutoff = cutoff / nyq order = 2 # 2.0*M_PI/TIME_CONSTANT with TIME_CONSTANT = 1 b, a = scipy.signal.butter(order, normal_cutoff, btype='low', analog=False) #self.enc_vel_f_lw = scipy.signal.filtfilt(b, a, self.enc_vel_lw) self.enc_vel_f_lw = scipy.signal.lfilter(b, a, self.enc_vel_lw) ax = plt.subplot(2, 1, 1) plt.plot(self.data['stamp'], self.data['lw_angle'], label='lw_angle') jpu.decorate(ax, title="angles", xlab='time', ylab='rad', legend=True) ax = plt.subplot(2, 1, 2) plt.plot(self.data['stamp'], self.data['lw_rvel'], alpha=0.5, label='lw_rvel') plt.plot(self.data['stamp'], self.data['lw_rvel_f'], label='lw_rvel_f') plt.plot(self.enc_vel_stamp, self.enc_vel_lw, label='lw_rvel1') plt.plot(self.enc_vel_stamp, self.enc_vel_f_lw, label='lw_rvelf1') jpu.decorate(ax, title="rvel", xlab='time', ylab='rad/s', legend=True)
def plot_pwm(_ds, filename=None): figsize = (20.48, 2.56) fig = jpu.prepare_fig(window_title='PWM', figsize=figsize) ax = plt.subplot(1, 1, 1) plt.plot(_ds.enc_stamp, _ds.lw_pwm, '.', label="lw") plt.plot(_ds.enc_stamp, _ds.rw_pwm, '.', label="rw") jpu.decorate(ax, title='PWM', xlab='time in s', ylab='', legend=True)
def plot_wheel_radius(self, filename=None): fig = jpu.prepare_fig(window_title='Wheel radius regression') inlier_mask = self.ransac_wr.inlier_mask_ outlier_mask = np.logical_not(inlier_mask) #plt.scatter(self.ds.enc_vel_sum, self.truth_lvel_body_1[:,0], color='gold', marker='.', label='Outliers') plt.scatter(self.ds.enc_vel_sum[outlier_mask], self.truth_lvel_body_1[:, 0][outlier_mask], color='gold', marker='.', label='Outliers') plt.scatter(self.ds.enc_vel_sum[inlier_mask], self.truth_lvel_body_1[:, 0][inlier_mask], color='yellowgreen', marker='.', label='Inliers') test_enc_vel_sum = np.linspace(np.min(self.ds.enc_vel_sum), np.max(self.ds.enc_vel_sum), 100)[:, np.newaxis] test_vel_simple = 0.5 * test_enc_vel_sum * self.wheel_radius_simple test_vel_ransac = 0.5 * self.ransac_wr.predict(test_enc_vel_sum) plt.plot(test_enc_vel_sum, test_vel_simple, label='simple') plt.plot(test_enc_vel_sum, test_vel_ransac, label='ransac') jpu.decorate(plt.gca(), title='wheel radius fit', xlab='enc_avg (rad/s)', ylab="v (m/s)", legend=True) #, xlim=[-1, 20], ylim=[-0.1, 1.]) jpu.savefig(filename)
def plot_wheel_sep(self, filename=None): fig = jpu.prepare_fig(window_title='Wheel sep regression') inlier_mask = self.ransac_ws.inlier_mask_ outlier_mask = np.logical_not(inlier_mask) plt.scatter(self.ds.enc_vel_dif[outlier_mask], self.truth_rvel_1[:, 2][outlier_mask], color='gold', marker='.', label='Outliers') plt.scatter(self.ds.enc_vel_dif[inlier_mask], self.truth_rvel_1[:, 2][inlier_mask], color='yellowgreen', marker='.', label='Inliers') test_enc_vel_dif = np.linspace(np.min(self.ds.enc_vel_dif), np.max(self.ds.enc_vel_dif), 100)[:, np.newaxis] test_rvel_simple = test_enc_vel_dif * -self.wheel_radius / self.wheel_sep_simple #test_rvel_ransac = self.ransac_wr.predict(test_enc_vel_dif) test_rvel_ransac = test_enc_vel_dif * -self.wheel_radius / self.wheel_sep_ransac plt.plot(test_enc_vel_dif, test_rvel_simple, label='simple') plt.plot(test_enc_vel_dif, test_rvel_ransac, label='ransac') jpu.decorate(plt.gca(), title='wheel separation fit', xlab='enc_diff (rad/s)', ylab="rvel (rad/s)", legend=True, xlim=[-5., 5.], ylim=[-2.5, 2.5]) jpu.savefig(filename)
def plot_training(ann): _h = ann.history.history plt.figure() plt.plot(_h['loss']) plt.plot(_h['val_loss']) jpu.decorate(plt.gca(), 'loss', xlab='epochs', legend=['training', 'validation'])
def plot_training_dataset(_ann_in, _ann_out): fig = jpu.prepare_fig(window_title='Training dataset') names = '$y_k$', '$y_{k-1}$', '$u_{k-1}$', '$u_k$', '$y_{k+1}$' for i in range(4): ax = plt.subplot(1, 5, i + 1) plt.hist(_ann_in[:, i]) jpu.decorate(ax, title=names[i]) ax = plt.subplot(1, 5, 5) plt.hist(_ann_out) jpu.decorate(ax, title=names[4])
def plot_interp(_d, _stamps, _d_1, _stamps_1): plt.plot(_stamps, _d, '.', label='orig') plt.plot(_stamps_1, _d_1, '.', label='interp') jpu.decorate(plt.gca(), title='Interp', xlab='time in s', ylab='', legend=True, xlim=None, ylim=None)
def plot_test(ax1_, ax2_, stamp, X, U, Xpred): plt.sca(ax1_) plt.plot(stamp, X, label='truth') plt.plot(stamp, Xpred, label='predicted') jpu.decorate(ax1_, title='Linear velocity', xlab='time in s', ylab='m/s', legend=True) plt.sca(ax2_) #plt.plot(stamp, U, label='sum pwm') plt.fill_between(stamp, U, label='sum pwm') jpu.decorate(ax2_, title='U', xlab='time in s', ylab='', legend=True)
def plot2d(_ds, filename=None): fig = jpu.prepare_fig(window_title='2D') plt.plot(_ds.truth_pos[:, 0], _ds.truth_pos[:, 1], '.', label='truth') jpu.decorate(plt.gca(), title='truth 2D points', xlab='m', ylab='m', legend=True, xlim=None, ylim=None) plt.axis('equal') jpu.savefig(filename) return fig
def simulate_plant_and_ann(plant, ann_plant): time = np.arange(0., 25.05, plant.dt) sp, yp, ya = scipy.signal.square(0.25 * time), np.zeros( len(time)), np.zeros(len(time)) for k in range(1, len(time) - 1): yp[k + 1] = plant.io_dyn(yp[k], yp[k - 1], sp[k], sp[k - 1]) ya[k + 1] = ann_plant.predict(ya[k], ya[k - 1], sp[k - 1], sp[k]) fig = jpu.prepare_fig(window_title='Time simulation') ax = plt.gca() plt.plot(time, sp, label='sp') plt.plot(time, yp, label='plant') plt.plot(time, ya, label='ann') jpu.decorate(ax, title='$y$', xlab='time in s', ylab='m', legend=True)
def ident(_ds): ax = plt.subplot(1, 2, 1) #plt.plot(_ds.data['lw_pwm'], _ds.data['lw_rvel_f'], '.') plt.scatter(_ds.data['lw_pwm'], _ds.data['lw_rvel_f'], s=0.1) jpu.decorate(ax, title="left wheel", xlab='lw_pwm', ylab='lw_rvel') #, legend=True) ax = plt.subplot(1, 2, 2) #plt.plot(_ds.data['rw_pwm'], _ds.data['rw_rvel_f'], '.') plt.scatter(_ds.data['rw_pwm'], _ds.data['rw_rvel_f'], s=0.1) jpu.decorate(ax, title="right wheel", xlab='rw_pwm', ylab='rw_rvel') #, legend=True) #ident_poly(_ds, order=1) #ident_spline(_ds) ident_ann(_ds)
def plot_ctl(time, ysp, yr, y): plt.figure() ax = plt.subplot(2, 1, 1) plt.plot(time, ysp, label='sp') plt.plot(time, yr, label='ref') plt.plot(time, y, label='plant') jpu.decorate(ax, title='$y$', xlab='time in s', ylab='m', legend=True) ax = plt.subplot(2, 1, 2) plt.plot(time, y - yr, label='track err') jpu.decorate(ax, title='$\epsilon$', xlab='time in s', ylab='m', legend=True)
def plot_imu(_ds, filename=None): figsize = (20.48, 5.12) fig = jpu.prepare_fig(window_title='IMU', figsize=figsize) ax = plt.subplot(2, 1, 1) plt.plot(_ds.enc_stamp, np.rad2deg(_ds.pitch), '.', label="pitch") jpu.decorate(ax, title='imu pitch', xlab='time in s', ylab='deg', legend=True) ax = plt.subplot(2, 1, 2) plt.plot(_ds.enc_stamp, np.rad2deg(_ds.pitch_dot), '.', label="pitch_dot") jpu.decorate(ax, title='imu pitch dot', xlab='time in s', ylab='deg/s', legend=True)
def plot_kinematics_truth(_ds, filename=None): #fig = jpu.prepare_fig(window_title='Encoders 3D') fig = hciods.plot_encoders_3D(_ds) ax = fig.add_subplot(121, projection='3d') X = np.arange(-1, 1, 0.01) Y = np.arange(-1, 1, 0.01) X, Y = np.meshgrid(X, Y) lvel, rvel = np.zeros_like(X), np.zeros_like(X) for i in range(X.shape[0]): for j in range(X.shape[1]): lvel[i, j], rvel[i, j] = hcu.kin_vel_of_wheel_vel( X[i, j], Y[i, j], _ds.w_r, _ds.w_s) #pdb.set_trace() ax = fig.add_subplot(121, projection='3d') surf = ax.plot_surface(X, Y, lvel, cmap=matplotlib.cm.coolwarm, linewidth=0, antialiased=True, alpha=0.5) ax.scatter(_ds.enc_vel_lw, _ds.enc_vel_rw, _ds.truth_lvel, s=0.1) #, c=c, marker=m) ax.set_xlabel('$\omega_l (rad/s)$') ax.set_ylabel('$\omega_r (rad/s)$') ax.set_zlabel('$V (m/s)$') jpu.decorate(ax, title='Linear Velocity vs/enc vels') ax = fig.add_subplot(122, projection='3d') surf = ax.plot_surface(X, Y, rvel, cmap=matplotlib.cm.coolwarm, linewidth=0, antialiased=True, alpha=0.5) ax.scatter(_ds.enc_vel_lw, _ds.enc_vel_rw, _ds.truth_rvel, s=0.1) #, c=c, marker=m) ax.set_xlabel('$\omega_l (rad/s)$') ax.set_ylabel('$\omega_r (rad/s)$') ax.set_zlabel('$\Omega (rad/s)$') jpu.decorate(ax, title='Angular Velocity vs/enc vels') jpu.savefig(filename)
def plot_encoders_3D(_ds, filename=None, title=''): fig = jpu.prepare_fig(window_title='Encoders 3D ({})'.format(title)) ax = fig.add_subplot(121, projection='3d') truth_lvel_body_1 = interpolate(_ds.truth_lvel_body, _ds.truth_vel_stamp, _ds.enc_vel_stamp) ax.scatter(_ds.enc_vel_lw, _ds.enc_vel_rw, truth_lvel_body_1[:, 0], s=0.1) ax.set_xlabel('$\omega_l (rad/s)$') ax.set_ylabel('$\omega_r (rad/s)$') ax.set_zlabel('$V (m/s)$') jpu.decorate(ax, title='Linear Velocity vs/enc vels') ax = fig.add_subplot(122, projection='3d') truth_rvel_1 = interpolate(_ds.truth_rvel, _ds.truth_vel_stamp, _ds.enc_vel_stamp) ax.scatter(_ds.enc_vel_lw, _ds.enc_vel_rw, truth_rvel_1[:, 2], s=0.1) #, c=c, marker=m) ax.set_xlabel('$\omega_l (rad/s)$') ax.set_ylabel('$\omega_r (rad/s)$') ax.set_zlabel('$\Omega (rad/s)$') jpu.decorate(ax, title='Angular Velocity vs/enc vels') jpu.savefig(filename) return fig
def plot_foo(ds, X, U): fig = jpu.prepare_fig(window_title='Foo') ax = plt.subplot(3, 1, 1) plt.plot(ds.truth_vel_stamp, ds.truth_lvel_body[:, 0], '.', label='vx') plt.plot(ds.truth_vel_stamp, X[:, 0], label='vx') jpu.decorate(ax, title='linear velocity', xlab='time in s', ylab='m/s', legend=True) ax = plt.subplot(3, 1, 2) plt.plot(ds.truth_vel_stamp, ds.truth_rvel[:, 2], '.', label='r') plt.plot(ds.truth_vel_stamp, X[:, 1], label='r') jpu.decorate(ax, title='angular velocity', xlab='time in s', ylab='rad/s', legend=True) ax = plt.subplot(3, 1, 3) plt.plot(_ds.enc_stamp, _ds.lw_pwm, '.', label='lw_pwm') plt.plot(_ds.enc_stamp, _ds.rw_pwm, '.', label='rw_pwm') plt.plot(_ds.enc_stamp, U[:, 0]) plt.plot(_ds.enc_stamp, U[:, 1]) jpu.decorate(ax, title='pwm', xlab='time in s', ylab='', legend=True)
def plot(self): ax = plt.subplot(2, 2, 1) plt.plot(self.data['stamp'], self.data['lw_angle'], label='lw_angle') plt.plot(self.data['stamp'], self.data['rw_angle'], label='rw_angle') jpu.decorate(ax, title="angles", xlab='time', ylab='rad', legend=True) ax = plt.subplot(2, 2, 2) plt.plot(self.data['stamp'], self.data['lw_rvel'], alpha=0.5, label='lw_rvel') plt.plot(self.data['stamp'], self.data['rw_rvel'], alpha=0.5, label='rw_rvel') plt.plot(self.data['stamp'], self.data['lw_rvel_f'], label='lw_rvel_f') plt.plot(self.data['stamp'], self.data['rw_rvel_f'], label='rw_rvel_f') jpu.decorate(ax, title="rvel", xlab='time', ylab='rad/s', legend=True) ax = plt.subplot(2, 2, 3) plt.plot(self.data['stamp'], self.data['lw_pwm'], label='lw_pwm') plt.plot(self.data['stamp'], self.data['rw_pwm'], label='rw_pwm') jpu.decorate(ax, title="pwm", xlab='time', ylab='-127,127', legend=True)
def plot_err(odom_pos, odom_yaw, ds, label, start_idx, run_len, filename): figsize = (20.48, 10.24) margins = 0.05, 0.07, 0.97, 0.95, 0.14, 0.39 fig = jpu.prepare_fig(window_title='Odometry error {} ({})'.format( label, filename), figsize=figsize, margins=margins) if run_len is None: run_len = len(ds.enc_stamp) - start_idx # we go to the end truth_pos_1 = hio.interpolate(ds.truth_pos, ds.truth_stamp, ds.enc_stamp[start_idx:start_idx + run_len]) pos_err = odom_pos - truth_pos_1 ax = plt.subplot(2, 1, 1) plt.plot(ds.enc_stamp[start_idx:start_idx + run_len], np.linalg.norm(pos_err, axis=1)) jpu.decorate(ax, title='odom translation error', xlab='time in s', ylab='m', legend=None, xlim=None, ylim=None) truth_yaw = np.array([ tf.transformations.euler_from_quaternion(q, axes='sxyz')[2] for q in ds.truth_ori ]) truth_yaw_1 = hio.interpolate(truth_yaw[:, np.newaxis], ds.truth_stamp, ds.enc_stamp[start_idx:start_idx + run_len]) yaw_err = hcu.wrap_angle(odom_yaw - truth_yaw_1) ax = plt.subplot(2, 1, 2) plt.plot(ds.enc_stamp[start_idx:start_idx + run_len], np.rad2deg(yaw_err)) jpu.decorate(ax, title='odom rotation error', xlab='time in s', ylab='deg', legend=None, xlim=None, ylim=None)
def foo(plant, plant_ann): time = np.arange(0., 3.2, 0.01) ysp = np.random.uniform(low=-1., high=1., size=len(time)) #time + 0.3 #ysp = scipy.signal.square(time) #ysp[0] = 0 y0, u0 = [0, 0], ysp[0] y, u = plant.sim_io(time, y0, u0, so_lti.IoCtlCst(ysp)) a0, a1, b0, b1 = plant.a0, plant.a1, plant.b0, plant.b1 #_u = compute_prev_control_from_output_history_horiz(y, a0, a1, b0, b1, debug=True) _u = compute_prev_control_from_output_history_mat(y, a0, a1, b0, b1, h=-1, debug=True) ax = plt.subplot(3, 1, 1) plt.plot(time, y, '.-') jpu.decorate(ax, title='y', legend=True) ax = plt.subplot(3, 1, 2) plt.plot(time[:-1], u[:-1], '.-', label='truth') plt.plot(time[0:-1], _u[::-1], '.-', label='est') jpu.decorate(ax, title='u', legend=True) err_u = _u[::-1] - u[:-1] ax = plt.subplot(3, 1, 3) jpu.decorate(ax, title='$\\tilde{u}$', legend=True) plt.plot(time[:-1], err_u, '.-') #pdb.set_trace() plt.show()
def plot_encoders_stats(_ds, filename=None, title=''): ''' plot encoders histograms ''' fig = jpu.prepare_fig(window_title='Encoders Stats ({})'.format(title)) ax = plt.subplot(2, 2, 1) plt.hist(_ds.enc_vel_lw) jpu.decorate(ax, title='Left Wheel', xlab='rvel in rad/s', ylab='samples') ax = plt.subplot(2, 2, 2) plt.hist(_ds.enc_vel_lw) jpu.decorate(ax, title='Right Wheel', xlab='rvel in rad/s', ylab='samples') ax = plt.subplot(2, 2, 3) plt.hist(_ds.truth_lvel_body[:, 0]) jpu.decorate(ax, title='Linear Velocity', xlab='m/s', ylab='samples') ax = plt.subplot(2, 2, 4) plt.hist(_ds.truth_rvel[:, 2]) jpu.decorate(ax, title='Angular Velocity', xlab='rad/s', ylab='samples') jpu.savefig(filename)
def test_compute_previous_control(plant, plant_ann): time = np.arange(0., 1.05, 0.01) #ysp = scipy.signal.square(time) #ysp = np.sin(time+1) ysp = np.random.uniform(low=-1., high=1., size=len(time)) y, u = plant.sim_io(time, [1, 1], ysp[0], so_lti.IoCtlCst(ysp)) u[0] = u[1] # test getting u_km1 from y_kp1, y_k, y_km1...y0 a0, a1, b0, b1 = plant.a0, plant.a1, plant.b0, plant.b1 _u1, _u2 = np.zeros(len(time)), np.zeros(len(time)) horiz = 11 for k in range(1, len(time) - 1): _u1[k - 1] = compute_prev_control_from_output_history( y[:k + 1], a0, a1, b0, b1) #_u2[k-1] = compute_prev_control_from_output_history_horiz(y[:k+1], a0, a1, b0, b1, h=horiz)#, u1=u[:k]) _u2[k - 1] = compute_prev_control_from_output_history_mat( y[:k + 1], a0, a1, b0, b1, h=horiz) #, u1=u[:k]) _u1[-1] = _u2[-1] = u[-1] #pdb.set_trace() ax = plt.gca() err_u1 = _u1 - u err_u2 = _u2 - u plt.hist(err_u1[1:-3], label='u1', alpha=0.5) plt.hist(err_u2[1:-3], label='u2', alpha=0.5) jpu.decorate(ax, title='hist', legend=True) plt.figure() ax = plt.subplot(3, 1, 1) plt.plot(time, y, label='plant') jpu.decorate(ax, title='$y$') ax = plt.subplot(3, 1, 2) plt.plot(time[:-2], u[:-2], label='$u real$') plt.plot(time[:-2], _u1[:-2], label='$\\tilde{u}1$') plt.plot(time[:-2], _u2[:-2], label='$\\tilde{u}2$') jpu.decorate(ax, title='$u$', legend=True) ax = plt.subplot(3, 1, 3) plt.plot(time[1:-3], err_u1[1:-3], label='u1') plt.plot(time[1:-3], err_u2[1:-3], label='u2') jpu.decorate(ax, title='$u-\\tilde{u}$', legend=True)
def plot_encoders(_ds, filename=None): fig = jpu.prepare_fig(window_title='Encoders') ax = plt.subplot(4, 1, 1) plt.plot(_ds.enc_stamp, _ds.enc_lw, '.', label="lw") plt.plot(_ds.enc_stamp, _ds.enc_rw, '.', label="rw") jpu.decorate(ax, title='Encoders position', xlab='time in s', ylab='rad', legend=True) ax = plt.subplot(4, 1, 2) plt.plot(_ds.enc_vel_stamp, _ds.enc_vel_lw, '.') plt.plot(_ds.enc_vel_stamp, _ds.enc_vel_rw, '.') jpu.decorate(ax, title='Encoders velocity', xlab='time in s', ylab='rad/s', legend=None, xlim=None, ylim=None) ax = plt.subplot(4, 1, 3) plt.plot(_ds.enc_vel_stamp, _ds.enc_vel_sum, '.') jpu.decorate(ax, title='Encoders velocity sum', xlab='time in s', ylab='rad/s', legend=None, xlim=None, ylim=None) ax = plt.subplot(4, 1, 4) plt.plot(_ds.enc_vel_stamp, _ds.enc_vel_dif, '.') jpu.decorate(ax, title='Encoders velocity dif', xlab='time in s', ylab='rad/s', legend=None, xlim=None, ylim=None) jpu.savefig(filename)
def sim_cl_ann(ann, K, dt=0.01): time = np.arange(0, 15, dt) X, U = np.zeros((len(time), 6)), np.zeros((len(time), 2)) phi0, gamma0, theta0 = np.deg2rad(1), np.deg2rad(1), np.deg2rad(1) X[0] = [phi0, gamma0, 0, 0, theta0, 0] for k in range(1, len(time)): U[k - 1] = -np.dot(K, X[k - 1]) _in_km1 = np.hstack((X[k - 1], U[k - 1]))[np.newaxis, :] X[k] = ann.predict(_in_km1) ax = plt.subplot(3, 1, 1) plt.plot(time, np.rad2deg(X[:, 0])) jpu.decorate(ax, title='phi', xlab='time in s', ylab='deg', legend=True) ax = plt.subplot(3, 1, 2) plt.plot(time, np.rad2deg(X[:, 1])) jpu.decorate(ax, title='gamma', xlab='time in s', ylab='deg', legend=True) ax = plt.subplot(3, 1, 3) plt.plot(time, np.rad2deg(X[:, 4])) jpu.decorate(ax, title='theta', xlab='time in s', ylab='deg', legend=True) plt.show()
def plot_training(ann): fig = jpu.prepare_fig(window_title='Training history') _h = ann.history.history plt.plot(_h['loss'], label='loss') plt.plot(_h['val_loss'], label='val_loss') jpu.decorate(plt.gca(), 'loss', xlab='epochs', legend=True)
def main( omega_plant=1., xi_plant=0.5, ): plant = so_lti.IoPlantModel(omega_plant, xi_plant) a0, a1, b0, b1 = plant.a0, plant.a1, plant.b0, plant.b1 print(plant.tf_disc) if 0: time = np.arange(0., 20.05, 0.01) ysp = scipy.signal.square(0.3 * time) y0, u0 = np.random.uniform(low=-1., high=1., size=2), np.random.uniform(low=-1., high=1., size=1) y0[1] = y0[0] + np.random.uniform(low=-0.1, high=0.1, size=1) y, u = plant.sim_io(time, y0, u0, so_lti.IoCtlCst(ysp)) plot(time, y, u, ysp) plt.show() horiz = 2 # build network net_i = keras.layers.Input((horiz + 2, ), name="net_i") #y_k, y_km1, ..., y_kmh, u_kmh w0 = 1 / a1 * np.array([[b0], [b1], [1.], [-a0]]) + np.random.uniform( low=-100., high=100., size=(4, 1)) pdb.set_trace() net_l = keras.layers.Dense( 1, activation='linear', #kernel_initializer='uniform', kernel_initializer=keras.initializers.Constant(w0), input_shape=(horiz + 1, ), use_bias=False, name="net_l") net_o = net_l(net_i) _ann = keras.models.Model(inputs=net_i, outputs=net_o) _opt = keras.optimizers.Adam(lr=0.001, beta_1=0.9, beta_2=0.999, epsilon=None, decay=0.0, amsgrad=False) #_opt = keras.optimizers.SGD(lr=0.01, decay=1e-6, momentum=0.9, nesterov=True) #_ann.compile(loss='mean_squared_error', optimizer=_opt) #_ann.compile(loss='mean_squared_logarithmic_error', optimizer=_opt) _ann.compile(loss='mean_absolute_error', optimizer=_opt) # prepare dataset if 1: ds_size = int(1e5) time = np.arange(0., (horiz + 1) * plant.dt, plant.dt) _input, _output = np.zeros( (ds_size, horiz + 2), dtype=np.float64), np.zeros((ds_size, 1), dtype=np.float64) _us = np.zeros((ds_size, horiz)) for i in range(ds_size): ysp = np.random.uniform(low=-1., high=1., size=horiz + 1) y0, u0 = np.random.uniform(low=-1., high=1., size=2), np.random.uniform(low=-1., high=1., size=1) y0[1] = y0[0] + np.random.uniform(low=-0.01, high=0.01, size=1) y, u = plant.sim_io(time, y0, u0, so_lti.IoCtlCst(ysp)) _input[i, :-1], _input[i, -1] = y, u[0] _output[i] = u[-2] _us[i] = u[:-1] np.savez('/tmp/ds.npz', _input=_input, _output=_output) else: _data = np.load('/tmp/ds.npz') _input, _output = _data['_input'], _data['_output'] ds_size = len(_output) # plot dataset if True: # #for i in range(ds_size): # plt.plot(time, _input[i][:-1]) for i in range(horiz + 1): ax = plt.subplot(2, horiz + 1, i + 1) plt.hist(_input[:, i]) jpu.decorate(ax, title='$y_{{{}}}$'.format(i)) for i in range(horiz): ax = plt.subplot(2, horiz + 1, i + 1 + horiz + 1) plt.hist(_us[:, i]) jpu.decorate(ax, title='$u_{{{}}}$'.format(i)) plt.show() # train if True: callbacks = [ keras.callbacks.EarlyStopping(monitor='val_loss', patience=10), keras.callbacks.ModelCheckpoint(filepath='/tmp/foo.h5', monitor='val_loss', save_best_only=True) ] history = _ann.fit(_input, _output, epochs=1000, batch_size=16, verbose=1, shuffle=True, validation_split=0.2, callbacks=callbacks) plt.figure() plt.plot(history.history['loss']) plt.plot(history.history['val_loss']) jpu.decorate(plt.gca(), 'loss', xlab='epochs', legend=['training', 'validation']) print(' trained weights:\n{}'.format(net_l.get_weights())) # force if False: print(' orig weight:\n{}'.format(net_l.get_weights())) a0, a1, b0, b1 = plant.a0, plant.a1, plant.b0, plant.b1 net_l.set_weights([1 / a1 * np.array([[b0], [b1], [1.], [-a0]])]) print(' new weight:\n{}'.format(net_l.get_weights())) # evaluate res = _ann.predict(_input) - _output loss = np.mean(np.square(res)) print('loss {:.2e}'.format(loss)) plt.figure() plt.hist(res) plt.show() loss = _ann.evaluate(_input, _output) print('loss {:.2e}'.format(loss))
def plot_residuals(self, info, filename=None): figsize = (20.48, 10.24) margins = 0.05, 0.07, 0.97, 0.95, 0.14, 0.39 fig = jpu.prepare_fig( window_title='Odometry parameters regression ({})'.format(info), figsize=figsize, margins=margins) inlier_mask = self.ransac_wr.inlier_mask_ outlier_mask = np.logical_not(inlier_mask) ax = plt.subplot(4, 1, 1) #plt.scatter(self.ds.enc_vel_stamp, self.res_radius_simple, marker='.') plt.scatter(self.ds.enc_vel_stamp[outlier_mask], self.res_radius_ransac[outlier_mask], color='gold', marker='.', label='Outliers') plt.scatter(self.ds.enc_vel_stamp[inlier_mask], self.res_radius_ransac[inlier_mask], color='yellowgreen', marker='.', label='Inliers') jpu.decorate(ax, title='wheel radius residuals', xlab='time in s', ylab='m', legend=True, xlim=None, ylim=None) _fmt_legend = '$\mu: {:.2e} \quad \sigma: {:.2e}$' ax = plt.subplot(4, 2, 3) plt.hist(self.res_radius_simple, normed=True, bins=30) jpu.decorate(ax, title='simple', xlab='m', legend=[ _fmt_legend.format(self.res_radius_simple_mu, self.res_radius_simple_sigma) ]) ax = plt.subplot(4, 2, 4) plt.hist(self.res_radius_ransac[self.ransac_wr.inlier_mask_], normed=True, bins=30) jpu.decorate(ax, title='ransac', xlab='m', legend=[ _fmt_legend.format(self.res_radius_ransac_mu, self.res_radius_ransac_sigma) ]) inlier_mask = self.ransac_ws.inlier_mask_ outlier_mask = np.logical_not(inlier_mask) ax = plt.subplot(4, 1, 3) #plt.scatter(self.ds.enc_vel_stamp, self.res_wheel_sep_simple, marker='.') plt.scatter(self.ds.enc_vel_stamp[outlier_mask], self.res_wheel_sep_ransac[outlier_mask], color='gold', marker='.', label='Outliers') plt.scatter(self.ds.enc_vel_stamp[inlier_mask], self.res_wheel_sep_ransac[inlier_mask], color='yellowgreen', marker='.', label='Inliers') jpu.decorate(ax, title='wheel separation residuals', xlab='time in s', ylab='', legend=True, xlim=None, ylim=None) ax = plt.subplot(4, 2, 7) plt.hist(self.res_wheel_sep_simple, normed=True, bins=30) jpu.decorate(ax, title='simple', legend=[ _fmt_legend.format(self.res_wheel_sep_simple_mu, self.res_wheel_sep_simple_sigma) ]) ax = plt.subplot(4, 2, 8) plt.hist(self.res_wheel_sep_ransac[self.ransac_ws.inlier_mask_], normed=True, bins=30) jpu.decorate(ax, title='ransac', legend=[ _fmt_legend.format(self.res_wheel_sep_ransac_mu, self.res_wheel_sep_ransac_sigma) ]) jpu.savefig(filename)
def plot_time_distribution(self): self.enc_dt = self.data['stamp'][1:] - self.data['stamp'][:-1] plt.hist(self.enc_dt, bins=100) jpu.decorate(plt.gca(), title="timestamps", xlab='s', ylab='')
def plot_training_history(self, filename=None): fig = jpu.prepare_fig(window_title='Training History') ax = plt.subplot(1,1,1) plt.plot(self.history.history['loss'], label='loss') plt.plot(self.history.history['val_loss'], label='val_loss') jpu.decorate(ax, title='loss', legend=True);