Esempio n. 1
0
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])}
Esempio n. 2
0
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)
Esempio n. 3
0
    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)
Esempio n. 4
0
    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
Esempio n. 5
0
    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
Esempio n. 6
0
    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')
Esempio n. 7
0
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])
Esempio n. 8
0
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
Esempio n. 10
0
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
Esempio n. 12
0
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))
Esempio n. 13
0
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
Esempio n. 14
0
 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
Esempio n. 15
0
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
Esempio n. 16
0
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)
Esempio n. 17
0
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
Esempio n. 18
0
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
Esempio n. 19
0
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
Esempio n. 20
0
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
Esempio n. 21
0
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]
Esempio n. 22
0
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
Esempio n. 24
0
 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
Esempio n. 27
0
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
Esempio n. 28
0
    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
Esempio n. 29
0
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
Esempio n. 30
0
 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)
Esempio n. 31
0
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]
Esempio n. 32
0
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]
Esempio n. 33
0
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])
Esempio n. 34
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)))
Esempio n. 35
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,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)))
Esempio n. 36
0
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
Esempio n. 37
0
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
Esempio n. 38
0
    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
Esempio n. 39
0
    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
Esempio n. 40
0
 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']
Esempio n. 41
0
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])
Esempio n. 42
0

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
===========================================
Esempio n. 43
0
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)
Esempio n. 44
0
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)
Esempio n. 45
0
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])
Esempio n. 46
0
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])
Esempio n. 47
0
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])])
Esempio n. 48
0
    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)
    
Esempio n. 49
0
                , 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):
Esempio n. 50
0
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
####################
Esempio n. 51
0
def sqrt(inputobj):

    return ca.sqrt(inputobj)
Esempio n. 52
0
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
===========================================
Esempio n. 53
0
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])
Esempio n. 54
0
    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()
Esempio n. 55
0
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
Esempio n. 56
0
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])
Esempio n. 57
0
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
Esempio n. 58
0
def getCdf():
    x = casadi.SX.sym('x')
    sqrt2 = casadi.sqrt(2)
    return casadi.Function('cdf', [x], [.5 - .5 * casadi.erf(-x/sqrt2)])
Esempio n. 59
0
      (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