def battery_hu(Erat = cs.MX.sym('Erat')): Q = 8280 # amp*s C = 51782 # farad R = 0.01 # ohm m = 0.07 # kilograms per cell imax = 70 # amp imin = -35 # amp U0 = 3.3 # volts SoC = cs.MX.sym('Soc') # state of charge P = cs.MX.sym('P') # internal power in watt n = Erat/(.5*Q**2/C +Q*U0) # num cells mass = m*n # total battery mass E = Erat*SoC # current energy dSoC = -P/Erat Pout = P - (R*C*P**2)/(2*E + n*C*U0**2) # Pmax = imax*cs.sqrt(n*(2*Erat/C + n*U0**2)) # Pmin = imin*cs.sqrt(n*(2*Erat/C + n*U0**2)) Pmax = imax*cs.sqrt(n*(2*E/C + n*U0**2)) Pmin = imin*cs.sqrt(n*(2*E/C + n*U0**2)) return {'dSoC':cs.Function('dSoC',[Erat,SoC,P],[dSoC]),\ 'Pout':cs.Function('Pout',[Erat,SoC,P],[Pout]),\ 'Pmax':cs.Function('Pmax',[Erat,SoC],[Pmax]),\ 'Pmin':cs.Function('Pmin',[Erat,SoC],[Pmin]),\ 'SoC_max':.8,\ 'SoC_min':.2 ,\ 'mass': cs.Function('mass',[Erat],[mass])}
def _k_mat52(x, y=None, variance=1., lengthscale=None, diag_only=False, ARD=False): """ Evaluate the Matern52 kernel function symbolically using Casadi""" n_x, dim_x = x.shape if diag_only: ret = SX(n_x, ) ret[:] = variance return ret if y is None: y = x n_y, _ = np.shape(y) if lengthscale is None: if ARD: lengthscale = np.ones((dim_x, )) else: lengthscale = 1. if ARD is False: lengthscale = lengthscale * np.ones((dim_x, )) lens_x = repmat(lengthscale.reshape(1, -1), n_x) lens_y = repmat(lengthscale.reshape(1, -1), n_y) r = _unscaled_dist(x / lens_x, y / lens_y) # GPY: self.variance*(1+np.sqrt(5.)*r+5./3*r**2)*np.exp(-np.sqrt(5.)*r) return variance * (1. + sqrt(5.) * r + 5. / 3 * r**2) * exp(-sqrt(5.) * r)
def __cost_saturation_l(self, x, x_ref, covar_x, u, covar_u, delta_u, Q, R, S): """ Stage Cost function: Expected Value of Saturating Cost """ Nx = ca.MX.size1(Q) Nu = ca.MX.size1(R) # Create symbols Q_s = ca.SX.sym('Q', Nx, Nx) R_s = ca.SX.sym('Q', Nu, Nu) x_s = ca.SX.sym('x', Nx) u_s = ca.SX.sym('x', Nu) covar_x_s = ca.SX.sym('covar_z', Nx, Nx) covar_u_s = ca.SX.sym('covar_u', ca.MX.size(R)) Z_x = ca.SX.eye(Nx) + 2 * covar_x_s @ Q_s Z_u = ca.SX.eye(Nu) + 2 * covar_u_s @ R_s cost_x = ca.Function('cost_x', [x_s, Q_s, covar_x_s], [ 1 - ca.exp(-(x_s.T @ ca.solve(Z_x.T, Q_s.T).T @ x_s)) / ca.sqrt(ca.det(Z_x)) ]) cost_u = ca.Function('cost_u', [u_s, R_s, covar_u_s], [ 1 - ca.exp(-(u_s.T @ ca.solve(Z_u.T, R_s.T).T @ u_s)) / ca.sqrt(ca.det(Z_u)) ]) return cost_x(x - x_ref, Q, covar_x) + cost_u(u, R, covar_u)
def generate_cost_function(self, p_0, u_0, p_all, q_all, mu_perf, sigma_perf, k_ff_safe, k_fb_safe, sigma_safe, k_fb_perf=None, k_ff_perf=None, gp_pred_sigma_perf=None, custom_cost_func=None, eps_noise=0.0): # Generate cost function if custom_cost_func is None: cost = 0 if self.n_perf > 1: n_cost_deviation = np.minimum(self.n_perf, self.n_safe) for i in range(1, n_cost_deviation): cost += mtimes(mu_perf[i, :] - p_all[i, :], mtimes(.1 * self.wx_cost, (mu_perf[i, :] - p_all[i, :]).T)) for i in range(self.n_perf): cost -= sqrt(sum2(gp_pred_sigma_perf[i, :] + eps_noise)) else: for i in range(self.n_safe): cost -= sqrt(sum2(sigma_safe[i, :] + eps_noise)) else: if self.n_perf > 1: cost = custom_cost_func(p_0, u_0, p_all, q_all, k_ff_safe, k_fb_safe, sigma_safe, mu_perf, sigma_perf, gp_pred_sigma_perf, k_fb_perf, k_ff_perf) else: cost = custom_cost_func(p_0, u_0, p_all, q_all, k_ff_safe, k_fb_safe, sigma_safe) return cost
def from_dcm(cls, R): assert R.shape == (3, 3) b1 = 0.5 * ca.sqrt(1 + R[0, 0] + R[1, 1] + R[2, 2]) b2 = 0.5 * ca.sqrt(1 + R[0, 0] - R[1, 1] - R[2, 2]) b3 = 0.5 * ca.sqrt(1 - R[0, 0] + R[1, 1] - R[2, 2]) b4 = 0.5 * ca.sqrt(1 - R[0, 0] - R[1, 1] - R[2, 2]) q1 = ca.SX(4, 1) q1[0] = b1 q1[1] = (R[2, 1] - R[1, 2]) / (4 * b1) q1[2] = (R[0, 2] - R[2, 0]) / (4 * b1) q1[3] = (R[1, 0] - R[0, 1]) / (4 * b1) q2 = ca.SX(4, 1) q2[0] = (R[2, 1] - R[1, 2]) / (4 * b2) q2[1] = b2 q2[2] = (R[0, 1] + R[1, 0]) / (4 * b2) q2[3] = (R[0, 2] + R[2, 0]) / (4 * b2) q3 = ca.SX(4, 1) q3[0] = (R[0, 2] - R[2, 0]) / (4 * b3) q3[1] = (R[0, 1] + R[1, 0]) / (4 * b3) q3[2] = b3 q3[3] = (R[1, 2] + R[2, 1]) / (4 * b3) q4 = ca.SX(4, 1) q4[0] = (R[1, 0] - R[0, 1]) / (4 * b4) q4[1] = (R[0, 2] + R[2, 0]) / (4 * b4) q4[2] = (R[1, 2] + R[2, 1]) / (4 * b4) q4[3] = b4 q = ca.if_else(R[0, 0] > 0, ca.if_else(R[1, 1] > 0, q1, q2), ca.if_else(R[1, 1] > R[2, 2], q3, q4)) return q
def _include_statistics_eqs_of_stochastic_variables( self, exp_phi, ls_factor, model, problem): self.stochastic_variables = vertcat(*self.stochastic_variables) for i in range(self.stochastic_variables.numel()): var = self.stochastic_variables[i] if var.is_symbolic(): name = var.name() else: name = 'stoch_var_' + str(i) _, _, _ = self._include_statistics_of_expression( var, name, exp_phi, ls_factor, model, problem) for i in range(self.socp.n_g_stochastic): var = self.socp.g_stochastic_ineq[i] rhs = self.socp.g_stochastic_rhs[i] name = 'stoch_constr_' + str(i) [stoch_ineq_mean, stoch_ineq_var, _] = self._include_statistics_of_expression( var, name, exp_phi, ls_factor, model, problem) stoch_cosntr_viol_prob = problem.create_optimization_theta( name + '_viol_prob', new_theta_opt_max=0.0) k_viol = sqrt(self.socp.g_stochastic_prob[i] / (1 - self.socp.g_stochastic_prob[i])) problem.include_time_equality( stoch_cosntr_viol_prob - (k_viol * sqrt(fmax(1e-6, stoch_ineq_var)) + stoch_ineq_mean - rhs), when='end')
def get_h_i_t(): w_i, alpha_i, beta_i, b_i, c_i, mu_i, v_i, h_tm1_agg_i, delta_t_agg_i = casadi.SX.sym('w_i'), casadi.SX.sym('alpha_i'), casadi.SX.sym('beta_i'), casadi.SX.sym('b_i'), casadi.SX.sym('c_i'), casadi.SX.sym('mu_i'), casadi.SX.sym('v_i'), casadi.SX.sym('h_tm1_agg_i'), casadi.SX.sym('delta_t_agg_i'), boolM = mu_i > 0 sqrthtpowmu = casadi.sqrt(h_tm1_agg_i) ** mu_i zTrue = (w_i + alpha_i * sqrthtpowmu * f_i(delta_t_agg_i, b_i, c_i) ** v_i + beta_i * sqrthtpowmu) ** (2./mu_i) sqrtht = casadi.sqrt(h_tm1_agg_i) zFalse= (casadi.exp(w_i + alpha_i * f_i(delta_t_agg_i, b_i, c_i) ** v_i + beta_i * casadi.log(sqrtht)) ** (2.)) z = casadi.if_else(boolM, zTrue, zFalse) return casadi.Function('h_i_t', [w_i, alpha_i, beta_i, b_i, c_i, mu_i, v_i, h_tm1_agg_i, delta_t_agg_i], [z])
def generate_salomon(n=2, a=1., b=2 * cs.np.pi, c=0.1, func_opts={}, data_type=cs.SX): x = data_type.sym("x", n) sumsq = cs.sumsqr(x) f = a - cs.cos(b * cs.sqrt(sumsq)) + c * cs.sqrt(sumsq) func = cs.Function("salomon", [x], [f], ["x"], ["f"], func_opts) return func, [[-10., 10.]] * n, [[0.] * n]
def JK(chi, chi_des, t_r, v_ini): T_s_est = Xu * chi[0] ud = ca.sqrt(chi[0]**2 + v_ini**2) d = ca.sqrt((chi_des[0] - chi[3])**2 + (chi_des[1] - chi[4])**2) Dpsi = wraptopi( ca.atan2(chi_des[1] - chi[4], chi_des[0] - chi[3]) - ca.atan2(chi[1], chi[0]) - chi[5]) t_rem = 2 * t_r + ca.fabs(d / ud - 2 * t_r * ca.sin(Dpsi) / (Dpsi + 1e-16)) J_horizontal = cal_yaw_moment(Dpsi, t_r, chi[2], T_s_est) J_K = ex_f * t_rem + 2 * cal_p( T_s_est / 2) * (t_rem - 2 * t_r) + J_horizontal return J_K
def multi_receiver_range_2d(x, params=None): if "y" in params: if "idx" not in params: idx = [0, 1] else: idx = params["idx"] y = sqrt((x[idx[0]] - params["y"][0])**2 + (x[idx[1]] - params["y"][1])**2 + .000001) elif "idxA" in params and "idxB" in params: idxA = params["idxA"] idxB = params["idxB"] y = sqrt((x[idxA[0]] - x[idxB[0]])**2 + (x[idxA[1]] - x[idxB[1]])**2 + .000001) return y
def find_closest_point_to_triangle(poly, posex, posey): point1 = find_closest_point_on_line(posex, posey, poly[0], poly[1], poly[2], poly[3]) point2 = find_closest_point_on_line(posex, posey, poly[2], poly[3], poly[4], poly[5]) point3 = find_closest_point_on_line(posex, posey, poly[4], poly[5], poly[0], poly[1]) dist1 = ca.sqrt((posex - point1[0])**2 + (posey - point1[1])**2) dist2 = ca.sqrt((posex - point2[0])**2 + (posey - point2[1])**2) dist3 = ca.sqrt((posex - point3[0])**2 + (posey - point3[1])**2) closest_point = ca.if_else(dist1 < dist2, point1, point2) distance = ca.if_else(dist1 < dist2, dist1, dist2) closest_point = ca.if_else(dist3 < distance, point3, closest_point) return closest_point
def quaternion_slerp(q1, q2, t): """ spherical linear interpolation that takes into account that q == -q :param q1: 4x1 Matrix :type q1: Matrix :param q2: 4x1 Matrix :type q2: Matrix :param t: float, 0-1 :type t: Union[float, Symbol] :return: 4x1 Matrix; Return spherical linear interpolation between two quaternions. :rtype: Matrix """ cos_half_theta = dot(q1.T, q2) if0 = -cos_half_theta q2 = if_greater_zero(if0, -q2, q2) cos_half_theta = if_greater_zero(if0, -cos_half_theta, cos_half_theta) if1 = Abs(cos_half_theta) - 1.0 # enforce acos(x) with -1 < x < 1 cos_half_theta = Min(1, cos_half_theta) cos_half_theta = Max(-1, cos_half_theta) half_theta = acos(cos_half_theta) sin_half_theta = sqrt(1.0 - cos_half_theta * cos_half_theta) if2 = 0.001 - Abs(sin_half_theta) ratio_a = save_division(sin((1.0 - t) * half_theta), sin_half_theta) ratio_b = save_division(sin(t * half_theta), sin_half_theta) return if_greater_eq_zero( if1, Matrix(q1), if_greater_zero(if2, 0.5 * q1 + 0.5 * q2, ratio_a * q1 + ratio_b * q2))
def pseudo_huber_loss(x, params=None): """ Returns the pseudo-huber loss using parameter delta and Q """ n = params["Q"].shape[0] huber = 0 for i in range(n): huber += 2*params["Q"][i,i]*params["delta"]**2*(casadi.sqrt(1 + (x[i])**2/params["delta"]**2) - 1.0) return huber
def getWind(): z0 = conf['z0'] zt_roughness = conf['zt_roughness'] zsat = 0.5*(z+C.sqrt(z*z)) wind_x = dae['w0']*C.log((zsat+zt_roughness+2)/zt_roughness)/C.log(z0/zt_roughness) # wind_x = dae['w0'] return wind_x
def compute_remainder_overapproximations(q, k_fb, l_mu, l_sigma): """ Compute symbolically the (hyper-)rectangle over-approximating the lagrangians of mu and sigma Parameters ---------- q: n_s x n_s ndarray[casadi.SX.sym] The shape matrix of the current state ellipsoid k_fb: n_u x n_s ndarray[casadi.SX.sym] The linear feedback term l_mu: n x 0 numpy 1darray[float] The lipschitz constants for the gradients of the predictive mean l_sigma n x 0 numpy 1darray[float] The lipschitz constans on the predictive variance Returns ------- u_mu: n_s x 0 numpy 1darray[casadi.SX.sym] The upper bound of the over-approximation of the mean lagrangian remainder u_sigma: n_s x 0 numpy 1darray[casadi.SX.sym] The upper bound of the over-approximation of the variance lagrangian remainder """ n_u, n_s = np.shape(k_fb) s = horzcat(np.eye(n_s), k_fb.T) b = mtimes(s, s.T) qb = mtimes(q, b) evals = matrix_norm_2_generalized(b, q) r_sqr = vec_max(evals) u_mu = l_mu * r_sqr u_sigma = l_sigma * sqrt(r_sqr) return u_mu, u_sigma
def get_speed_of_sound_from_temperature(temperature): """ Finds the speed of sound from a specified temperature. Assumes ideal gas properties. :param temperature: Temperature, in Kelvin :return: Speed of sound, in m/s """ return cas.sqrt(1.4 * R_air * temperature)
def Cd_wave_rae2822(Cl, mach, sweep=0.): """ A curve fit I did to RAE2822 airfoil data. Within -0.4 < CL < 0.75 and 0 < mach < ~0.9, has R^2 = 0.9982. See: C:\Projects\GitHub\firefly_aerodynamics\MSES Interface\analysis\rae2822 :param Cl: Lift coefficient :param mach: Mach number :param sweep: Sweep angle, in deg :return: Wave drag coefficient. """ mach = cas.fmax(mach, 0) sweep_rad = np.pi / 180 * sweep mach_perpendicular = mach * cas.cos( sweep_rad) # Relation from FVA Eq. 8.176 Cl_perpendicular = Cl / cas.cos( sweep_rad)**2 # Relation from FVA Eq. 8.177 # Coeffs c2 = 4.5776476424519119e+00 mc0 = 9.5623337929607111e-01 mc1 = 2.0552787101770234e-01 mc2 = 1.1259268018737063e+00 mc3 = 1.9538856688443659e-01 m = mach_perpendicular l = Cl_perpendicular Cd_wave = cas.fmax(m - (mc0 - mc1 * cas.sqrt(mc2 + (l - mc3)**2)), 0)**2 * c2 return Cd_wave
def diffable_axis_angle_from_matrix(rotation_matrix): """ MAKE SURE MATRIX IS NORMALIZED :param rotation_matrix: 4x4 or 3x3 Matrix :type rotation_matrix: Matrix :return: 3x1 Matrix, angle :rtype: (Matrix, Union[float, Symbol]) """ # TODO nan if angle 0 # TODO buggy when angle == pi # TODO use 'if' to make angle always positive? rm = rotation_matrix cos_angle = (trace(rm[:3, :3]) - 1) / 2 cos_angle = diffable_min_fast(cos_angle, 1) cos_angle = diffable_max_fast(cos_angle, -1) angle = acos(cos_angle) x = (rm[2, 1] - rm[1, 2]) y = (rm[0, 2] - rm[2, 0]) z = (rm[1, 0] - rm[0, 1]) n = sqrt(x * x + y * y + z * z) axis = Matrix([ if_eq(Abs(cos_angle), 1, 0, x / n), if_eq(Abs(cos_angle), 1, 0, y / n), if_eq(Abs(cos_angle), 1, 1, z / n) ]) return axis, angle
def Cd_wave_e216(Cl, mach, sweep=0.): """ A curve fit I did to Eppler 216 airfoil data. Within -0.4 < CL < 0.75 and 0 < mach < ~0.9, has R^2 = 0.9982. See: C:\Projects\GitHub\firefly_aerodynamics\MSES Interface\analysis\e216 :param Cl: Lift coefficient :param mach: Mach number :param sweep: Sweep angle, in deg :return: Wave drag coefficient. """ mach = cas.fmax(mach, 0) sweep_rad = np.pi / 180 * sweep mach_perpendicular = mach * cas.cos( sweep_rad) # Relation from FVA Eq. 8.176 Cl_perpendicular = Cl / cas.cos( sweep_rad)**2 # Relation from FVA Eq. 8.177 # Coeffs c0 = 7.2685945744797997e-01 c1 = -1.5483144040727698e-01 c3 = 2.1305118052118968e-01 c4 = 7.8812272501525316e-01 c5 = 3.3888938102072169e-03 l0 = 1.5298928303149546e+00 l1 = 5.2389999717540392e-01 m = mach_perpendicular l = Cl_perpendicular Cd_wave = (cas.fmax(m - (c0 + c1 * cas.sqrt(c3 + (l - c4)**2) + c5 * l), 0) * (l0 + l1 * l))**2 return Cd_wave
def axis_angle_from_matrix(rotation_matrix): """ :param rotation_matrix: 4x4 or 3x3 Matrix :type rotation_matrix: Matrix :return: 3x1 Matrix, angle :rtype: (Matrix, Union[float, Symbol]) """ rm = rotation_matrix angle = (trace(rm[:3, :3]) - 1) / 2 angle = Min(angle, 1) angle = Max(angle, -1) angle = acos(angle) x = (rm[2, 1] - rm[1, 2]) y = (rm[0, 2] - rm[2, 0]) z = (rm[1, 0] - rm[0, 1]) n = sqrt(x * x + y * y + z * z) m = if_eq_zero(n, 1, n) axis = Matrix([ if_eq_zero(n, 0, x / m), if_eq_zero(n, 0, y / m), if_eq_zero(n, 1, z / m) ]) sign = if_eq_zero(angle, 1, diffable_sign(angle)) axis *= sign angle = sign * angle return axis, angle
def generate_ackleyn2(n=2, a=200, b=0.02, func_opts={}, data_type=cs.SX): if n != 2: raise ValueError("ackleyn2 is only defined for n=2") x = data_type.sym("x", n) f = -a * cs.exp(-b * cs.sqrt(x[0]**2 + x[1]**2)) func = cs.Function("ackleyn2", [x], [f], ["x"], ["f"], func_opts) return func, [[-32., 32.]] * n, [[0.] * n]
def axis_angle_from_matrix(rotation_matrix): """ MAKE SURE MATRIX IS NORMALIZED :param rotation_matrix: 4x4 or 3x3 Matrix :type rotation_matrix: Matrix :return: 3x1 Matrix, angle :rtype: (Matrix, Union[float, Symbol]) """ q = quaternion_from_matrix(rotation_matrix) return axis_angle_from_quaternion(q[0], q[1], q[2], q[3]) # TODO use 'if' to make angle always positive? rm = rotation_matrix cos_angle = (trace(rm[:3, :3]) - 1) / 2 cos_angle = Min(cos_angle, 1) cos_angle = Max(cos_angle, -1) angle = acos(cos_angle) x = (rm[2, 1] - rm[1, 2]) y = (rm[0, 2] - rm[2, 0]) z = (rm[1, 0] - rm[0, 1]) n = sqrt(x * x + y * y + z * z) axis = Matrix([ if_eq(Abs(cos_angle), 1, 0, x / n), if_eq(Abs(cos_angle), 1, 0, y / n), if_eq(Abs(cos_angle), 1, 1, z / n) ]) return axis, angle
def expansion_ratio_from_pressure(chamber_pressure, exit_pressure, gamma, oxamide_fraction): """Find the nozzle expansion ratio from the chamber and exit pressures. See :ref:`expansion-ratio-tutorial-label` for a physical description of the expansion ratio. Reference: Rocket Propulsion Elements, 8th Edition, Equation 3-25 Arguments: chamber_pressure (scalar): Nozzle stagnation chamber pressure [units: pascal]. exit_pressure (scalar): Nozzle exit static pressure [units: pascal]. gamma (scalar): Exhaust gas ratio of specific heats [units: dimensionless]. Returns: scalar: Expansion ratio :math:`\\epsilon = A_e / A_t` [units: dimensionless] """ chamber_pressure = cas.fmax( chamber_pressure, dubious_min_combustion_pressure(oxamide_fraction)) chamber_pressure = cas.fmax(chamber_pressure, exit_pressure * 1.5) AtAe = ((gamma + 1) / 2) ** (1 / (gamma - 1)) \ * (exit_pressure / chamber_pressure) ** (1 / gamma) \ * cas.sqrt((gamma + 1) / (gamma - 1) * (1 - (exit_pressure / chamber_pressure) ** ((gamma - 1) / gamma))) er = 1 / AtAe return er
def fk_rpy_casadi(self, q): T = self.fk_casadi(q) s = ca.sqrt(T[0, 0] * T[0, 0] + T[0, 1] * T[0, 1]) r_x = ca.arctan2(-T[1, 2], T[2, 2]) r_y = ca.arctan2(T[0, 2], s) r_z = ca.arctan2(-T[0, 1], T[2, 2]) return ca.vcat([T[:3, 3], r_x, r_y, r_z])
def if_poly_line(const_vect, poly, posex, posey, keep_dist): closest_point = find_closest_point_on_line(posex, posey, poly[0], poly[1], poly[2], poly[3]) const_vect = ca.vertcat( const_vect, -ca.sqrt((posex - closest_point[0])**2 + (posey - closest_point[1])**2) + keep_dist) return const_vect
def lin_ellipsoid_safety_distance(p_center, q_shape, h_mat, h_vec, c_safety=1.0): """Compute symbolically the distance between eLlipsoid and polytope in casadi. Evaluate the distance of an ellipsoid E(p_center,q_shape), to a polytopic set of the form: h_mat * x <= h_vec. Parameters ---------- p_center: n_s x 1 array The center of the state ellipsoid q_shape: n_s x n_s array The shape matrix of the state ellipsoid h_mat: m x n_s array: The shape matrix of the safe polytope (see above) h_vec: m x 1 array The additive vector of the safe polytope (see above) Returns ------- d_safety: 1darray of length m The distance of the ellipsoid to the polytope. If d < 0 (elementwise), the ellipsoid is inside the poltyope (safe), otherwise safety is not guaranteed. """ d_center = mtimes(h_mat, p_center) d_shape = c_safety * sqrt(sum1(mtimes(q_shape, h_mat.T) * h_mat.T)).T d_safety = d_center + d_shape - h_vec return d_safety
def CL_over_Cl(AR, mach=0, sweep=0): """ Returns the ratio of 3D lift_force coefficient (with compressibility) to 2D lift_force (incompressible) coefficient. :param AR: Aspect ratio :param mach: Mach number :param sweep: Sweep angle [deg] :return: """ beta = cas.sqrt(1 - mach**2) sweep_rad = sweep * np.pi / 180 # return AR / (AR + 2) # Equivalent to equation in Drela's FVA in incompressible, 2*pi*alpha limit. # return AR / (2 + cas.sqrt(4 + AR ** 2)) # more theoretically sound at low AR eta = 0.95 return AR / (2 + cas.sqrt(4 + (AR * beta / eta)**2 * (1 + (cas.tan(sweep_rad) / beta)**2)) ) # From Raymer, Sect. 12.4.1; citing DATCOM
def initial_guess(self, t, tf_init, params): x_guess = self.xd() inclination = 30.0 * ca.pi / 180.0 # the other angle than the one you're thinking of dcmInclination = np.array( [[ca.cos(inclination), 0.0, -ca.sin(inclination)], [0.0, 1.0, 0.0], [ca.sin(inclination), 0.0, ca.cos(inclination)]]) dtheta = 2.0 * ca.pi / tf_init theta = t * dtheta r = 0.25 * params['l'] angle = ca.SX.sym('angle') x_cir = ca.sqrt(params['l']**2 - r**2) y_cir = r * ca.cos(angle) z_cir = r * ca.sin(angle) init_pos_fun = ca.Function( 'init_pos', [angle], [ca.mtimes(dcmInclination, ca.veccat(x_cir, y_cir, z_cir))]) init_vel_fun = init_pos_fun.jacobian() ret = {} ret['q'] = init_pos_fun(theta) ret['dq'] = init_vel_fun(theta, 0) * dtheta ret['w'] = ca.veccat(0.0, 0.0, dtheta) norm_vel = ca.norm_2(ret['dq']) norm_pos = ca.norm_2(ret['q']) R0 = ret['dq'] / norm_vel R2 = ret['q'] / norm_pos R1 = ca.cross(ret['q'] / norm_pos, ret['dq'] / norm_vel) ret['R'] = ca.vertcat(R0.T, R1.T, R2.T).T return ret
def quaternion_from_matrix(matrix): """ !takes a loooong time to compile! :param matrix: 4x4 or 3x3 Matrix :type matrix: Matrix :return: 4x1 Matrix :rtype: Matrix """ q = Matrix([0, 0, 0, 0]) if isinstance(matrix, np.ndarray): M = Matrix(matrix.tolist()) else: M = Matrix(matrix) t = trace(M) if0 = t - M[3, 3] if1 = M[1, 1] - M[0, 0] m_i_i = if_greater_zero(if1, M[1, 1], M[0, 0]) m_i_j = if_greater_zero(if1, M[1, 2], M[0, 1]) m_i_k = if_greater_zero(if1, M[1, 0], M[0, 2]) m_j_i = if_greater_zero(if1, M[2, 1], M[1, 0]) m_j_j = if_greater_zero(if1, M[2, 2], M[1, 1]) m_j_k = if_greater_zero(if1, M[2, 0], M[1, 2]) m_k_i = if_greater_zero(if1, M[0, 1], M[2, 0]) m_k_j = if_greater_zero(if1, M[0, 2], M[2, 1]) m_k_k = if_greater_zero(if1, M[0, 0], M[2, 2]) if2 = M[2, 2] - m_i_i m_i_i = if_greater_zero(if2, M[2, 2], m_i_i) m_i_j = if_greater_zero(if2, M[2, 0], m_i_j) m_i_k = if_greater_zero(if2, M[2, 1], m_i_k) m_j_i = if_greater_zero(if2, M[0, 2], m_j_i) m_j_j = if_greater_zero(if2, M[0, 0], m_j_j) m_j_k = if_greater_zero(if2, M[0, 1], m_j_k) m_k_i = if_greater_zero(if2, M[1, 2], m_k_i) m_k_j = if_greater_zero(if2, M[1, 0], m_k_j) m_k_k = if_greater_zero(if2, M[1, 1], m_k_k) t = if_greater_zero(if0, t, m_i_i - (m_j_j + m_k_k) + M[3, 3]) q[0] = if_greater_zero(if0, M[2, 1] - M[1, 2], if_greater_zero(if2, m_i_j + m_j_i, if_greater_zero(if1, m_k_i + m_i_k, t))) q[1] = if_greater_zero(if0, M[0, 2] - M[2, 0], if_greater_zero(if2, m_k_i + m_i_k, if_greater_zero(if1, t, m_i_j + m_j_i))) q[2] = if_greater_zero(if0, M[1, 0] - M[0, 1], if_greater_zero(if2, t, if_greater_zero(if1, m_i_j + m_j_i, m_k_i + m_i_k))) q[3] = if_greater_zero(if0, t, m_k_j - m_j_k) q *= 0.5 / sqrt(t * M[3, 3]) return q
def _estimate_simulation_duration(self): # 1. Unpack mean z-coordinate and z-velocity z_b0 = self.m0['z_b'] vz_b0 = self.m0['vz_b'] # 2. Use kinematic equation of the ball to find time T = (vz_b0 + ca.sqrt(vz_b0 ** 2 + 2 * self.g * z_b0)) / self.g # 3. Divide time by time-step duration return int(float(T) // self.dt)
def generate_griewank(n=2, a=1., b=4000., func_opts={}, data_type=cs.SX): x = data_type.sym("x", n) product_part = 1. for i in range(n): product_part = product_part * cs.cos(x[i] / cs.sqrt(i + 1)) f = a + (1. / b) * cs.sumsqr(x) - product_part func = cs.Function("griewank", [x], [f], ["x"], ["f"], func_opts) return func, [[-600., 600.]] * n, [[0.] * n]
def generate_alpinen2(n=2, func_opts={}, data_type=cs.SX): x = data_type.sym("x", n) f = 1. for i in range(n): f = f * cs.sqrt(x[i]) * cs.sin(x[i]) f = -f # Note the minus! func = cs.Function("alpinen2", [x], [f], ["x"], ["f"], func_opts) return func, [[0., 10.]] * n, [[7.917] * n]
def setC0(x00,conf): zt = conf['kite']['zt'] x = x00[0] y = x00[1] z = x00[2] e31 = x00[9] e32 = x00[10] e33 = x00[11] r = C.sqrt((x + zt*e31)**2 + (y + zt*e32)**2 + (z + zt*e33)**2) return C.veccat([x00,r,0])
def constrainAirspeedAlphaBeta(ocp): for k in range(0,ocp.nk): for j in range(0,ocp.deg+1): ocp.constrainBnds(ocp.lookup('airspeed',timestep=k,degIdx=j), (10,65), tag=('airspeed',(k,j))) ocp.constrainBnds(ocp.lookup('alpha_deg',timestep=k,degIdx=j), (-4.5,8.5), tag=('alpha(deg)',(k,j))) ocp.constrain(ocp.lookup('cL',timestep=k,degIdx=j), '>=', -0.15, tag=('CL > -0.15',(k,j))) ocp.constrainBnds(ocp.lookup('beta_deg', timestep=k,degIdx=j), (-10,10), tag=('beta(deg)',(k,j))) x = ocp('x', timestep=k,degIdx=j) y = ocp('y', timestep=k,degIdx=j) z = ocp('z', timestep=k,degIdx=j) ocp.constrain(C.sqrt(x**2 + y**2), '>=', -z, tag=('azimuth not too high',(k,j)))
def constrainAirspeedAlphaBeta(ocp): for k in range(0,ocp.nk): for j in range(0,ocp.deg+1): ocp.constrainBnds(ocp.lookup('airspeed',timestep=k,degIdx=j), (10,75), tag=('airspeed',(k,j))) ocp.constrainBnds(ocp.lookup('alpha_deg',timestep=k,degIdx=j), (-4.5,8.5), tag=('alpha(deg)',(k,j))) #ocp.constrain(ocp.lookup('cL',timestep=k,degIdx=j), '>=', -0.15, tag=('CL > -0.15',(k,j))) ocp.constrainBnds(ocp.lookup('beta_deg', timestep=k,degIdx=j), (-10,10), tag=('beta(deg)',(k,j))) x = ocp('x', timestep=k,degIdx=j) y = ocp('y', timestep=k,degIdx=j) z = ocp('z', timestep=k,degIdx=j) r = C.sqrt(x**2 + y**2) ocp.constrain(-z, '>=', 0.25*r - 2.5, tag=('farther out you go, higher you stay',(k,j)))
def apply_unary_opcode(code, p): assert code in unary_opcodes, "Opcode not recognized!" if code==5: return -p elif code==10: return casadi.sqrt(p) elif code==11: return casadi.sin(p) elif code==12: return casadi.cos(p) elif code==13: return casadi.tan(p) assert False
def getCosLineAngle(ocp,k): r31 = ocp.lookup('e31',timestep=k) r32 = ocp.lookup('e32',timestep=k) r33 = ocp.lookup('e33',timestep=k) x = ocp.lookup('x',timestep=k) y = ocp.lookup('y',timestep=k) z = ocp.lookup('z',timestep=k) # r = ocp.lookup('r',timestep=k) r = C.sqrt(x*x + y*y + z*z) return (r31*x + r32*y + r33*z)/r
def _make_objective(self): """Construct objective function from the problem definition Make time optimal objective function and add a regularization to ensure a unique solution. When additional objective terms are defined these are added to the objective as well. TODO: Improve accuracy of integration """ order = self.sys.order N = self.options['N'] b = self.prob['vars'] # ds = 1.0/(N+1) obj = 2 * sum(np.diff(self.prob['s']) / (cas.sqrt(b[:N, 0]) + cas.sqrt(b[1:, 0]))) # reg = sum(cas.sqrt((b[1:, order - 1] - b[:N, order - 1]) ** 2)) reg = sum((b[2:, order - 1] - 2 * b[1:N, order - 1] + b[:(N - 1), order - 1]) ** 2) for f in self.objective['Lagrange']: path, bs = self._make_path()[0:2] # S = np.arange(0, 1, 1.0/(N+1)) S = self.prob['s'] b = self.prob['vars'] L = cas.substitute(f, self.sys.y, path) L = 2 * sum(cas.vertcat([ cas.substitute( L, cas.vertcat([self.s[0], bs]), cas.vertcat([S[j], b[j, :].T]) ) for j in range(1, N + 1) ]) * np.diff(self.prob['s']) / (cas.sqrt(b[:N, 0]) + cas.sqrt(b[1:, 0])) ) # L = sum(cas.vertcat([cas.substitute(L, cas.vertcat([self.s[0], # [bs[i] for i in range(0, bs.numel())]]), # cas.vertcat([S[j], [b[j, i] for i in range(0, self.sys.order)]])) # for j in range(0, N + 1)])) obj = obj + L self.prob['obj'] = obj + self.options['reg'] * reg
def _make_path(self): """Rewrite the path as a function of the optimization variables. Substitutes the time derivatives of s in the expression of the path by expressions that are function of b and its path derivatives by repeatedly applying the chainrule Returns: * SXMatrix. The substituted path * SXMatrix. b and the path derivatives * SXMatrix. The derivatives of s as a function of b """ b = cas.ssym("b", self.sys.order) db = cas.vertcat((b[1:], 0)) Ds = cas.SXMatrix.nan(self.sys.order) # Time derivatives of s Ds[0] = cas.sqrt(b[0]) Ds[1] = b[1] / 2 # Apply chainrule for finding higher order derivatives for i in range(1, self.sys.order - 1): Ds[i + 1] = (cas.mul(cas.jacobian(Ds[i], b), db) * self.s[1] + cas.jacobian(Ds[i], self.s[1]) * Ds[1]) Ds = cas.substitute(Ds, self.s[1], cas.sqrt(b[0])) return cas.substitute(self.path, self.s[1:], Ds), b, Ds
def getWind(): if 'wind_model' not in conf: return 0 if conf['wind_model']['name'] == 'wind_shear': # use a logarithmic wind shear model # wind(z) = w0 * log((z+zt)/zt) / log(z0/zt) # where w0 is wind at z0 altitude # zt is surface roughness characteristic length z0 = conf['wind_model']['z0'] zt_roughness = conf['wind_model']['zt_roughness'] zsat = 0.5*(-z+C.sqrt(z*z)) return dae['w0']*C.log((zsat+zt_roughness+2)/zt_roughness)/C.log(z0/zt_roughness) elif conf['wind_model']['name'] == 'constant': # constant wind return dae['w0']
def get_f(): r_t, h_i_tm1, lambda_i, gamma_i = casadi.SX.sym('r_t'), casadi.SX.sym('h_i_tm1'), casadi.SX.sym('lambda_i'), casadi.SX.sym('gamma_i') z = 1/casadi.sqrt(2*numpy.pi * h_i_tm1) * casadi.exp(-((r_t - (lambda_i + gamma_i * casadi.sqrt(h_i_tm1))) ** 2.) / (2 * h_i_tm1)) return casadi.Function('f', [r_t, h_i_tm1, lambda_i, gamma_i], [z])
p_mean = pl.mean(p_test) p_std = pl.std(p_test, ddof=0) pe_test.compute_covariance_matrix() pe_test.print_estimation_results() # Generate report print("\np_mean = " + str(ca.DMatrix(p_mean))) print("phat_last_exp = " + str(ca.DMatrix(pe_test.estimated_parameters))) print("\np_sd = " + str(ca.DMatrix(p_std))) print("sd_from_covmat = " + str(ca.diag(ca.sqrt(pe_test.covariance_matrix)))) print("beta = " + str(pe_test.beta)) print("\ndelta_abs_sd = " + str(ca.fabs(ca.DMatrix(p_std) - \ ca.diag(ca.sqrt(pe_test.covariance_matrix))))) print("delta_rel_sd = " + str(ca.fabs(ca.DMatrix(p_std) - \ ca.diag(ca.sqrt(pe_test.covariance_matrix))) / ca.DMatrix(p_std))) fname = os.path.basename(__file__)[:-3] + ".rst" report = open(fname, "w") report.write( \ '''Concept test: covariance matrix computation ===========================================
def forcesTorques(state, u, p, outputs): rho = 1.23 # density of the air # [ kg/m^3] rA = 1.085 #(dixit Kurt) alpha0 = -0*C.pi/180 #TAIL LENGTH lT = 0.4 #ROLL DAMPING rD = 1e-2 pD = 0*1e-3 yD = 0*1e-3 #WIND-TUNNEL PARAMETERS #Lift (report p. 67) cLA = 5.064 cLe = -1.924 cL0 = 0.239 #Drag (report p. 70) cDA = -0.195 cDA2 = 4.268 cDB2 = 5 # cDe = 0.044 # cDr = 0.111 cD0 = 0.026 #Roll (report p. 72) cRB = -0.062 cRAB = -0.271 cRr = -5.637e-1 #Pitch (report p. 74) cPA = 0.293 cPe = -4.9766e-1 cP0 = 0.03 #Yaw (report p. 76) cYB = 0.05 cYAB = 0.229 # rudder (fudged by Greg) cYr = 0.02 span = 0.96 chord = 0.1 ########### model integ ################### x = state['x'] y = state['y'] e11 = state['e11'] e12 = state['e12'] e13 = state['e13'] e21 = state['e21'] e22 = state['e22'] e23 = state['e23'] e31 = state['e31'] e32 = state['e32'] e33 = state['e33'] dx = state['dx'] dy = state['dy'] dz = state['dz'] w1 = state['w1'] w2 = state['w2'] w3 = state['w3'] delta = 0 ddelta = 0 aileron = u['aileron'] elevator = u['elevator'] rudder = u['rudder'] ####### kinfile ###### dpE = C.veccat( [ dx*e11 + dy*e12 + dz*e13 + ddelta*e12*rA + ddelta*e12*x - ddelta*e11*y , dx*e21 + dy*e22 + dz*e23 + ddelta*e22*rA + ddelta*e22*x - ddelta*e21*y , dx*e31 + dy*e32 + dz*e33 + ddelta*e32*rA + ddelta*e32*x - ddelta*e31*y ]) dp_carousel_frame = C.veccat( [ dx - ddelta*y , dy + ddelta*rA + ddelta*x , dz ]) - C.veccat([C.cos(delta)*p['wind_x'],C.sin(delta)*p['wind_x'],0]) ##### more model_integ ########### # EFFECTIVE WIND IN THE KITE`S SYSTEM : # ############################### #Airfoil speed in carousel frame we1 = dp_carousel_frame[0] we2 = dp_carousel_frame[1] we3 = dp_carousel_frame[2] vKite2 = C.mul(dp_carousel_frame.trans(), dp_carousel_frame) #Airfoil speed^2 vKite = C.sqrt(vKite2) #Airfoil speed outputs['airspeed'] = vKite # CALCULATION OF THE FORCES : # ############################### # # FORCE ARE COMPUTED IN THE CAROUSEL FRAME @>@>@> #Aero coeff. # LIFT DIRECTION VECTOR # ############ # Relative wind speed in Airfoil's referential 'E' wE1 = dpE[0] wE2 = dpE[1] wE3 = dpE[2] # Airfoil's transversal axis in carousel referential 'e' eTe1 = e21 eTe2 = e22 eTe3 = e23 # Lift axis ** Normed to we @>@> ** eLe1 = - eTe2*we3 + eTe3*we2 eLe2 = - eTe3*we1 + eTe1*we3 eLe3 = - eTe1*we2 + eTe2*we1 # AERODYNAMIC COEEFICIENTS # ################# vT1 = wE1 vT2 = -lT*w3 + wE2 vT3 = lT*w2 + wE3 alpha = alpha0-wE3/wE1 #NOTE: beta & alphaTail are compensated for the tail motion induced by #omega @>@> betaTail = vT2/C.sqrt(vT1*vT1 + vT3*vT3) beta = wE2/C.sqrt(wE1*wE1 + wE3*wE3) alphaTail = alpha0-vT3/vT1 outputs['alpha(deg)']=alpha*180/C.pi outputs['alphaTail(deg)']=alphaTail*180/C.pi outputs['beta(deg)']=beta*180/C.pi outputs['betaTail(deg)']=betaTail*180/C.pi # cL = cLA*alpha + cLe*elevator + cL0 # cD = cDA*alpha + cDA2*alpha*alpha + cDB2*beta*beta + cDe*elevator + cDr*aileron + cD0 # cR = -rD*w1 + cRB*beta + cRAB*alphaTail*beta + cRr*aileron # cP = cPA*alphaTail + cPe*elevator + cP0 # cY = cYB*beta + cYAB*alphaTail*beta cL_ = cLA*alpha + cLe*elevator + cL0 cD_ = cDA*alpha + cDA2*alpha*alpha + cDB2*beta*beta + cD0 cR = -rD*w1 + cRB*betaTail + cRr*aileron + cRAB*alphaTail*betaTail cP = -pD*w2 + cPA*alphaTail + cPe*elevator + cP0 cY = -yD*w3 + cYB*betaTail + cYAB*alphaTail*betaTail + cYr*rudder # LIFT : # ############################### cL = 0.2*cL_ cD = 0.5*cD_ outputs['cL'] = cL outputs['cD'] = cD fL1 = rho*cL*eLe1*vKite/2.0 fL2 = rho*cL*eLe2*vKite/2.0 fL3 = rho*cL*eLe3*vKite/2.0 # DRAG : # ############################# fD1 = -rho*vKite*cD*we1/2.0 fD2 = -rho*vKite*cD*we2/2.0 fD3 = -rho*vKite*cD*we3/2.0 # FORCES (AERO) # ############################### f1 = fL1 + fD1 f2 = fL2 + fD2 f3 = fL3 + fD3 #f = f-fT # TORQUES (AERO) # ############################### t1 = 0.5*rho*vKite2*span*cR t2 = 0.5*rho*vKite2*chord*cP t3 = 0.5*rho*vKite2*span*cY return (f1, f2, f3, t1, t2, t3)
def setupModel(dae, conf): # PARAMETERS OF THE KITE : # ############## m = conf["kite"]["mass"] # mass of the kite # [ kg ] # PHYSICAL CONSTANTS : # ############## g = conf["env"]["g"] # gravitational constant # [ m /s^2] # PARAMETERS OF THE CABLE : # ############## # INERTIA MATRIX (Kurt's direct measurements) j1 = conf["kite"]["j1"] j31 = conf["kite"]["j31"] j2 = conf["kite"]["j2"] j3 = conf["kite"]["j3"] # Carousel Friction & inertia jCarousel = conf["carousel"]["jCarousel"] cfric = conf["carousel"]["cfric"] zt = conf["kite"]["zt"] rA = conf["carousel"]["rArm"] ########### model integ ################### e11 = dae.x("e11") e12 = dae.x("e12") e13 = dae.x("e13") e21 = dae.x("e21") e22 = dae.x("e22") e23 = dae.x("e23") e31 = dae.x("e31") e32 = dae.x("e32") e33 = dae.x("e33") x = dae.x("x") y = dae.x("y") z = dae.x("z") dx = dae.x("dx") dy = dae.x("dy") dz = dae.x("dz") w1 = dae.x("w1") w2 = dae.x("w2") w3 = dae.x("w3") delta = 0 ddelta = 0 r = dae.x("r") dr = dae.x("dr") ddr = dae.u("ddr") # wind z0 = conf["wind shear"]["z0"] zt_roughness = conf["wind shear"]["zt_roughness"] zsat = 0.5 * (z + C.sqrt(z * z)) wind_x = dae.p("w0") * C.log((zsat + zt_roughness + 2) / zt_roughness) / C.log(z0 / zt_roughness) dae.addOutput("wind at altitude", wind_x) dae.addOutput("w0", dae.p("w0")) dp_carousel_frame = C.veccat([dx - ddelta * y, dy + ddelta * (rA + x), dz]) - C.veccat( [C.cos(delta) * wind_x, C.sin(delta) * wind_x, 0] ) R_c2b = C.veccat(dae.x(["e11", "e12", "e13", "e21", "e22", "e23", "e31", "e32", "e33"])).reshape((3, 3)) # Aircraft velocity w.r.t. inertial frame, given in its own reference frame # (needed to compute the aero forces and torques !) dpE = C.mul(R_c2b, dp_carousel_frame) (f1, f2, f3, t1, t2, t3) = aeroForcesTorques( dae, conf, dp_carousel_frame, dpE, dae.x(("w1", "w2", "w3")), dae.x(("e21", "e22", "e23")), dae.u(("aileron", "elevator")), ) # mass matrix mm = C.SXMatrix(7, 7) mm[0, 0] = m mm[0, 1] = 0 mm[0, 2] = 0 mm[0, 3] = 0 mm[0, 4] = 0 mm[0, 5] = 0 mm[0, 6] = x + zt * e31 mm[1, 0] = 0 mm[1, 1] = m mm[1, 2] = 0 mm[1, 3] = 0 mm[1, 4] = 0 mm[1, 5] = 0 mm[1, 6] = y + zt * e32 mm[2, 0] = 0 mm[2, 1] = 0 mm[2, 2] = m mm[2, 3] = 0 mm[2, 4] = 0 mm[2, 5] = 0 mm[2, 6] = z + zt * e33 mm[3, 0] = 0 mm[3, 1] = 0 mm[3, 2] = 0 mm[3, 3] = j1 mm[3, 4] = 0 mm[3, 5] = j31 mm[3, 6] = -zt * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 + zt * e23 * e33) mm[4, 0] = 0 mm[4, 1] = 0 mm[4, 2] = 0 mm[4, 3] = 0 mm[4, 4] = j2 mm[4, 5] = 0 mm[4, 6] = zt * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 + zt * e13 * e33) mm[5, 0] = 0 mm[5, 1] = 0 mm[5, 2] = 0 mm[5, 3] = j31 mm[5, 4] = 0 mm[5, 5] = j3 mm[5, 6] = 0 mm[6, 0] = x + zt * e31 mm[6, 1] = y + zt * e32 mm[6, 2] = z + zt * e33 mm[6, 3] = -zt * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 + zt * e23 * e33) mm[6, 4] = zt * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 + zt * e13 * e33) mm[6, 5] = 0 mm[6, 6] = 0 # right hand side zt2 = zt * zt rhs = C.veccat( [ f1 + ddelta * m * (dy + ddelta * rA + ddelta * x) + ddelta * dy * m, f2 - ddelta * m * (dx - ddelta * y) - ddelta * dx * m, f3 - g * m, t1 - w2 * (j3 * w3 + j31 * w1) + j2 * w2 * w3, t2 + w1 * (j3 * w3 + j31 * w1) - w3 * (j1 * w1 + j31 * w3), t3 + w2 * (j1 * w1 + j31 * w3) - j2 * w1 * w2, ddr * r - ( zt * w1 * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 + zt * e13 * e33) + zt * w2 * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 + zt * e23 * e33) ) * (w3 - ddelta * e33) - dx * (dx - zt * e21 * (w1 - ddelta * e13) + zt * e11 * (w2 - ddelta * e23)) - dy * (dy - zt * e22 * (w1 - ddelta * e13) + zt * e12 * (w2 - ddelta * e23)) - dz * (dz - zt * e23 * (w1 - ddelta * e13) + zt * e13 * (w2 - ddelta * e23)) + dr * dr + (w1 - ddelta * e13) * ( e21 * (zt * dx - zt2 * e21 * (w1 - ddelta * e13) + zt2 * e11 * (w2 - ddelta * e23)) + e22 * (zt * dy - zt2 * e22 * (w1 - ddelta * e13) + zt2 * e12 * (w2 - ddelta * e23)) + zt * e23 * (dz + zt * e13 * w2 - zt * e23 * w1) + zt * e33 * ( w1 * z + zt * e33 * w1 + ddelta * e11 * x + ddelta * e12 * y + zt * ddelta * e11 * e31 + zt * ddelta * e12 * e32 ) + zt * e31 * (x + zt * e31) * (w1 - ddelta * e13) + zt * e32 * (y + zt * e32) * (w1 - ddelta * e13) ) - (w2 - ddelta * e23) * ( e11 * (zt * dx - zt2 * e21 * (w1 - ddelta * e13) + zt2 * e11 * (w2 - ddelta * e23)) + e12 * (zt * dy - zt2 * e22 * (w1 - ddelta * e13) + zt2 * e12 * (w2 - ddelta * e23)) + zt * e13 * (dz + zt * e13 * w2 - zt * e23 * w1) - zt * e33 * ( w2 * z + zt * e33 * w2 + ddelta * e21 * x + ddelta * e22 * y + zt * ddelta * e21 * e31 + zt * ddelta * e22 * e32 ) - zt * e31 * (x + zt * e31) * (w2 - ddelta * e23) - zt * e32 * (y + zt * e32) * (w2 - ddelta * e23) ), ] ) dRexp = C.SXMatrix(3, 3) dRexp[0, 0] = e21 * (w3 - ddelta * e33) - e31 * (w2 - ddelta * e23) dRexp[0, 1] = e31 * (w1 - ddelta * e13) - e11 * (w3 - ddelta * e33) dRexp[0, 2] = e11 * (w2 - ddelta * e23) - e21 * (w1 - ddelta * e13) dRexp[1, 0] = e22 * (w3 - ddelta * e33) - e32 * (w2 - ddelta * e23) dRexp[1, 1] = e32 * (w1 - ddelta * e13) - e12 * (w3 - ddelta * e33) dRexp[1, 2] = e12 * (w2 - ddelta * e23) - e22 * (w1 - ddelta * e13) dRexp[2, 0] = e23 * w3 - e33 * w2 dRexp[2, 1] = e33 * w1 - e13 * w3 dRexp[2, 2] = e13 * w2 - e23 * w1 c = (x + zt * e31) ** 2 / 2 + (y + zt * e32) ** 2 / 2 + (z + zt * e33) ** 2 / 2 - r ** 2 / 2 cdot = ( dx * (x + zt * e31) + dy * (y + zt * e32) + dz * (z + zt * e33) + zt * (w2 - ddelta * e23) * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 + zt * e13 * e33) - zt * (w1 - ddelta * e13) * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 + zt * e23 * e33) - r * dr ) ddx = dae.z("ddx") ddy = dae.z("ddy") ddz = dae.z("ddz") dw1 = dae.z("dw1") dw2 = dae.z("dw2") dddelta = 0 cddot = ( -(w1 - ddelta * e13) * ( zt * e23 * (dz + zt * e13 * w2 - zt * e23 * w1) + zt * e33 * ( w1 * z + zt * e33 * w1 + ddelta * e11 * x + ddelta * e12 * y + zt * ddelta * e11 * e31 + zt * ddelta * e12 * e32 ) + zt * e21 * (dx + zt * e11 * w2 - zt * e21 * w1 - zt * ddelta * e11 * e23 + zt * ddelta * e13 * e21) + zt * e22 * (dy + zt * e12 * w2 - zt * e22 * w1 - zt * ddelta * e12 * e23 + zt * ddelta * e13 * e22) + zt * e31 * (x + zt * e31) * (w1 - ddelta * e13) + zt * e32 * (y + zt * e32) * (w1 - ddelta * e13) ) + (w2 - ddelta * e23) * ( zt * e13 * (dz + zt * e13 * w2 - zt * e23 * w1) - zt * e33 * ( w2 * z + zt * e33 * w2 + ddelta * e21 * x + ddelta * e22 * y + zt * ddelta * e21 * e31 + zt * ddelta * e22 * e32 ) + zt * e11 * (dx + zt * e11 * w2 - zt * e21 * w1 - zt * ddelta * e11 * e23 + zt * ddelta * e13 * e21) + zt * e12 * (dy + zt * e12 * w2 - zt * e22 * w1 - zt * ddelta * e12 * e23 + zt * ddelta * e13 * e22) - zt * e31 * (x + zt * e31) * (w2 - ddelta * e23) - zt * e32 * (y + zt * e32) * (w2 - ddelta * e23) ) - ddr * r + ( zt * w1 * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 + zt * e13 * e33) + zt * w2 * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 + zt * e23 * e33) ) * (w3 - ddelta * e33) + dx * (dx + zt * e11 * w2 - zt * e21 * w1 - zt * ddelta * e11 * e23 + zt * ddelta * e13 * e21) + dy * (dy + zt * e12 * w2 - zt * e22 * w1 - zt * ddelta * e12 * e23 + zt * ddelta * e13 * e22) + dz * (dz + zt * e13 * w2 - zt * e23 * w1) + ddx * (x + zt * e31) + ddy * (y + zt * e32) + ddz * (z + zt * e33) - dr * dr + zt * (dw2 - dddelta * e23) * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 + zt * e13 * e33) - zt * (dw1 - dddelta * e13) * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 + zt * e23 * e33) - zt * dddelta * ( e11 * e23 * x - e13 * e21 * x + e12 * e23 * y - e13 * e22 * y + zt * e11 * e23 * e31 - zt * e13 * e21 * e31 + zt * e12 * e23 * e32 - zt * e13 * e22 * e32 ) ) # cddot = (zt*w1*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) + zt*w2*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33))*(w3 - ddelta*e33) + dx*(dx + zt*e11*w2 - zt*e21*w1 - zt*ddelta*e11*e23 + zt*ddelta*e13*e21) + dy*(dy + zt*e12*w2 - zt*e22*w1 - zt*ddelta*e12*e23 + zt*ddelta*e13*e22) + dz*(dz + zt*e13*w2 - zt*e23*w1) + ddx*(x + zt*e31) + ddy*(y + zt*e32) + ddz*(z + zt*e33) - (w1 - ddelta*e13)*(e21*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e22*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) + zt*e33*(z*w1 + ddelta*e11*x + ddelta*e12*y + zt*e33*w1 + zt*ddelta*e11*e31 + zt*ddelta*e12*e32) + zt*e23*(dz + zt*e13*w2 - zt*e23*w1) + zt*e31*(w1 - ddelta*e13)*(x + zt*e31) + zt*e32*(w1 - ddelta*e13)*(y + zt*e32)) + (w2 - ddelta*e23)*(e11*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e12*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) - zt*e33*(z*w2 + ddelta*e21*x + ddelta*e22*y + zt*e33*w2 + zt*ddelta*e21*e31 + zt*ddelta*e22*e32) + zt*e13*(dz + zt*e13*w2 - zt*e23*w1) - zt*e31*(w2 - ddelta*e23)*(x + zt*e31) - zt*e32*(w2 - ddelta*e23)*(y + zt*e32)) + zt*(dw2 - dddelta*e23)*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) - zt*(dw1 - dddelta*e13)*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33) - zt*dddelta*(e11*e23*x - e13*e21*x + e12*e23*y - e13*e22*y + zt*e11*e23*e31 - zt*e13*e21*e31 + zt*e12*e23*e32 - zt*e13*e22*e32) # where # dw1 = dw @> 0 # dw2 = dw @> 1 # {- # dw3 = dw @> 2 # -} # ddx = ddX @> 0 # ddy = ddX @> 1 # ddz = ddX @> 2 # dddelta = dddelta' @> 0 dae.addOutput("c", c) dae.addOutput("cdot", cdot) dae.addOutput("cddot", cddot) return (mm, rhs, dRexp)
def get_p_1_tm1_agg_1(): d_1, e_1, lambda_1, gamma_1, h_1_tm1, p_1_tm1, d_2, e_2, lambda_2, gamma_2, h_2_tm1, p_2_tm1 = casadi.SX.sym('d_1'), casadi.SX.sym('e_1'), casadi.SX.sym('lambda_1'), casadi.SX.sym('gamma_1'), casadi.SX.sym('h_1_tm1'), casadi.SX.sym('p_1_tm1'), casadi.SX.sym('d_2'), casadi.SX.sym('e_2'), casadi.SX.sym('lambda_2'), casadi.SX.sym('gamma_2'), casadi.SX.sym('h_2_tm1'), casadi.SX.sym('p_2_tm1'), p1term = cdf(d_1+e_1*(lambda_1 + gamma_1 * casadi.sqrt(h_1_tm1)))*p_1_tm1 p2term = (1 - cdf(d_2+e_2*(lambda_2 + gamma_2 * casadi.sqrt(h_2_tm1))))*p_2_tm1 z = p1term/(p1term + p2term) return casadi.Function('p_1_tm1_agg_1', [d_1, e_1, lambda_1, gamma_1, h_1_tm1, p_1_tm1, d_2, e_2, lambda_2, gamma_2, h_2_tm1, p_2_tm1], [z])
def get_h_tm1_agg_i(): p_1_tm1_agg_i, h_1_tm1, h_2_tm1, lambda1, gamma1, lambda2, gamma2 = casadi.SX.sym('p_1_tm1_agg_i'), casadi.SX.sym('h_1_tm1'), casadi.SX.sym('h_2_tm1'), casadi.SX.sym('lambda1'), casadi.SX.sym('gamma1'), casadi.SX.sym('lambda2'), casadi.SX.sym('gamma2'), z = (p_1_tm1_agg_i * h_1_tm1 + (1-p_1_tm1_agg_i) * h_2_tm1 + p_1_tm1_agg_i * (1-p_1_tm1_agg_i) * (lambda1 + gamma1*casadi.sqrt(h_1_tm1) - (lambda2 + gamma2*casadi.sqrt(h_2_tm1))) **2) return casadi.Function('h_tm1_agg_i', [p_1_tm1_agg_i, h_1_tm1, h_2_tm1, lambda1, gamma1, lambda2, gamma2], [z])
def orthonormalizeDcm(m): ## OGRE (www.ogre3d.org) is made available under the MIT License. ## ## Copyright (c) 2000-2009 Torus Knot Software Ltd ## ## Permission is hereby granted, free of charge, to any person obtaining a copy ## of this software and associated documentation files (the "Software"), to deal ## in the Software without restriction, including without limitation the rights ## to use, copy, modify, merge, publish, distribute, sublicense, and/or sell ## copies of the Software, and to permit persons to whom the Software is ## furnished to do so, subject to the following conditions: ## ## The above copyright notice and this permission notice shall be included in ## all copies or substantial portions of the Software. ## ## THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR ## IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, ## FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE ## AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER ## LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, ## OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN ## THE SOFTWARE. # Algorithm uses Gram-Schmidt orthogonalization. If 'this' matrix is # M = [m0|m1|m2], then orthonormal output matrix is Q = [q0|q1|q2], # # q0 = m0/|m0| # q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0| # q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1| # # where |V| indicates length of vector V and A*B indicates dot # product of vectors A and B. m00 = m[0,0] m01 = m[0,1] m02 = m[0,2] m10 = m[1,0] m11 = m[1,1] m12 = m[1,2] m20 = m[2,0] m21 = m[2,1] m22 = m[2,2] # compute q0 fInvLength = 1.0/C.sqrt(m00*m00 + m10*m10 + m20*m20) m00 *= fInvLength m10 *= fInvLength m20 *= fInvLength # compute q1 fDot0 = m00*m01 + m10*m11 + m20*m21 m01 -= fDot0*m00 m11 -= fDot0*m10 m21 -= fDot0*m20 fInvLength = 1.0/C.sqrt(m01*m01 + m11*m11 + m21*m21) m01 *= fInvLength m11 *= fInvLength m21 *= fInvLength # compute q2 fDot1 = m01*m02 + m11*m12 + m21*m22 fDot0 = m00*m02 + m10*m12 + m20*m22 m02 -= fDot0*m00 + fDot1*m01 m12 -= fDot0*m10 + fDot1*m11 m22 -= fDot0*m20 + fDot1*m21 fInvLength = 1.0/C.sqrt(m02*m02 + m12*m12 + m22*m22) m02 *= fInvLength m12 *= fInvLength m22 *= fInvLength return C.vertcat([C.horzcat([m00,m01,m02]), C.horzcat([m10,m11,m12]), C.horzcat([m20,m21,m22])])
cPA = conf['aero']['cPA'] cPe = conf['aero']['cPe'] cP0 = conf['aero']['cP0'] #Yaw (report p. 76) cYB = conf['aero']['cYB'] cYAB = conf['aero']['cYAB'] #TAIL LENGTH lT = conf['kite']['lT'] AR = conf['kite']['AR'] area = conf['kite']['area'] span = C.sqrt(area*AR) chord = C.sqrt(area/AR) ##### more model_integ ########### # EFFECTIVE WIND IN THE KITE`S SYSTEM : # ############################### #Airfoil speed in carousel frame we1 = we[0] we2 = we[1] we3 = we[2] vKite2 = C.mul(we.trans(), we) #Airfoil speed^2 vKite = C.sqrt(vKite2) #Airfoil speed dae.addOutput('airspeed', vKite)
, 0.316039689643 , -0.073559821379 , 0.945889986864 , 0.940484536806 , -0.106993361072 , -0.322554269411 , 0.000000000000 , 0.000000000000 , 0.000000000000 , 0.137035790811 , 3.664945343102 , -1.249768772258 , 3.874600000000 , 0.0 ]) x0=C.veccat([x0,C.sqrt(C.sumAll(x0[0:2]*x0[0:2])),0,0,0]) def setupOcp(dae,conf,nk=50,nicp=1,deg=4): ocp = rawe.collocation.Coll(dae, nk=nk,nicp=nicp,deg=deg) ocp.setupCollocation(ocp.lookup('endTime')) # constrain invariants def constrainInvariantErrs(): dcm = ocp.lookup('dcm',timestep=0) rawekite.kiteutils.makeOrthonormal(ocp, dcm) ocp.constrain(ocp.lookup('c',timestep=0), '==', 0) ocp.constrain(ocp.lookup('cdot',timestep=0), '==', 0) constrainInvariantErrs() # constraint line angle for k in range(0,nk):
m = 7.0 # kg Cd = 0.47 # Drag coefficient for a sphere rho_water = 1000. # kg / m^2 pi = 3.1415 # volume = 4/3 * pi * r*r*r # m = volume * rho_water # m/rho_water = 4/3 * pi * r*r*r r = (m/rho_water*3./4./pi)**(1./3.) A = pi * r * r rho_air = 1.2 # kg / m^2 # Constant in front of the v^2 CD = 0.5*Cd*A*rho_air # Aero. force in world frame as function of theta,t vdotv = mul(v_w.T,v_w) speed = sqrt(vdotv) f_w = -CD*speed*v_w # Generalized aerodynamic force dxdq=casadi.vertcat([r*-s_theta, r*c_theta]) f_gen = mul(dxdq.T,f_w) # Formulate the Lagrangian KE = 0.5*m*vdotv PE = 0 L = KE-PE #################### # Derive the implicit ODE (DAE-like) form of the equations of motion ####################
def sqrt(inputobj): return ca.sqrt(inputobj)
for j, e in enumerate(p_true): p_mean.append(pl.mean([k[j] for k in p_test])) p_std.append(pl.std([k[j] for k in p_test], ddof = 0)) lsqpe_test.compute_covariance_matrix() # Generate report print("\np_mean = " + str(ca.DMatrix(p_mean))) print("phat_last_exp = " + str(ca.DMatrix(lsqpe_test.phat))) print("\np_sd = " + str(ca.DMatrix(p_std))) print("sd_from_covmat = " + str(ca.diag(ca.sqrt(lsqpe_test.Covp)))) print("beta = " + str(lsqpe_test.beta)) print("\ndelta_abs_sd = " + str(ca.fabs(ca.DMatrix(p_std) - \ ca.diag(ca.sqrt(lsqpe_test.Covp))))) print("delta_rel_sd = " + str(ca.fabs(ca.DMatrix(p_std) - \ ca.diag(ca.sqrt(lsqpe_test.Covp))) / ca.DMatrix(p_std))) fname = os.path.basename(__file__)[:-3] + ".rst" report = open(fname, "w") report.write( \ '''Concept test: covariance matrix computation ===========================================
def get_f_i(): delta_i_t, b_i, c_i = casadi.SX.sym('delta_i_t'), casadi.SX.sym('b_i'), casadi.SX.sym('c_i'), z = casadi.sqrt((delta_i_t - b_i)**2) - c_i * (delta_i_t - b_i) return casadi.Function('f_i', [delta_i_t, b_i, c_i], [z])
def show_results(self): r''' :raises: AttributeError This function displays the results of the parameter estimation computations. It can not be used before function :func:`run_parameter_estimation()` has been used. The results displayed by the function contain: - the values of the estimated parameters :math:`\hat{p}` and their corresponding standard deviations (the values of the standard deviations are presented only if the covariance matrix had already been computed), - the values of the covariance matrix :math:`\Sigma_{\hat{p}}` for the estimated parameters (if it had already been computed), - in the case of the estimation of a dynamic system, the estimated value of the first state :math:`\hat{x}(t_{0})` and the estimated value of the last state :math:`\hat{x}(t_{N})`, - the value of :math:`R^2` measuring the goodness of fit of the estimated parameters, and - the durations of the setup and the estimation. ''' intro.pecas_intro() np.set_printoptions(linewidth = 200, \ formatter={'float': lambda x: format(x, ' 10.8e')}) try: print('\n' + 21 * '-' + \ ' PECas Parameter estimation results ' + 21 * '-') print("\nEstimated parameters p_i:") for i, xi in enumerate(self.phat): try: print(" p_{0:<3} = {1: 10.8e} +/- {2: 10.8e}".format(\ i, xi[0], ca.sqrt(abs(self.Covp[i, i])))) except AttributeError: print(" p_{0:<3} = {1: 10.8e}".format(\ i, xi[0])) try: print("\nInitial states value estimated from collocation: ") print(" x(t_0) = {0}".format(self.Xhat[:,0])) print("\nFinal states value estimated from collocation: ") print(" x(t_N) = {0}".format(self.Xhat[:,-1])) except AttributeError: pass print("\nCovariance matrix for the estimated parameters:") try: print(np.atleast_2d(self.Covp)) except AttributeError: print( \ ''' Covariance matrix for the estimated parameters not yet computed. Run class function compute_covariance_matrix() to do so.''') print( \ "\nGoodness of fit R^2" + 30 * "." + ": {0:10.8e}".format(\ float(self.R_squared))) print("\nDuration of the problem setup"+ 20 * "." + \ ": {0:10.8e} s".format(self.pesetup.duration_setup)) print("Duration of the estimation" + 23 * "." + \ ": {0:10.8e} s".format(self.duration_estimation)) try: print("Duration of the covariance matrix computation...." + \ ": {0:10.8e} s".format(self.duration_cov_computation)) except AttributeError: pass except AttributeError: raise AttributeError(''' You must execute at least run_parameter_estimation() to obtain results, and compute_covariance_matrix() before all results can be displayed. ''') finally: np.set_printoptions()
def carouselModel(conf): ''' pass this a conf, and it'll return you a dae ''' # empty Dae dae = Dae() # add some differential states/algebraic vars/controls/params dae.addZ("nu") dae.addX( [ "x" , "y" , "z" , "e11" , "e12" , "e13" , "e21" , "e22" , "e23" , "e31" , "e32" , "e33" , "dx" , "dy" , "dz" , "w_bn_b_x" , "w_bn_b_y" , "w_bn_b_z" , "ddelta" , "r" , "dr" , "aileron" , "elevator" , "motor_torque" , "ddr" ] ) if 'cn_rudder' in conf: dae.addX('rudder') dae.addU('drudder') if 'cL_flaps' in conf: dae.addX('flaps') dae.addU('dflaps') if conf['delta_parameterization'] == 'linear': dae.addX('delta') dae['cos_delta'] = C.cos(dae['delta']) dae['sin_delta'] = C.sin(dae['delta']) dae_delta_residual = dae.ddt('delta') - dae['ddelta'] elif conf['delta_parameterization'] == 'cos_sin': dae.addX("cos_delta") dae.addX("sin_delta") norm = dae['cos_delta']**2 + dae['sin_delta']**2 if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True: pole_delta = 0.5 else: pole_delta = 0.0 cos_delta_dot_st = -pole_delta/2.* ( dae['cos_delta'] - dae['cos_delta'] / norm ) sin_delta_dot_st = -pole_delta/2.* ( dae['sin_delta'] - dae['sin_delta'] / norm ) dae_delta_residual = C.veccat([dae.ddt('cos_delta') - (-dae['sin_delta']*dae['ddelta'] + cos_delta_dot_st), dae.ddt('sin_delta') - ( dae['cos_delta']*dae['ddelta'] + sin_delta_dot_st) ]) else: raise ValueError('unrecognized delta_parameterization "'+conf['delta_parameterization']+'"') dae.addU( [ "daileron" , "delevator" , "dmotor_torque" , 'dddr' ] ) # add wind parameter if wind shear is in configuration if 'wind_model' in conf: if conf['wind_model']['name'] == 'hardcoded': dae['w0'] = conf['wind_model']['hardcoded_value'] elif conf['wind_model']['name'] != 'random_walk': dae.addP( ['w0'] ) # set some state derivatives as outputs dae['ddx'] = dae.ddt('dx') dae['ddy'] = dae.ddt('dy') dae['ddz'] = dae.ddt('dz') dae['ddt_w_bn_b_x'] = dae.ddt('w_bn_b_x') dae['ddt_w_bn_b_y'] = dae.ddt('w_bn_b_y') dae['ddt_w_bn_b_z'] = dae.ddt('w_bn_b_z') dae['w_bn_b'] = C.veccat([dae['w_bn_b_x'], dae['w_bn_b_y'], dae['w_bn_b_z']]) # some outputs in for plotting dae['carousel_RPM'] = dae['ddelta']*60/(2*C.pi) dae['aileron_deg'] = dae['aileron']*180/C.pi dae['elevator_deg'] = dae['elevator']*180/C.pi dae['daileron_deg_s'] = dae['daileron']*180/C.pi dae['delevator_deg_s'] = dae['delevator']*180/C.pi # tether tension == radius*nu where nu is alg. var associated with x^2+y^2+z^2-r^2==0 dae['tether_tension'] = dae['r']*dae['nu'] # theoretical mechanical power dae['mechanical_winch_power'] = -dae['tether_tension']*dae['dr'] # carousel2 motor model from thesis data dae['rpm'] = -dae['dr']/0.1*60/(2*C.pi) dae['torque'] = dae['tether_tension']*0.1 dae['electrical_winch_power'] = 293.5816373499238 + \ 0.0003931623408*dae['rpm']*dae['rpm'] + \ 0.0665919381751*dae['torque']*dae['torque'] + \ 0.1078628659825*dae['rpm']*dae['torque'] dae['R_c2b'] = C.blockcat([[dae['e11'],dae['e12'],dae['e13']], [dae['e21'],dae['e22'],dae['e23']], [dae['e31'],dae['e32'],dae['e33']]]) dae["yaw"] = C.arctan2(dae["e12"], dae["e11"]) * 180.0 / C.pi dae["pitch"] = C.arcsin( -dae["e13"] ) * 180.0 / C.pi dae["roll"] = C.arctan2(dae["e23"], dae["e33"]) * 180.0 / C.pi # line angle dae['cos_line_angle'] = \ -(dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2) dae['line_angle_deg'] = C.arccos(dae['cos_line_angle'])*180.0/C.pi (massMatrix, rhs, dRexp) = setupModel(dae, conf) if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True: RotPole = 0.5 else: RotPole = 0.0 Rst = RotPole*C.mul( dae['R_c2b'], (C.inv(C.mul(dae['R_c2b'].T,dae['R_c2b'])) - numpy.eye(3)) ) ode = C.veccat([ C.veccat([dae.ddt(name) for name in ['x','y','z']]) - C.veccat([dae['dx'],dae['dy'],dae['dz']]), C.veccat([dae.ddt(name) for name in ["e11","e12","e13", "e21","e22","e23", "e31","e32","e33"]]) - ( dRexp.T.reshape([9,1]) + Rst.reshape([9,1]) ), dae_delta_residual, # C.veccat([dae.ddt(name) for name in ['dx','dy','dz']]) - C.veccat([dae['ddx'],dae['ddy'],dae['ddz']]), # C.veccat([dae.ddt(name) for name in ['w1','w2','w3']]) - C.veccat([dae['dw1'],dae['dw2'],dae['dw3']]), # dae.ddt('ddelta') - dae['dddelta'], dae.ddt('r') - dae['dr'], dae.ddt('dr') - dae['ddr'], dae.ddt('aileron') - dae['daileron'], dae.ddt('elevator') - dae['delevator'], dae.ddt('motor_torque') - dae['dmotor_torque'], dae.ddt('ddr') - dae['dddr'] ]) if 'rudder' in dae: ode = C.veccat([ode, dae.ddt('rudder') - dae['drudder']]) if 'flaps' in dae: ode = C.veccat([ode, dae.ddt('flaps') - dae['dflaps']]) if 'useVirtualTorques' in conf: _v = conf[ 'useVirtualTorques' ] if isinstance(_v, str): _type = _v _which = True, True, True else: assert isinstance(_v, dict) _type = _v["type"] _which = _v["which"] if _type == 'random_walk': if _which[ 0 ]: ode = C.veccat([ode, dae.ddt('t1_disturbance') - dae['dt1_disturbance']]) if _which[ 1 ]: ode = C.veccat([ode, dae.ddt('t2_disturbance') - dae['dt2_disturbance']]) if _which[ 2 ]: ode = C.veccat([ode, dae.ddt('t3_disturbance') - dae['dt3_disturbance']]) elif _type == 'parameter': if _which[ 0 ]: ode = C.veccat([ode, dae.ddt('t1_disturbance')]) if _which[ 1 ]: ode = C.veccat([ode, dae.ddt('t2_disturbance')]) if _which[ 2 ]: ode = C.veccat([ode, dae.ddt('t3_disturbance')]) if 'useVirtualForces' in conf: _v = conf[ 'useVirtualForces' ] if isinstance(_v, str): _type = _v _which = True, True, True else: assert isinstance(_v, dict) _type = _v["type"] _which = _v["which"] if _type is 'random_walk': if _which[ 0 ]: ode = C.veccat([ode, dae.ddt('f1_disturbance') - dae['df1_disturbance']]) if _which[ 1 ]: ode = C.veccat([ode, dae.ddt('f2_disturbance') - dae['df2_disturbance']]) if _which[ 2 ]: ode = C.veccat([ode, dae.ddt('f3_disturbance') - dae['df3_disturbance']]) elif _type == 'parameter': if _which[ 0 ]: ode = C.veccat([ode, dae.ddt('f1_disturbance')]) if _which[ 1 ]: ode = C.veccat([ode, dae.ddt('f2_disturbance')]) if _which[ 2 ]: ode = C.veccat([ode, dae.ddt('f3_disturbance')]) if 'wind_model' in conf and conf['wind_model']['name'] == 'random_walk': tau_wind = conf['wind_model']['tau_wind'] ode = C.veccat([ode, dae.ddt('wind_x') - (-dae['wind_x'] / tau_wind + dae['delta_wind_x']) , dae.ddt('wind_y') - (-dae['wind_y'] / tau_wind + dae['delta_wind_y']) , dae.ddt('wind_z') - (-dae['wind_z'] / tau_wind + dae['delta_wind_z']) ]) if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True: cPole = 0.5 else: cPole = 0.0 rhs[-1] -= 2*cPole*dae['cdot'] + cPole*cPole*dae['c'] psuedoZVec = C.veccat([dae.ddt(name) for name in ['ddelta','dx','dy','dz','w_bn_b_x','w_bn_b_y','w_bn_b_z']]+[dae['nu']]) alg = C.mul(massMatrix, psuedoZVec) - rhs dae.setResidual([ode,alg]) return dae
def get_delta_t_agg_i(): p_1_tm1_agg_i, r_t, lambda_1, gamma_1, lambda_2, gamma_2, h_1_tm1, h_2_tm1 = casadi.SX.sym('p_1_tm1_agg_i'), casadi.SX.sym('r_t'), casadi.SX.sym('lambda_1'), casadi.SX.sym('gamma_1'), casadi.SX.sym('lambda_2'), casadi.SX.sym('gamma_2'), casadi.SX.sym('h_1_tm1'), casadi.SX.sym('h_2_tm1'), p1 = p_1_tm1_agg_i * ((r_t- (lambda_1 + gamma_1 * casadi.sqrt(h_1_tm1)))/casadi.sqrt(h_1_tm1)) p2 = (1 - p_1_tm1_agg_i) * ((r_t- (lambda_2 + gamma_2 * casadi.sqrt(h_2_tm1)))/casadi.sqrt(h_2_tm1)) z = p1 + p2 return casadi.Function('delta_t_agg_i', [p_1_tm1_agg_i, r_t, lambda_1, gamma_1, lambda_2, gamma_2, h_1_tm1, h_2_tm1], [z])
def crosswind_model(conf): ''' pass this a conf, and it'll return you a dae ''' # empty Dae dae = Dae() # add some differential states/algebraic vars/controls/params dae.addZ('nu') dae.addX( ['r_n2b_n_x', 'r_n2b_n_y', 'r_n2b_n_z', 'e11', 'e12', 'e13', 'e21', 'e22', 'e23', 'e31', 'e32', 'e33', 'v_bn_n_x', 'v_bn_n_y', 'v_bn_n_z', 'w_bn_b_x', 'w_bn_b_y', 'w_bn_b_z', 'aileron', 'elevator', 'rudder', 'flaps', 'prop_drag' ]) dae.addU( [ 'daileron' , 'delevator' , 'drudder' , 'dflaps' , 'dprop_drag' ] ) dae.addP( ['r','w0'] ) # set some state derivatives as outputs dae['ddx'] = dae.ddt('v_bn_n_x') dae['ddy'] = dae.ddt('v_bn_n_y') dae['ddz'] = dae.ddt('v_bn_n_z') dae['ddt_w_bn_b_x'] = dae.ddt('w_bn_b_x') dae['ddt_w_bn_b_y'] = dae.ddt('w_bn_b_y') dae['ddt_w_bn_b_z'] = dae.ddt('w_bn_b_z') dae['w_bn_b'] = C.veccat([dae['w_bn_b_x'], dae['w_bn_b_y'], dae['w_bn_b_z']]) # some outputs in degrees for plotting dae['aileron_deg'] = dae['aileron']*180/C.pi dae['elevator_deg'] = dae['elevator']*180/C.pi dae['rudder_deg'] = dae['rudder']*180/C.pi dae['daileron_deg_s'] = dae['daileron']*180/C.pi dae['delevator_deg_s'] = dae['delevator']*180/C.pi dae['drudder_deg_s'] = dae['drudder']*180/C.pi # tether tension == radius*nu where nu is alg. var associated with x^2+y^2+z^2-r^2==0 dae['tether_tension'] = dae['r']*dae['nu'] dae['R_n2b'] = C.vertcat([C.horzcat([dae['e11'], dae['e12'], dae['e13']]), C.horzcat([dae['e21'], dae['e22'], dae['e23']]), C.horzcat([dae['e31'], dae['e32'], dae['e33']])]) # local wind dae['wind_at_altitude'] = get_wind(dae, conf) # wind velocity in NED dae['v_wn_n'] = C.veccat([dae['wind_at_altitude'], 0, 0]) # body velocity in NED dae['v_bn_n'] = C.veccat( [ dae['v_bn_n_x'] , dae['v_bn_n_y'], dae['v_bn_n_z'] ] ) # body velocity w.r.t. wind in NED v_bw_n = dae['v_bn_n'] - dae['v_wn_n'] # body velocity w.r.t. wind in body frame v_bw_b = C.mul( dae['R_n2b'], v_bw_n ) # compute aerodynamic forces and moments (f1, f2, f3, t1, t2, t3) = \ aeroForcesTorques(dae, conf, v_bw_n, v_bw_b, dae['w_bn_b'], (dae['e21'], dae['e22'], dae['e23'])) dae['prop_drag_vector'] = dae['prop_drag']*v_bw_n/dae['airspeed'] dae['prop_power'] = C.mul(dae['prop_drag_vector'].T, v_bw_n) f1 -= dae['prop_drag_vector'][0] f2 -= dae['prop_drag_vector'][1] f3 -= dae['prop_drag_vector'][2] # if we are running a homotopy, # add psudeo forces and moments as algebraic states if 'runHomotopy' in conf and conf['runHomotopy']: gamma_homotopy = dae.addP('gamma_homotopy') f1 = f1*gamma_homotopy + dae.addZ('f1_homotopy')*(1 - gamma_homotopy) f2 = f2*gamma_homotopy + dae.addZ('f2_homotopy')*(1 - gamma_homotopy) f3 = f3*gamma_homotopy + dae.addZ('f3_homotopy')*(1 - gamma_homotopy) t1 = t1*gamma_homotopy + dae.addZ('t1_homotopy')*(1 - gamma_homotopy) t2 = t2*gamma_homotopy + dae.addZ('t2_homotopy')*(1 - gamma_homotopy) t3 = t3*gamma_homotopy + dae.addZ('t3_homotopy')*(1 - gamma_homotopy) # derivative of dcm dRexp = C.mul(skew_mat(dae['w_bn_b']).T, dae['R_n2b']) ddt_R_n2b = C.vertcat(\ [C.horzcat([dae.ddt(name) for name in ['e11', 'e12', 'e13']]), C.horzcat([dae.ddt(name) for name in ['e21', 'e22', 'e23']]), C.horzcat([dae.ddt(name) for name in ['e31', 'e32', 'e33']])]) # get mass matrix, rhs (mass_matrix, rhs) = compute_mass_matrix(dae, conf, f1, f2, f3, t1, t2, t3) # set up the residual ode = C.veccat([ C.veccat([dae.ddt('r_n2b_n_'+name) - dae['v_bn_n_'+name] for name in ['x','y','z']]), C.veccat([ddt_R_n2b - dRexp]), dae.ddt('aileron') - dae['daileron'], dae.ddt('elevator') - dae['delevator'], dae.ddt('rudder') - dae['drudder'], dae.ddt('flaps') - dae['dflaps'], dae.ddt('prop_drag') - dae['dprop_drag'] ]) # acceleration for plotting ddx = dae['ddx'] ddy = dae['ddy'] ddz = dae['ddz'] dae['accel_g'] = C.sqrt(ddx**2 + ddy**2 + (ddz + 9.8)**2)/9.8 dae['accel_without_gravity_g'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)/9.8 dae['accel'] = C.sqrt(ddx**2 + ddy**2 + (ddz+9.8)**2) dae['accel_without_gravity'] = C.sqrt(ddx**2 + ddy**2 + ddz**2) # line angle dae['cos_line_angle'] = \ -(dae['e31']*dae['r_n2b_n_x'] + dae['e32']*dae['r_n2b_n_y'] + dae['e33']*dae['r_n2b_n_z']) / \ C.sqrt(dae['r_n2b_n_x']**2 + dae['r_n2b_n_y']**2 + dae['r_n2b_n_z']**2) dae['line_angle_deg'] = C.arccos(dae['cos_line_angle'])*180.0/C.pi # add local loyd's limit def addLoydsLimit(): w = dae['wind_at_altitude'] cL = dae['cL'] cD = dae['cD'] + dae['cD_tether'] rho = conf['rho'] S = conf['sref'] loyds = 2/27.0*rho*S*w**3*cL**3/cD**2 loyds2 = 2/27.0*rho*S*w**3*(cL**2/cD**2 + 1)**(1.5)*cD dae["loyds_limit"] = loyds dae["loyds_limit_exact"] = loyds2 addLoydsLimit() psuedo_zvec = C.veccat([dae.ddt(name) for name in \ ['v_bn_n_x','v_bn_n_y','v_bn_n_z','w_bn_b_x','w_bn_b_y','w_bn_b_z']]+[dae['nu']]) alg = C.mul(mass_matrix, psuedo_zvec) - rhs dae.setResidual( [ode, alg] ) return dae
def getCdf(): x = casadi.SX.sym('x') sqrt2 = casadi.sqrt(2) return casadi.Function('cdf', [x], [.5 - .5 * casadi.erf(-x/sqrt2)])
(f_f_1, f_f_2, f_f_3, t_b_1, t_b_2, t_b_3) First three entries denote forces, expressed in frame (f) Last three entries denote moments, expressed in body frame (b) """ eTe_f = C.veccat([eTe_f_1, eTe_f_2, eTe_f_3]) rho = conf["rho"] alpha0 = conf["alpha0deg"] * C.pi / 180 sref = conf["sref"] bref = conf["bref"] cref = conf["cref"] vKite2 = C.mul(v_bw_f.T, v_bw_f) # Airfoil speed^2 vKite = C.sqrt(vKite2) # Airfoil speed dae["airspeed"] = vKite # Lift axis, normed to airspeed eLe_v_f = C.cross(eTe_f, v_bw_f) # sideforce axis, normalized to airspeed^2 eYe_v2_f = C.cross(eLe_v_f, -v_bw_f) if conf["alpha_beta_computation"] == "first_order": alpha = alpha0 + v_bw_b[2] / v_bw_b[0] beta = v_bw_b[1] / v_bw_b[0] # beta = v_bw_b_y / C.sqrt(v_bw_b[0]*v_bw_b[0] + v_bw_b[2]*v_bw_b[2]) elif conf["alpha_beta_computation"] == "closed_form": (alpha, beta) = getWindAnglesFrom_v_bw_b(vKite, v_bw_b) alpha += alpha0