def compute_mass_matrix(dae, conf, f1, f2, f3, t1, t2, t3): ''' take the dae that has x/z/u/p added to it already and return the states added to it and return mass matrix and rhs of the dae residual ''' R_b2n = dae['R_n2b'].T r_n2b_n = C.veccat([dae['r_n2b_n_x'], dae['r_n2b_n_y'], dae['r_n2b_n_z']]) r_b2bridle_b = C.veccat([0,0,conf['zt']]) v_bn_n = C.veccat([dae['v_bn_n_x'],dae['v_bn_n_y'],dae['v_bn_n_z']]) r_n2bridle_n = r_n2b_n + C.mul(R_b2n, r_b2bridle_b) mm00 = C.diag([1,1,1]) * (conf['mass'] + conf['tether_mass']/3.0) mm01 = C.SXMatrix(3,3) mm10 = mm01.T mm02 = r_n2bridle_n mm20 = mm02.T J = C.blockcat([[ conf['j1'], 0, conf['j31']], [ 0, conf['j2'], 0], [conf['j31'], 0, conf['j3']]]) mm11 = J mm12 = C.cross(r_b2bridle_b, C.mul(dae['R_n2b'], r_n2b_n)) mm21 = mm12.T mm22 = C.SXMatrix(1,1) mm = C.blockcat([[mm00,mm01,mm02], [mm10,mm11,mm12], [mm20,mm21,mm22]]) # right hand side rhs0 = C.veccat([f1,f2,f3 + conf['g']*(conf['mass'] + conf['tether_mass']*0.5)]) rhs1 = C.veccat([t1,t2,t3]) - C.cross(dae['w_bn_b'], C.mul(J, dae['w_bn_b'])) # last element of RHS R_n2b = dae['R_n2b'] w_bn_b = dae['w_bn_b'] grad_r_cdot = v_bn_n + C.mul(R_b2n, C.cross(dae['w_bn_b'], r_b2bridle_b)) tPR = - C.cross(C.mul(R_n2b, r_n2b_n), C.cross(w_bn_b, r_b2bridle_b)) - \ C.cross(C.mul(R_n2b, v_bn_n), r_b2bridle_b) rhs2 = -C.mul(grad_r_cdot.T, v_bn_n) - C.mul(tPR.T, w_bn_b) + dae['dr']**2 + dae['r']*dae['ddr'] rhs = C.veccat([rhs0,rhs1,rhs2]) c = 0.5*(C.mul(r_n2bridle_n.T, r_n2bridle_n) - dae['r']**2) v_bridlen_n = v_bn_n + C.mul(R_b2n, C.cross(w_bn_b, r_b2bridle_b)) cdot = C.mul(r_n2bridle_n.T, v_bridlen_n) - dae['r']*dae['dr'] a_bn_n = C.veccat([dae.ddt(name) for name in ['v_bn_n_x','v_bn_n_y','v_bn_n_z']]) dw_bn_b = C.veccat([dae.ddt(name) for name in ['w_bn_b_x','w_bn_b_y','w_bn_b_z']]) a_bridlen_n = a_bn_n + C.mul(R_b2n, C.cross(dw_bn_b, r_b2bridle_b) + C.cross(w_bn_b, C.cross(w_bn_b, r_b2bridle_b))) cddot = C.mul(v_bridlen_n.T, v_bridlen_n) + C.mul(r_n2bridle_n.T, a_bridlen_n) - \ dae['dr']**2 - dae['r']*dae['ddr'] dae['c'] = c dae['cdot'] = cdot dae['cddot'] = cddot return (mm, rhs)
def U_upper_bound(self, robot): ones = c.DM.ones(robot.nu, self.T) ub = c.blockcat([[robot.vel_max * ones[0, :]], [robot.ang_vel_max * ones[1, :]]]) ub = c.reshape(ub, robot.nu * self.T, 1) return ub
def kinematics(self, state, U, epsilon=0): f,_,_ = self.proc_model() w0 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[0,0])) w1 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[1,1])) w = c.DM([[w0],[w1]]) #state_n = state + self.dt*c.blockcat([[vx],[vy],[vtheta]]) state_n = f(state,c.blockcat([[U[0] + w[0]],[U[1] + w[1]]])) # state_n = c.MX(self.nx,1) # state_n[0] = f(state,c.blockcat([[U[0] + w[0]],[U[1] + w[1]]]))[0] # state_n[1] = f(state,c.blockcat([[U[0] + w[0]],[U[1] + w[1]]]))[1] # state_n[2] = f(state,c.blockcat([[U[0] + w[0]],[U[1] + w[1]]]))[2] # state_n[3] = f(state,c.blockcat([[U[0] + w[0]],[U[1] + w[1]]]))[3] state_n[2] = c.atan2(c.sin(state_n[2]),c.cos(state_n[2])) state_n[4] = c.atan2(c.sin(state_n[4]),c.cos(state_n[4])) state_n[5] = c.atan2(c.sin(state_n[5]),c.cos(state_n[5])) return state_n
def U_lower_bound(self, robot): ones = c.DM.ones(self.N * self.T) lb = c.blockcat([[-robot.vel_max * ones], [-robot.vel_max * ones], [-robot.ang_vel_max * ones]]) lb = c.reshape(lb, robot.nu * self.N * self.T, 1) return lb
def sqrt_correct(Rs, H, W): """ source: Fast Stable Kalman Filter Algorithms Utilising the Square Root, Steward 98 Rs: sqrt(R) H: measurement matrix W: sqrt(P) @return: Wp: sqrt(P+) = sqrt((I - KH)P) K: Kalman gain Ss: Innovation variance """ n_x = H.shape[1] n_y = H.shape[0] B = ca.sparsify(ca.blockcat(Rs, ca.mtimes(H, W), ca.SX.zeros(n_x, n_y), W)) # qr by default is upper triangular, so we transpose inputs and outputs B_Q, B_R = ca.qr(B.T) # B_Q orthogonal, B_R, lower triangular B_Q = B_Q.T B_R = B_R.T Wp = B_R[n_y:, n_y:] Ss = B_R[:n_y, :n_y] P_HT_SsInv = B_R[n_y:, :n_y] K = ca.mtimes(P_HT_SsInv, ca.inv(Ss)) return Wp, K, Ss
def state_contraints(self, robot, U): constraintVar = c.MX( 2 * (robot.nx - 1) * self.N, self.T) #skipping orientation. 2* to include min and max U = c.reshape(U, robot.nu * self.N, self.T) X = c.MX(robot.nx * self.N, self.T + 1) X[:, 0] = np.reshape(self.X0, (robot.nx * self.N, )) for i in range(self.T): for n in range(self.N): X[robot.nx * n:robot.nx * (n + 1), i + 1] = robot.kinematics( X[robot.nx * n:robot.nx * (n + 1), i], U[robot.nu * n, i], U[robot.nu * n + 1, i], U[robot.nu * n + 2, i]) constraintVar[2 * (robot.nx - 1) * n:2 * (robot.nx - 1) * (n + 1), i] = c.blockcat( [[ self.Xmax - X[robot.nx * n:robot.nx * (n + 1) - 1, i + 1] ], [ X[robot.nx * n:robot.nx * (n + 1) - 1, i + 1] - self.Xmin ]]) constraintVar = c.reshape(constraintVar, 1, 2 * (robot.nx - 1) * self.N * self.T) return constraintVar
def U_upper_bound(self, robot): ones = c.DM.ones(robot.nu, self.T) ub = c.blockcat([[robot.thrust_max * ones[0, :]], [robot.torque_max * ones[1, :]], [robot.torque_max * ones[2, :]], [robot.torque_max * ones[3, :]]]) ub = c.reshape(ub, robot.nu * self.T, 1) return ub
def dynamics_rear(X_prev, S, v): dt = 0.1 L = 2.33 X_new = X_prev + dt*casadi.blockcat([[casadi.mtimes(v/2.0,casadi.cos(X_prev[2] - S - 8*m.pi/180)+casadi.cos(X_prev[2] + (S + 8*m.pi/180)))], [casadi.mtimes(v/2.0,casadi.sin(X_prev[2] - S - 8*m.pi/180) + casadi.sin(X_prev[2] + (S + 8*m.pi/180)))], [casadi.mtimes(v,casadi.sin(-S - 8*m.pi/180)*1/L)]]) X_new[2] = casadi.atan2(casadi.sin(X_new[2]), casadi.cos(X_new[2])) return X_new
def U_lower_bound(self, robot): ones = c.DM.ones(robot.nu, self.T) lb = c.blockcat([[robot.thrust_min * ones[0, :]], [-robot.torque_max * ones[1, :]], [-robot.torque_max * ones[2, :]], [-robot.torque_max * ones[3, :]]]) lb = c.reshape(lb, robot.nu * self.T, 1) return lb
def skew_mat(xyz): ''' return the skew symmetrix matrix of a 3-vector ''' x = xyz[0] y = xyz[1] z = xyz[2] return C.blockcat([[ 0, -z, y], [ z, 0, -x], [-y, x, 0]])
def state_contraints(self, robot, U): constraintVar = c.MX( 2 * 2, self.T ) #skipping orientation & steer angle. 2* to include min and max U = c.reshape(U, robot.nu, self.T) X = c.MX(robot.nx, self.T + 1) X[:, 0] = self.X0 for i in range(self.T): X[:, i + 1] = robot.kinematics(X[:, i], U[:, i]) constraintVar[:, i] = c.blockcat([[self.Xmax - X[0:2, i + 1]], [X[0:2, i + 1] - self.Xmin]]) constraintVar = c.reshape(constraintVar, 1, 2 * 2 * self.T) return constraintVar
def kinematics(self, state, U, epsilon=0): f,_,_ = self.proc_model() w0 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[0,0])) w1 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[1,1])) w2 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[2,2])) w3 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[3,3])) w = c.DM([[w0],[w1],[w2],[w3]]) state_n = f(state,c.blockcat([[U[0] + w[0]],[U[1] + w[1]],[U[2] + w[2]],[U[3] + w[3]]])) state_n[6] = c.atan2(c.sin(state_n[6]),c.cos(state_n[6])) state_n[7] = c.atan2(c.sin(state_n[7]),c.cos(state_n[7])) state_n[8] = c.atan2(c.sin(state_n[8]),c.cos(state_n[8])) return state_n
def kinematics(self, state, vx, vy, vtheta, epsilon=0): f,_,_ = self.proc_model() #vx = vx + epsilon*self.vel_max*np.random.normal(0,1) #vy = vy + epsilon*self.vel_max*np.random.normal(0,1) #vtheta = vtheta + epsilon*self.ang_vel_max*np.random.normal(0,1) w0 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[0,0])) w1 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[1,1])) w2 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[2,2])) w = c.DM([[w0],[w1],[w2]]) #state_n = state + self.dt*c.blockcat([[vx],[vy],[vtheta]]) state_n = f(state,c.blockcat([[vx],[vy],[vtheta]])) + c.mtimes(self.G,w) state_n[2] = c.atan2(c.sin(state_n[2]),c.cos(state_n[2])) return state_n
def update(X, U): """ X contains [x,y,theta] in a column U contains [v,s] in a column """ x = X[0] y = X[1] theta = X[2] v = U[0] s = U[1] dt = 0.1 #update time L, _ = robot_params() #kinematics x_new = x + v * casadi.cos(theta) * dt y_new = y + v * casadi.sin(theta) * dt theta_new = theta + ((v * casadi.tan(s)) / L) * dt #lets make it into a casadi thing X_new = casadi.blockcat([[x_new], [y_new], [theta_new]]) return X_new
def skew( w ): x = w[0] y = w[1] z = w[2] return C.blockcat([[0,-z,y],[z,0,-x],[-y,x,0]])
custom_hess_u = W_u J = horzcat(SX.eye(2), SX(2, 2)) print(DM(J.sparsity())) # diagonal matrix with second order terms of outer loss function. D = SX.sym('D', Sparsity.diag(2)) D[0, 0] = 1 [hess_tan, grad_tan] = hessian(tanh(theta)**2, theta) D[1, 1] = if_else(theta == 0, hess_tan, grad_tan / theta) custom_hess_x = J.T @ D @ J zeros = SX(1, nx) cost_expr_ext_cost_custom_hess = blockcat(custom_hess_u, zeros, zeros.T, custom_hess_x) cost_expr_ext_cost_custom_hess_e = custom_hess_x ocp.model.cost_expr_ext_cost_custom_hess = cost_expr_ext_cost_custom_hess ocp.model.cost_expr_ext_cost_custom_hess_e = cost_expr_ext_cost_custom_hess_e # set constraints Fmax = 35 ocp.constraints.lbu = np.array([-Fmax]) ocp.constraints.ubu = np.array([+Fmax]) ocp.constraints.idxbu = np.array([0]) x0 = np.array([0.0, np.pi, 0.0, 0.0]) xf = np.array([0.0, 0.0, 0.0, 0.0]) ocp.constraints.x0 = x0 # ocp.constraints.x0 = np.array([0.0, 0.001, 0.0, 0.0])
dae['L_over_D'] = cL / cD cD = dae['cD'] + dae['cD_tether'] dae['L_over_D_with_tether'] = cL / cD ######## moment coefficients ####### # offset dae['momentCoeffs0'] = C.DMatrix([0, conf['cm0'], 0]) # with roll rates # non-dimensionalized angular velocity w_bn_b_hat = C.veccat([conf['bref'], conf['cref'], conf['bref'] ]) * 0.5 / dae['airspeed'] * w_bn_b momentCoeffs_pqr = C.mul( C.blockcat([[conf['cl_p'], conf['cl_q'], conf['cl_r']], [conf['cm_p'], conf['cm_q'], conf['cm_r']], [conf['cn_p'], conf['cn_q'], conf['cn_r']]]), w_bn_b_hat) dae['momentCoeffs_pqr'] = momentCoeffs_pqr # with alpha beta momentCoeffs_AB = C.mul( C.blockcat([[0, conf['cl_B'], conf['cl_AB']], [conf['cm_A'], 0, 0], [0, conf['cn_B'], conf['cn_AB']]]), C.vertcat([alpha, beta, alpha * beta])) dae['momentCoeffs_AB'] = momentCoeffs_AB # with control surfaces momentCoeffs_surf = C.SXMatrix(3, 1, 0) momentCoeffs_surf[0] += conf['cl_ail'] * dae['aileron'] momentCoeffs_surf[1] += conf['cm_elev'] * dae['elevator'] if 'flaps' in dae:
def compute_covariance_matrix(self): r''' This function computes the covariance matrix of the estimated parameters from the inverse of the KKT matrix for the parameter estimation problem. This allows then for statements on the quality of the values of the estimated parameters. For efficiency, only the inverse of the relevant part of the matrix is computed using the Schur complement. A more detailed description of this function will follow in future versions. ''' intro.pecas_intro() print('\n' + 20 * '-' + \ ' PECas covariance matrix computation ' + 21 * '-') print(''' Computing the covariance matrix for the estimated parameters, this might take some time ... ''') self.tstart_cov_computation = time.time() try: N1 = ca.MX(self.Vars.shape[0] - self.w.shape[0], \ self.w.shape[0]) N2 = ca.MX(self.Vars.shape[0] - self.w.shape[0], \ self.Vars.shape[0] - self.w.shape[0]) hess = ca.blockcat([ [N2, N1], [N1.T, ca.diag(self.w)], ]) # hess = hess + 1e-10 * ca.diag(self.Vars) # J2 can be re-used from parameter estimation, right? J2 = ca.jacobian(self.g, self.Vars) kkt = ca.blockcat( \ [[hess, \ J2.T], \ [J2, \ ca.MX(self.g.size1(), self.g.size1())]] \ ) B1 = kkt[:self.pesetup.np, :self.pesetup.np] E = kkt[self.pesetup.np:, :self.pesetup.np] D = kkt[self.pesetup.np:, self.pesetup.np:] Dinv = ca.solve(D, E, "csparse") F11 = B1 - ca.mul([E.T, Dinv]) self.fbeta = ca.MXFunction("fbeta", [self.Vars], [ca.mul([self.R.T, self.R]) / \ (self.yN.size + self.g.size1() - self.Vars.size())]) [self.beta] = self.fbeta([self.Varshat]) self.fcovp = ca.MXFunction("fcovp", [self.Vars], \ [self.beta * ca.solve(F11, ca.MX.eye(F11.size1()))]) [self.Covp] = self.fcovp([self.Varshat]) print( \ '''Covariance matrix computation finished, run show_results() to visualize.''') except AttributeError as err: errmsg = ''' You must execute run_parameter_estimation() first before the covariance matrix for the estimated parameters can be computed. ''' raise AttributeError(errmsg) finally: self.tend_cov_computation = time.time() self.duration_cov_computation = self.tend_cov_computation - \ self.tstart_cov_computation
def skew(a, b, c): " creates skew-symmetric matrix" d = ca.blockcat([[0., -c, b], [c, 0., -a], [-b, a, 0.]]) return d
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', 'r', 'dr', 'ddr', 'aileron', 'elevator', 'rudder', 'flaps' ]) dae.addU( [ 'daileron' , 'delevator' , 'drudder' , 'dflaps' , 'dddr' ] ) dae.addP( ['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'] # 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_n2b'] = C.blockcat([[dae['e11'], dae['e12'], dae['e13']], [dae['e21'], dae['e22'], dae['e23']], [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'])) # 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.blockcat(\ [[dae.ddt(name) for name in ['e11', 'e12', 'e13']], [dae.ddt(name) for name in ['e21', 'e22', 'e23']], [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('r') - dae['dr'], dae.ddt('dr') - dae['ddr'], dae.ddt('ddr') - dae['dddr'], dae.ddt('aileron') - dae['daileron'], dae.ddt('elevator') - dae['delevator'], dae.ddt('rudder') - dae['drudder'], dae.ddt('flaps') - dae['dflaps'] ]) # 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 dae['neg_electrical_winch_power'] = -dae['electrical_winch_power'] dae['neg_mechanical_winch_power'] = -dae['mechanical_winch_power'] 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
obj = [x_end - ydata[0, :].T] for k in range(int(N)): x_end = rk4(x0=x_end, p=ca.vertcat(udata[k], EPS_U[k], P))["xf"] obj.append(x_end - ydata_noise[k + 1, :].T) r = ca.vertcat(ca.vertcat(*obj), EPS_U) wv = (1.0 / sigma_y**2) * pl.ones(ydata.shape) weps_u = (1.0 / sigma_u**2) * pl.ones(udata.shape) Sigma_y_inv = ca.diag(ca.vec(wv)) Sigma_u_inv = ca.diag(weps_u) Sigma = ca.blockcat(Sigma_y_inv, ca.DM(pl.zeros((Sigma_y_inv.shape[0], Sigma_u_inv.shape[1]))),\ ca.DM(pl.zeros((Sigma_u_inv.shape[0], Sigma_y_inv.shape[1]))), Sigma_u_inv) nlp = {"x": V, "f": ca.mtimes([r.T, Sigma, r])} nlpsolver = ca.nlpsol("nlpsolver", "ipopt", nlp) V0 = ca.vertcat( pl.ones(3), \ pl.zeros(N), \ ydata_noise[0,:].T ) sol = nlpsolver(x0=V0)
def setupModel(dae, conf): ''' take the dae that has x/z/u/p added to it already and return the states added to it and return mass matrix and rhs of the dae residual mass matrix columns: ddt(ddelta dx dy dz w_bn_b_x w_bn_b_y w_bn_b_z) nu rhs: forces/torques acting on : ddt(ddelta dx dy dz w_bn_b_x w_bn_b_y w_bn_b_z) nu ''' # Parameters m = conf['mass'] g = conf['g'] j1 = conf['j1'] j31 = conf['j31'] j2 = conf['j2'] j3 = conf['j3'] jCarousel = conf['jCarousel'] cfric = conf['cfric'] zt = conf['zt'] rA = conf['rArm'] # Frames # ========================================== # World frame (n) NED North-East-Down # # Wind carrying frame (w) # - Translates along the world frame's x axis with the wind speed # # Carousel frame (c) # - Origin coincides with the world origin # - Makes and angle delta # # Carousel tip frame (a) # - Origin sits at tip of arm # - e_x extends radially outwards # - e_z points downwards # # Body frame (b) # - attached to body of aircraft # - e_x extends to the nose # - e_z points downwards # # Vector naming convention # ========================= # v_ab_c # Velocity of point a, differentiated in frame b, expressed in frame c (Greg terminlogy) # Velocity of point a w.r.t. frame b, expressed in frame c (Joris terminology) # States # Components that make up the rotation matrix from carousel frame to body frame R_c2b [-] # e11 e12 e13 # e21 e22 e23 # e31 e32 e33 e11 = dae['e11'] e12 = dae['e12'] e13 = dae['e13'] e21 = dae['e21'] e22 = dae['e22'] e23 = dae['e23'] e31 = dae['e31'] e32 = dae['e32'] e33 = dae['e33'] x = dae['x'] # Aircraft position in carousel tip frame coordinates [m] y = dae['y'] z = dae['z'] dx = dae['dx'] # Time derivatives of x [m/s] dy = dae['dy'] dz = dae['dz'] w1 = dae['w_bn_b_x'] # Body angular rate w.r.t world frame, expressed in body coordinates [rad/s^2] w2 = dae['w_bn_b_y'] w3 = dae['w_bn_b_z'] ddelta = dae['ddelta'] # Carousel turning speed [rad/s] r = dae['r'] dr = dae['dr'] ddr = dae['ddr'] tc = dae['motor_torque'] #Carousel motor torque if 'xt' in conf: t_xt = dae['tether_tension'] * conf['xt'] else: t_xt = 0.0 R_g2c = C.blockcat([[dae['cos_delta'], -dae['sin_delta'], 0.0], [dae['sin_delta'], dae['cos_delta'], 0.0], [0.0, 0.0, 1.0]]) # wind model def getWind(): if 'wind_model' not in conf: return C.veccat([0, 0, 0]) if conf['wind_model']['name'] == 'wind_shear': # use a logarithmic wind shear model # wind(z) = w0 * log((altitude+zt)/zt) / log(z0/zt) # where w0 is wind at z0 altitude # zt is surface roughness characteristic length # altitude is -z - altitude0, where altitude0 is a user parameter z0 = conf['wind_model']['z0'] zt_roughness = conf['wind_model']['zt_roughness'] altitude = -z - conf['wind_model']['altitude0'] wind = dae['w0']*C.log((altitude+zt_roughness)/zt_roughness)/C.log(z0/zt_roughness) dae['wind_at_altitude'] = wind return C.mul([R_g2c, C.veccat([wind, 0, 0])]) elif conf['wind_model']['name'] in ['constant','hardcoded']: # constant wind dae['wind_at_altitude'] = dae['w0'] return C.mul([R_g2c, C.veccat([dae['w0'], 0, 0])]) elif conf['wind_model']['name'] == 'random_walk': dae.addU('delta_wind_x') dae.addU('delta_wind_y') dae.addU('delta_wind_z') wind_x = dae.addX('wind_x') wind_y = dae.addX('wind_y') wind_z = dae.addX('wind_z') dae['wind_at_altitude'] = wind_x return C.veccat([wind_x, wind_y, wind_z]) wind = getWind() # Velocity of aircraft w.r.t wind carrying frame, expressed in carousel frame v_bw_c = C.veccat( [ dx - ddelta*y , dy + ddelta*(rA + x) , dz ]) - wind # Velocity of aircraft w.r.t wind carrying frame, expressed in body frame # (needed to compute the aero forces and torques !) v_bw_b = C.mul( dae['R_c2b'], v_bw_c ) (f1, f2, f3, t1, t2, t3) = aeroForcesTorques(dae, conf, v_bw_c, v_bw_b, dae['w_bn_b'], (dae['e21'], dae['e22'], dae['e23']) # y-axis of body frame in carousel coordinates ) # f1..f3 expressed in carrousel coordinates # t1..t3 expressed in body coordinates # 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) 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 == 'random_walk': if _which[ 0 ]: dae.addU('df1_disturbance') f1 += dae['rho_sref_v2'] * dae.addX('f1_disturbance') if _which[ 1 ]: dae.addU('df2_disturbance') f2 += dae['rho_sref_v2'] * dae.addX('f2_disturbance') if _which[ 2 ]: dae.addU('df3_disturbance') f3 += dae['rho_sref_v2'] * dae.addX('f3_disturbance') elif _type == 'parameter': if _which[ 0 ]: f1 += dae['rho_sref_v2'] * dae.addX('f1_disturbance') if _which[ 1 ]: f2 += dae['rho_sref_v2'] * dae.addX('f2_disturbance') if _which[ 2 ]: f3 += dae['rho_sref_v2'] * dae.addX('f3_disturbance') elif _type == 'online_data': if _which[ 0 ]: f1 += dae['rho_sref_v2'] * dae.addP('f1_disturbance') if _which[ 1 ]: f2 += dae['rho_sref_v2'] * dae.addP('f2_disturbance') if _which[ 2 ]: f3 += dae['rho_sref_v2'] * dae.addP('f3_disturbance') else: raise ValueError("WTF?") 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 ]: dae.addU('dt1_disturbance') t1 += dae['rho_sref_v2'] * conf['bref'] * dae.addX('t1_disturbance') if _which[ 1 ]: dae.addU('dt2_disturbance') t2 += dae['rho_sref_v2'] * conf['cref'] * dae.addX('t2_disturbance') if _which[ 2 ]: dae.addU('dt3_disturbance') t3 += dae['rho_sref_v2'] * conf['bref'] * dae.addX('t3_disturbance') elif _type == 'parameter': if _which[ 0 ]: t1 += dae['rho_sref_v2'] * conf['bref'] * dae.addX('t1_disturbance') if _which[ 1 ]: t2 += dae['rho_sref_v2'] * conf['cref'] * dae.addX('t2_disturbance') if _which[ 2 ]: t3 += dae['rho_sref_v2'] * conf['bref'] * dae.addX('t3_disturbance') elif _type == 'online_data': if _which[ 0 ]: t1 += dae['rho_sref_v2'] * conf['bref'] * dae.addP('t1_disturbance') if _which[ 1 ]: t2 += dae['rho_sref_v2'] * conf['cref'] * dae.addP('t2_disturbance') if _which[ 2 ]: t3 += dae['rho_sref_v2'] * conf['bref'] * dae.addP('t3_disturbance') else: raise ValueError("WTF?") # mass matrix mm = C.SX.zeros(8, 8) mm[0,0] = jCarousel + m*rA*rA + m*x*x + m*y*y + 2*m*rA*x mm[0,1] = -m*y mm[0,2] = m*(rA + x) mm[0,3] = 0 mm[0,4] = 0 mm[0,5] = 0 mm[0,6] = 0 mm[0,7] = 0 mm[1,0] = -m*y mm[1,1] = m mm[1,2] = 0 mm[1,3] = 0 mm[1,4] = 0 mm[1,5] = 0 mm[1,6] = 0 mm[1,7] = x + zt*e31 mm[2,0] = m*(rA + x) mm[2,1] = 0 mm[2,2] = m mm[2,3] = 0 mm[2,4] = 0 mm[2,5] = 0 mm[2,6] = 0 mm[2,7] = y + zt*e32 mm[3,0] = 0 mm[3,1] = 0 mm[3,2] = 0 mm[3,3] = m mm[3,4] = 0 mm[3,5] = 0 mm[3,6] = 0 mm[3,7] = z + zt*e33 mm[4,0] = 0 mm[4,1] = 0 mm[4,2] = 0 mm[4,3] = 0 mm[4,4] = j1 mm[4,5] = 0 mm[4,6] = j31 mm[4,7] = -zt*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33) mm[5,0] = 0 mm[5,1] = 0 mm[5,2] = 0 mm[5,3] = 0 mm[5,4] = 0 mm[5,5] = j2 mm[5,6] = 0 mm[5,7] = zt*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) mm[6,0] = 0 mm[6,1] = 0 mm[6,2] = 0 mm[6,3] = 0 mm[6,4] = j31 mm[6,5] = 0 mm[6,6] = j3 mm[6,7] = 0 mm[7,0] = -zt*(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) mm[7,1] = x + zt*e31 mm[7,2] = y + zt*e32 mm[7,3] = z + zt*e33 mm[7,4] = -zt*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33) mm[7,5] = zt*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) mm[7,6] = 0 mm[7,7] = 0 # right hand side zt2 = zt*zt rhs = C.veccat( [ tc - cfric*ddelta - f1*y + f2*(rA + x) + dy*m*(dx - 2*ddelta*y) - dx*m*(dy + 2*ddelta*rA + 2*ddelta*x) , 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) + t_xt , 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.SX.zeros(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 # The cable constraint 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['ddx'] # ddy = dae['ddy'] # ddz = dae['ddz'] # dw1 = dae['dw1'] # dw2 = dae['dw2'] # dddelta = dae['dddelta'] ddx = dae.ddt('dx') ddy = dae.ddt('dy') ddz = dae.ddt('dz') dw1 = dae.ddt('w_bn_b_x') dw2 = dae.ddt('w_bn_b_y') dddelta = dae.ddt('ddelta') 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['c'] = c dae['cdot'] = cdot dae['cddot'] = cddot return (mm, rhs, dRexp)
def blockcat(a, b, c, d): return ca.blockcat(a, b, c, d)
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']+'"') if "parametrizeInputsAsOnlineData" in conf and conf[ "parametrizeInputsAsOnlineData" ] is True: dae.addP( [ "daileron", "delevator", "dmotor_torque", 'dddr' ] ) else: 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
x_end = X0 obj = [x_end - ydata_noise[0,:].T] for k in range(N): x_end = rk4(x0 = x_end, p = ca.vertcat(udata[k], EPS_U[k], P))["xf"] obj.append(x_end - ydata_noise[k+1, :].T) r = ca.vertcat(ca.vertcat(*obj), EPS_U) Sigma_y_inv = ca.diag(ca.vec(wv)) Sigma_u_inv = ca.diag(weps_u) Z = ca.DM(pl.zeros((Sigma_y_inv.shape[0], Sigma_u_inv.shape[1]))) Sigma = ca.blockcat(Sigma_y_inv, Z, Z.T, Sigma_u_inv) nlp = {"x": V, "f": 0.5 * ca.mtimes([r.T, Sigma, r])} nlpsolver = ca.nlpsol("nlpsolver", "ipopt", nlp) V0 = ca.vertcat( pl.ones(3), \ pl.zeros(N), \ ydata[0,:].T ) sol = nlpsolver(x0 = V0)
dae['cD_tether'] = 0.25*dae['r']*0.001/sref dae['L_over_D'] = cL/cD cD = dae['cD'] + dae['cD_tether'] dae['L_over_D_with_tether'] = cL/cD ######## moment coefficients ####### # offset dae['momentCoeffs0'] = C.DMatrix([0, conf['cm0'], 0]) # with roll rates # non-dimensionalized angular velocity w_bn_b_hat = C.veccat([conf['bref'],conf['cref'],conf['bref']])*0.5/dae['airspeed']*w_bn_b momentCoeffs_pqr = C.mul(C.blockcat([[conf['cl_p'], conf['cl_q'], conf['cl_r']], [conf['cm_p'], conf['cm_q'], conf['cm_r']], [conf['cn_p'], conf['cn_q'], conf['cn_r']]]), w_bn_b_hat) dae['momentCoeffs_pqr'] = momentCoeffs_pqr # with alpha beta momentCoeffs_AB = C.mul(C.blockcat([[ 0, conf['cl_B'], conf['cl_AB']], [conf['cm_A'], 0, 0], [ 0, conf['cn_B'], conf['cn_AB']]]), C.vertcat([alpha, beta, alpha*beta])) dae['momentCoeffs_AB'] = momentCoeffs_AB # with control surfaces momentCoeffs_surf = C.SXMatrix(3, 1, 0) momentCoeffs_surf[0] += conf['cl_ail']*dae['aileron'] momentCoeffs_surf[1] += conf['cm_elev']*dae['elevator']
obj = [x_end - ydata[0,:].T] for k in range(int(N)): x_end = rk4(x0 = x_end, p = ca.vertcat([udata[k], EPS_U[k], P]))["xf"] obj.append(x_end - ydata_noise[k+1, :].T) r = ca.vertcat([ca.vertcat(obj), EPS_U]) wv = (1.0 / sigma_y**2) * pl.ones(ydata.shape) weps_u = (1.0 / sigma_u**2) * pl.ones(udata.shape) Sigma_y_inv = ca.diag(ca.vec(wv)) Sigma_u_inv = ca.diag(weps_u) Sigma = ca.blockcat(Sigma_y_inv, ca.DMatrix(pl.zeros((Sigma_y_inv.shape[0], Sigma_u_inv.shape[1]))),\ ca.DMatrix(pl.zeros((Sigma_u_inv.shape[0], Sigma_y_inv.shape[1]))), Sigma_u_inv) nlp = ca.MXFunction("nlp", ca.nlpIn(x = V), ca.nlpOut(f = ca.mul([r.T, Sigma, r]))) nlpsolver = ca.NlpSolver("nlpsolver", "ipopt", nlp) V0 = ca.vertcat([ pl.ones(3), \ pl.zeros(N), \ ydata_noise[0,:].T ]) sol = nlpsolver(x0 = V0)
def setupModel(dae, conf): ''' take the dae that has x/z/u/p added to it already and return the states added to it and return mass matrix and rhs of the dae residual mass matrix columns: ddt(ddelta dx dy dz w_bn_b_x w_bn_b_y w_bn_b_z) nu rhs: forces/torques acting on : ddt(ddelta dx dy dz w_bn_b_x w_bn_b_y w_bn_b_z) nu ''' # Parameters m = conf['mass'] g = conf['g'] j1 = conf['j1'] j31 = conf['j31'] j2 = conf['j2'] j3 = conf['j3'] jCarousel = conf['jCarousel'] cfric = conf['cfric'] zt = conf['zt'] rA = conf['rArm'] # Frames # ========================================== # World frame (n) NED North-East-Down # # Wind carrying frame (w) # - Translates along the world frame's x axis with the wind speed # # Carousel frame (c) # - Origin coincides with the world origin # - Makes and angle delta # # Carousel tip frame (a) # - Origin sits at tip of arm # - e_x extends radially outwards # - e_z points downwards # # Body frame (b) # - attached to body of aircraft # - e_x extends to the nose # - e_z points downwards # # Vector naming convention # ========================= # v_ab_c # Velocity of point a, differentiated in frame b, expressed in frame c (Greg terminlogy) # Velocity of point a w.r.t. frame b, expressed in frame c (Joris terminology) # States # Components that make up the rotation matrix from carousel frame to body frame R_c2b [-] # e11 e12 e13 # e21 e22 e23 # e31 e32 e33 e11 = dae['e11'] e12 = dae['e12'] e13 = dae['e13'] e21 = dae['e21'] e22 = dae['e22'] e23 = dae['e23'] e31 = dae['e31'] e32 = dae['e32'] e33 = dae['e33'] x = dae['x'] # Aircraft position in carousel tip frame coordinates [m] y = dae['y'] z = dae['z'] dx = dae['dx'] # Time derivatives of x [m/s] dy = dae['dy'] dz = dae['dz'] w1 = dae['w_bn_b_x'] # Body angular rate w.r.t world frame, expressed in body coordinates [rad/s^2] w2 = dae['w_bn_b_y'] w3 = dae['w_bn_b_z'] ddelta = dae['ddelta'] # Carousel turning speed [rad/s] r = dae['r'] dr = dae['dr'] ddr = dae['ddr'] tc = dae['motor_torque'] #Carousel motor torque if 'xt' in conf: t_xt = dae['tether_tension'] * conf['xt'] else: t_xt = 0.0 R_g2c = C.blockcat([[dae['cos_delta'], -dae['sin_delta'], 0.0], [dae['sin_delta'], dae['cos_delta'], 0.0], [0.0, 0.0, 1.0]]) # wind model def getWind(): if 'wind_model' not in conf: return C.veccat([0, 0, 0]) if conf['wind_model']['name'] == 'wind_shear': # use a logarithmic wind shear model # wind(z) = w0 * log((altitude+zt)/zt) / log(z0/zt) # where w0 is wind at z0 altitude # zt is surface roughness characteristic length # altitude is -z - altitude0, where altitude0 is a user parameter z0 = conf['wind_model']['z0'] zt_roughness = conf['wind_model']['zt_roughness'] altitude = -z - conf['wind_model']['altitude0'] wind = dae['w0']*C.log((altitude+zt_roughness)/zt_roughness)/C.log(z0/zt_roughness) dae['wind_at_altitude'] = wind return C.mul([R_g2c, C.veccat([wind, 0, 0])]) elif conf['wind_model']['name'] in ['constant','hardcoded']: # constant wind dae['wind_at_altitude'] = dae['w0'] return C.mul([R_g2c, C.veccat([dae['w0'], 0, 0])]) elif conf['wind_model']['name'] == 'random_walk': dae.addU('delta_wind_x') dae.addU('delta_wind_y') dae.addU('delta_wind_z') wind_x = dae.addX('wind_x') wind_y = dae.addX('wind_y') wind_z = dae.addX('wind_z') dae['wind_at_altitude'] = wind_x return C.veccat([wind_x, wind_y, wind_z]) wind = getWind() # Velocity of aircraft w.r.t wind carrying frame, expressed in carousel frame v_bw_c = C.veccat( [ dx - ddelta*y , dy + ddelta*(rA + x) , dz ]) - wind # Velocity of aircraft w.r.t wind carrying frame, expressed in body frame # (needed to compute the aero forces and torques !) v_bw_b = C.mul( dae['R_c2b'], v_bw_c ) (f1, f2, f3, t1, t2, t3) = aeroForcesTorques(dae, conf, v_bw_c, v_bw_b, dae['w_bn_b'], (dae['e21'], dae['e22'], dae['e23']) # y-axis of body frame in carousel coordinates ) # f1..f3 expressed in carrousel coordinates # t1..t3 expressed in body coordinates # 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) fx_dist = 0 fy_dist = 0 fz_dist = 0 mx_dist = 0 my_dist = 0 mz_dist = 0 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 == 'random_walk': if _which[ 0 ]: dae.addU('df1_disturbance') fx_dist += dae['rho_sref_v2'] * dae.addX('f1_disturbance') if _which[ 1 ]: dae.addU('df2_disturbance') fy_dist += dae['rho_sref_v2'] * dae.addX('f2_disturbance') if _which[ 2 ]: dae.addU('df3_disturbance') fz_dist += dae['rho_sref_v2'] * dae.addX('f3_disturbance') elif _type == 'parameter': if _which[ 0 ]: fx_dist += dae['rho_sref_v2'] * dae.addX('f1_disturbance') if _which[ 1 ]: fy_dist += dae['rho_sref_v2'] * dae.addX('f2_disturbance') if _which[ 2 ]: fz_dist += dae['rho_sref_v2'] * dae.addX('f3_disturbance') elif _type == 'online_data': if _which[ 0 ]: fx_dist += dae['rho_sref_v2'] * dae.addP('f1_disturbance') if _which[ 1 ]: fy_dist += dae['rho_sref_v2'] * dae.addP('f2_disturbance') if _which[ 2 ]: fz_dist += dae['rho_sref_v2'] * dae.addP('f3_disturbance') else: raise ValueError("WTF?") f1 += fx_dist f2 += fy_dist f3 += fz_dist dae["aero_fx_dist"] = fx_dist dae["aero_fy_dist"] = fy_dist dae["aero_fz_dist"] = fz_dist 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 ]: dae.addU('dt1_disturbance') mx_dist += dae['rho_sref_v2'] * conf['bref'] * dae.addX('t1_disturbance') if _which[ 1 ]: dae.addU('dt2_disturbance') my_dist += dae['rho_sref_v2'] * conf['cref'] * dae.addX('t2_disturbance') if _which[ 2 ]: dae.addU('dt3_disturbance') mz_dist += dae['rho_sref_v2'] * conf['bref'] * dae.addX('t3_disturbance') elif _type == 'parameter': if _which[ 0 ]: mx_dist += dae['rho_sref_v2'] * conf['bref'] * dae.addX('t1_disturbance') if _which[ 1 ]: my_dist += dae['rho_sref_v2'] * conf['cref'] * dae.addX('t2_disturbance') if _which[ 2 ]: mz_dist += dae['rho_sref_v2'] * conf['bref'] * dae.addX('t3_disturbance') elif _type == 'online_data': if _which[ 0 ]: mx_dist += dae['rho_sref_v2'] * conf['bref'] * dae.addP('t1_disturbance') if _which[ 1 ]: my_dist += dae['rho_sref_v2'] * conf['cref'] * dae.addP('t2_disturbance') if _which[ 2 ]: mz_dist += dae['rho_sref_v2'] * conf['bref'] * dae.addP('t3_disturbance') else: raise ValueError("WTF?") t1 += mx_dist t2 += my_dist t3 += mz_dist dae["aero_mx_dist"] = mx_dist dae["aero_my_dist"] = my_dist dae["aero_mz_dist"] = mz_dist # mass matrix mm = C.SXMatrix(8, 8) mm[0,0] = jCarousel + m*rA*rA + m*x*x + m*y*y + 2*m*rA*x mm[0,1] = -m*y mm[0,2] = m*(rA + x) mm[0,3] = 0 mm[0,4] = 0 mm[0,5] = 0 mm[0,6] = 0 mm[0,7] = 0 mm[1,0] = -m*y mm[1,1] = m mm[1,2] = 0 mm[1,3] = 0 mm[1,4] = 0 mm[1,5] = 0 mm[1,6] = 0 mm[1,7] = x + zt*e31 mm[2,0] = m*(rA + x) mm[2,1] = 0 mm[2,2] = m mm[2,3] = 0 mm[2,4] = 0 mm[2,5] = 0 mm[2,6] = 0 mm[2,7] = y + zt*e32 mm[3,0] = 0 mm[3,1] = 0 mm[3,2] = 0 mm[3,3] = m mm[3,4] = 0 mm[3,5] = 0 mm[3,6] = 0 mm[3,7] = z + zt*e33 mm[4,0] = 0 mm[4,1] = 0 mm[4,2] = 0 mm[4,3] = 0 mm[4,4] = j1 mm[4,5] = 0 mm[4,6] = j31 mm[4,7] = -zt*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33) mm[5,0] = 0 mm[5,1] = 0 mm[5,2] = 0 mm[5,3] = 0 mm[5,4] = 0 mm[5,5] = j2 mm[5,6] = 0 mm[5,7] = zt*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) mm[6,0] = 0 mm[6,1] = 0 mm[6,2] = 0 mm[6,3] = 0 mm[6,4] = j31 mm[6,5] = 0 mm[6,6] = j3 mm[6,7] = 0 mm[7,0] = -zt*(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) mm[7,1] = x + zt*e31 mm[7,2] = y + zt*e32 mm[7,3] = z + zt*e33 mm[7,4] = -zt*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33) mm[7,5] = zt*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) mm[7,6] = 0 mm[7,7] = 0 # right hand side zt2 = zt*zt rhs = C.veccat( [ tc - cfric*ddelta - f1*y + f2*(rA + x) + dy*m*(dx - 2*ddelta*y) - dx*m*(dy + 2*ddelta*rA + 2*ddelta*x) , 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) + t_xt , 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 # The cable constraint 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['ddx'] # ddy = dae['ddy'] # ddz = dae['ddz'] # dw1 = dae['dw1'] # dw2 = dae['dw2'] # dddelta = dae['dddelta'] ddx = dae.ddt('dx') ddy = dae.ddt('dy') ddz = dae.ddt('dz') dw1 = dae.ddt('w_bn_b_x') dw2 = dae.ddt('w_bn_b_y') dddelta = dae.ddt('ddelta') 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['c'] = c dae['cdot'] = cdot dae['cddot'] = cddot return (mm, rhs, dRexp)
def __init__(self, horizon, model, gp=None, Q=None, P=None, R=None, S=None, lam=None, lam_state=None, ulb=None, uub=None, xlb=None, xub=None, terminal_constraint=None, feedback=True, percentile=None, gp_method='TA', costFunc='quad', solver_opts=None, discrete_method='gp', inequality_constraints=None, num_con_par=0, hybrid=None, Bd=None, Bf=None): """ Initialize and build the MPC solver # Arguments: horizon: Prediction horizon with control inputs model: System model # Optional Argumants: gp: GP model Q: State penalty matrix, default=diag(1,...,1) P: Termial penalty matrix, default=diag(1,...,1) if feedback is True, then P is the solution of the DARE, discarding this option. R: Input penalty matrix, default=diag(1,...,1)*0.01 S: Input rate of change penalty matrix, default=diag(1,...,1)*0.1 lam: Slack variable penalty for constraints, defalt=1000 lam_state: Slack variable penalty for violation of upper/lower state boundy, defalt=None ulb: Lower boundry input uub: Upper boundry input xlb: Lower boundry state xub: Upper boundry state terminal_constraint: Terminal condition on the state * if None: No terminal constraint is used * if zero: Terminal state is equal to zero * if nonzero: Terminal state is bounded within +/- the constraint * if not None and feedback is True, then the expected value of the Lyapunov function E{x^TPx} < terminal_constraint is used as a terminal constraint. feedback: If true, use an LQR feedback function u= Kx + v percentile: Measure how far from the contrain that is allowed, P(X in constrained set) > percentile, percentile= 1 - probability of violation, default=0.95 gp_method: Method of propagating the uncertainty Possible options: 'TA': Second order Taylor approximation 'ME': Mean equivalent approximation costFunc: Cost function to use in the objective 'quad': Expected valaue of Quadratic Cost 'sat': Expected value of Saturating cost solver_opts: Additional options to pass to the NLP solver e.g.: solver_opts['print_time'] = False solver_opts['ipopt.tol'] = 1e-8 discrete_method: 'gp' - Gaussian process model 'rk4' - Runga-Kutta 4 Integrator 'exact' - CVODES or IDEAS (for ODEs or DEAs) 'hybrid' - GP model for dynamic equations, and RK4 for kinematic equations 'd_hybrid' - Same as above, without uncertainty 'f_hybrid' - GP estimating modelling errors, with RK4 computing the the actual model num_con_par: Number of parameters to pass to the inequality function inequality_constraints: Additional inequality constraints Use a function with inputs (x, covar, u, eps) and that returns a dictionary with inequality constraints and limits. e.g. cons = dict(con_ineq=con_ineq_array, con_ineq_lb=con_ineq_lb_array, con_ineq_ub=con_ineq_ub_array ) # NOTES: * Differentiation of Sundails integrators is not supported with SX graph, meaning that the solver option 'extend_graph' must be set to False to use MX graph instead when using the 'exact' discrete method. * At the moment the f_hybrid option is not finished implemented... """ build_solver_time = -time.time() dt = model.sampling_time() Ny, Nu, Np = model.size() Nx = Nu + Ny Nt = int(horizon / dt) self.__dt = dt self.__Nt = Nt self.__Ny = Ny self.__Nx = Nx self.__Nu = Nu self.__num_con_par = num_con_par self.__model = model self.__hybrid = hybrid self.__gp = gp self.__feedback = feedback self.__discrete_method = discrete_method """ Default penalty values """ if P is None: P = np.eye(Ny) if Q is None: Q = np.eye(Ny) if R is None: R = np.eye(Nu) * 0.01 if S is None: S = np.eye(Nu) * 0.1 if lam is None: lam = 1000 self.__Q = Q self.__P = P self.__R = R self.__S = S self.__Bd = Bd self.__Bf = Bf if xub is None: xub = np.full((Ny), np.inf) if xlb is None: xlb = np.full((Ny), -np.inf) if uub is None: uub = np.full((Nu), np.inf) if ulb is None: ulb = np.full((Nu), -np.inf) """ Default percentile probability """ if percentile is None: percentile = 0.95 quantile_x = np.ones(Ny) * norm.ppf(percentile) quantile_u = np.ones(Nu) * norm.ppf(percentile) Hx = ca.MX.eye(Ny) Hu = ca.MX.eye(Nu) """ Create parameter symbols """ mean_0_s = ca.MX.sym('mean_0', Ny) mean_ref_s = ca.MX.sym('mean_ref', Ny) u_0_s = ca.MX.sym('u_0', Nu) covariance_0_s = ca.MX.sym('covariance_0', Ny * Ny) K_s = ca.MX.sym('K', Nu * Ny) P_s = ca.MX.sym('P', Ny * Ny) con_par = ca.MX.sym('con_par', num_con_par) param_s = ca.vertcat(mean_0_s, mean_ref_s, covariance_0_s, u_0_s, K_s, P_s, con_par) """ Select wich GP function to use """ if discrete_method is 'gp': self.__gp.set_method(gp_method) #TODO:Fix if solver_opts['expand'] is not False and discrete_method is 'exact': raise TypeError( "Can't use exact discrete system with expanded graph") """ Initialize state variance with the GP noise variance """ if gp is not None: #TODO: Cannot use gp variance with hybrid model self.__variance_0 = np.full((Ny), 1e-10) #gp.noise_variance() else: self.__variance_0 = np.full((Ny), 1e-10) """ Define which cost function to use """ self.__set_cost_function(costFunc, mean_ref_s, P_s.reshape((Ny, Ny))) """ Feedback function """ mean_s = ca.MX.sym('mean', Ny) v_s = ca.MX.sym('v', Nu) if feedback: u_func = ca.Function( 'u', [mean_s, mean_ref_s, v_s, K_s], [v_s + ca.mtimes(K_s.reshape((Nu, Ny)), mean_s - mean_ref_s)]) else: u_func = ca.Function('u', [mean_s, mean_ref_s, v_s, K_s], [v_s]) self.__u_func = u_func """ Create variables struct """ var = ctools.struct_symMX([( ctools.entry('mean', shape=(Ny, ), repeat=Nt + 1), ctools.entry('L', shape=(int((Ny**2 - Ny) / 2 + Ny), ), repeat=Nt + 1), ctools.entry('v', shape=(Nu, ), repeat=Nt), ctools.entry('eps', shape=(3, ), repeat=Nt + 1), ctools.entry('eps_state', shape=(Ny, ), repeat=Nt + 1), )]) num_slack = 3 #TODO: Make this a little more dynamic... num_state_slack = Ny self.__var = var self.__num_var = var.size # Decision variable boundries self.__varlb = var(-np.inf) self.__varub = var(np.inf) """ Adjust hard boundries """ for t in range(Nt + 1): j = Ny k = 0 for i in range(Ny): # Lower boundry of diagonal self.__varlb['L', t, k] = 0 k += j j -= 1 self.__varlb['eps', t] = 0 self.__varlb['eps_state', t] = 0 if xub is not None: self.__varub['mean', t] = xub if xlb is not None: self.__varlb['mean', t] = xlb if lam_state is None: self.__varub['eps_state'] = 0 """ Input covariance matrix """ if discrete_method is 'hybrid': N_gp, Ny_gp, Nu_gp = self.__gp.get_size() Nz_gp = Ny_gp + Nu_gp covar_d_sx = ca.SX.sym('cov_d', Ny_gp, Ny_gp) K_sx = ca.SX.sym('K', Nu, Ny) covar_u_func = ca.Function( 'cov_u', [covar_d_sx, K_sx], # [K_sx @ covar_d_sx @ K_sx.T]) [ca.SX(Nu, Nu)]) covar_s = ca.SX(Nz_gp, Nz_gp) covar_s[:Ny_gp, :Ny_gp] = covar_d_sx # covar_s = ca.blockcat(covar_x_s, cov_xu, cov_xu.T, cov_u) covar_func = ca.Function('covar', [covar_d_sx], [covar_s]) elif discrete_method is 'f_hybrid': #TODO: Fix this... N_gp, Ny_gp, Nu_gp = self.__gp.get_size() Nz_gp = Ny_gp + Nu_gp # covar_x_s = ca.MX.sym('covar_x', Ny_gp, Ny_gp) covar_d_sx = ca.SX.sym('cov_d', Ny_gp, Ny_gp) K_sx = ca.SX.sym('K', Nu, Ny) # covar_u_func = ca.Function( 'cov_u', [covar_d_sx, K_sx], # [K_sx @ covar_d_sx @ K_sx.T]) [ca.SX(Nu, Nu)]) # cov_xu_func = ca.Function('cov_xu', [covar_x_sx, K_sx], # [covar_x_sx @ K_sx.T]) # cov_xu = cov_xu_func(covar_x_s, K_s.reshape((Nu, Ny))) # cov_u = covar_u_func(covar_x_s, K_s.reshape((Nu, Ny))) covar_s = ca.SX(Nz_gp, Nz_gp) covar_s[:Ny_gp, :Ny_gp] = covar_d_sx # covar_s = ca.blockcat(covar_x_s, cov_xu, cov_xu.T, cov_u) covar_func = ca.Function('covar', [covar_d_sx], [covar_s]) else: covar_x_s = ca.MX.sym('covar_x', Ny, Ny) covar_x_sx = ca.SX.sym('cov_x', Ny, Ny) K_sx = ca.SX.sym('K', Nu, Ny) covar_u_func = ca.Function('cov_u', [covar_x_sx, K_sx], [K_sx @ covar_x_sx @ K_sx.T]) cov_xu_func = ca.Function('cov_xu', [covar_x_sx, K_sx], [covar_x_sx @ K_sx.T]) cov_xu = cov_xu_func(covar_x_s, K_s.reshape((Nu, Ny))) cov_u = covar_u_func(covar_x_s, K_s.reshape((Nu, Ny))) covar_s = ca.blockcat(covar_x_s, cov_xu, cov_xu.T, cov_u) covar_func = ca.Function('covar', [covar_x_s], [covar_s]) """ Hybrid output covariance matrix """ if discrete_method is 'hybrid': N_gp, Ny_gp, Nu_gp = self.__gp.get_size() covar_d_sx = ca.SX.sym('covar_d', Ny_gp, Ny_gp) covar_x_sx = ca.SX.sym('covar_x', Ny, Ny) u_s = ca.SX.sym('u', Nu) cov_x_next_s = ca.SX(Ny, Ny) cov_x_next_s[:Ny_gp, :Ny_gp] = covar_d_sx #TODO: Missing kinematic states covar_x_next_func = ca.Function( 'cov', #[mean_s, u_s, covar_d_sx, covar_x_sx], [covar_d_sx], [cov_x_next_s]) """ f_hybrid output covariance matrix """ elif discrete_method is 'f_hybrid': N_gp, Ny_gp, Nu_gp = self.__gp.get_size() # Nz_gp = Ny_gp + Nu_gp covar_d_sx = ca.SX.sym('covar_d', Ny_gp, Ny_gp) covar_x_sx = ca.SX.sym('covar_x', Ny, Ny) # L_x = ca.SX.sym('L', ca.Sparsity.lower(Ny)) # L_d = ca.SX.sym('L', ca.Sparsity.lower(3)) mean_s = ca.SX.sym('mean', Ny) u_s = ca.SX.sym('u', Nu) # A_f = hybrid.rk4_jacobian_x(mean_s[Ny_gp:], mean_s[:Ny_gp]) # B_f = hybrid.rk4_jacobian_u(mean_s[Ny_gp:], mean_s[:Ny_gp]) # C = ca.horzcat(A_f, B_f) # cov = ca.blocksplit(covar_x_s, Ny_gp, Ny_gp) # cov[-1][-1] = covar_d_sx # cov_i = ca.blockcat(cov) # cov_f = C @ cov_i @ C.T # cov[0][0] = cov_f cov_x_next_s = ca.SX(Ny, Ny) cov_x_next_s[:Ny_gp, :Ny_gp] = covar_d_sx # cov_x_next_s[Ny_gp:, Ny_gp:] = #TODO: Pre-solve the GP jacobian using the initial condition in the solve iteration # jac_mean = ca.SX(Ny_gp, Ny) # jac_mean = self.__gp.jacobian(mean_s[:Ny_gp], u_s, 0) # A = ca.horzcat(jac_f, Bd) # jac = Bf @ jac_f @ Bf.T + Bd @ jac_mean @ Bd.T # temp = jac_mean @ covar_x_s # temp = jac_mean @ L_s # cov_i = ca.SX(Ny + 3, Ny + 3) # cov_i[:Ny,:Ny] = covar_x_s # cov_i[Ny:, Ny:] = covar_d_s # cov_i[Ny:, :Ny] = temp # cov_i[:Ny, Ny:] = temp.T #TODO: This is just a new TA implementation... CLEAN UP... covar_x_next_func = ca.Function( 'cov', [mean_s, u_s, covar_d_sx, covar_x_sx], #TODO: Clean up # [A @ cov_i @ A.T]) # [Bd @ covar_d_s @ Bd.T + jac @ covar_x_s @ jac.T]) # [ca.blockcat(cov)]) [cov_x_next_s]) # Cholesky factorization of covariance function # S_x_next_func = ca.Function( 'S_x', [mean_s, u_s, covar_d_s, covar_x_s], # [Bd @ covar_d_s + jac @ covar_x_s]) L_s = ca.SX.sym('L', ca.Sparsity.lower(Ny)) L_to_cov_func = ca.Function('cov', [L_s], [L_s @ L_s.T]) covar_x_sx = ca.SX.sym('cov_x', Ny, Ny) cholesky = ca.Function('cholesky', [covar_x_sx], [ca.chol(covar_x_sx).T]) """ Set initial values """ obj = ca.MX(0) con_eq = [] con_ineq = [] con_ineq_lb = [] con_ineq_ub = [] con_eq.append(var['mean', 0] - mean_0_s) L_0_s = ca.MX(ca.Sparsity.lower(Ny), var['L', 0]) L_init = cholesky(covariance_0_s.reshape((Ny, Ny))) con_eq.append(L_0_s.nz[:] - L_init.nz[:]) u_past = u_0_s """ Build constraints """ for t in range(Nt): # Input to GP mean_t = var['mean', t] u_t = u_func(mean_t, mean_ref_s, var['v', t], K_s) L_x = ca.MX(ca.Sparsity.lower(Ny), var['L', t]) covar_x_t = L_to_cov_func(L_x) if discrete_method is 'hybrid': N_gp, Ny_gp, Nu_gp = self.__gp.get_size() covar_t = covar_func(covar_x_t[:Ny_gp, :Ny_gp]) elif discrete_method is 'd_hybrid': N_gp, Ny_gp, Nu_gp = self.__gp.get_size() covar_t = ca.MX(Ny_gp + Nu_gp, Ny_gp + Nu_gp) elif discrete_method is 'gp': covar_t = covar_func(covar_x_t) else: covar_t = ca.MX(Nx, Nx) """ Select the chosen integrator """ if discrete_method is 'rk4': mean_next_pred = model.rk4(mean_t, u_t, []) covar_x_next_pred = ca.MX(Ny, Ny) elif discrete_method is 'exact': mean_next_pred = model.Integrator(x0=mean_t, p=u_t)['xf'] covar_x_next_pred = ca.MX(Ny, Ny) elif discrete_method is 'd_hybrid': # Deterministic hybrid GP model N_gp, Ny_gp, Nu_gp = self.__gp.get_size() mean_d, covar_d = self.__gp.predict(mean_t[:Ny_gp], u_t, covar_t) mean_next_pred = ca.vertcat( mean_d, hybrid.rk4(mean_t[Ny_gp:], mean_t[:Ny_gp], [])) covar_x_next_pred = ca.MX(Ny, Ny) elif discrete_method is 'hybrid': # Hybrid GP model N_gp, Ny_gp, Nu_gp = self.__gp.get_size() mean_d, covar_d = self.__gp.predict(mean_t[:Ny_gp], u_t, covar_t) mean_next_pred = ca.vertcat( mean_d, hybrid.rk4(mean_t[Ny_gp:], mean_t[:Ny_gp], [])) #covar_x_next_pred = covar_x_next_func(mean_t, u_t, covar_d, # covar_x_t) covar_x_next_pred = covar_x_next_func(covar_d) elif discrete_method is 'f_hybrid': #TODO: Hybrid GP model estimating model error N_gp, Ny_gp, Nu_gp = self.__gp.get_size() mean_d, covar_d = self.__gp.predict(mean_t[:Ny_gp], u_t, covar_t) mean_next_pred = ca.vertcat( mean_d, hybrid.rk4(mean_t[Ny_gp:], mean_t[:Ny_gp], [])) covar_x_next_pred = covar_x_next_func(mean_t, u_t, covar_d, covar_x_t) else: # Use GP as default mean_next_pred, covar_x_next_pred = self.__gp.predict( mean_t, u_t, covar_t) """ Continuity constraints """ mean_next = var['mean', t + 1] con_eq.append(mean_next_pred - mean_next) L_x_next = ca.MX(ca.Sparsity.lower(Ny), var['L', t + 1]) covar_x_next = L_to_cov_func(L_x_next).reshape((Ny * Ny, 1)) L_x_next_pred = cholesky(covar_x_next_pred) con_eq.append(L_x_next_pred.nz[:] - L_x_next.nz[:]) """ Chance state constraints """ cons = self.__constraint(mean_next, L_x_next, Hx, quantile_x, xub, xlb, var['eps_state', t]) con_ineq.extend(cons['con']) con_ineq_lb.extend(cons['con_lb']) con_ineq_ub.extend(cons['con_ub']) """ Input constraints """ # cov_u = covar_u_func(covar_x_t, K_s.reshape((Nu, Ny))) cov_u = ca.MX(Nu, Nu) # cons = self.__constraint(u_t, cov_u, Hu, quantile_u, uub, ulb) # con_ineq.extend(cons['con']) # con_ineq_lb.extend(cons['con_lb']) # con_ineq_ub.extend(cons['con_ub']) if uub is not None: con_ineq.append(u_t) con_ineq_ub.extend(uub) con_ineq_lb.append(np.full((Nu, ), -ca.inf)) if ulb is not None: con_ineq.append(u_t) con_ineq_ub.append(np.full((Nu, ), ca.inf)) con_ineq_lb.append(ulb) """ Add extra constraints """ if inequality_constraints is not None: cons = inequality_constraints(var['mean', t + 1], covar_x_next, u_t, var['eps', t], con_par) con_ineq.extend(cons['con_ineq']) con_ineq_lb.extend(cons['con_ineq_lb']) con_ineq_ub.extend(cons['con_ineq_ub']) """ Objective function """ u_delta = u_t - u_past obj += self.__l_func(var['mean', t], covar_x_t, u_t, cov_u, u_delta) \ + np.full((1, num_slack),lam) @ var['eps', t] if lam_state is not None: obj += np.full( (1, num_state_slack), lam_state) @ var['eps_state', t] u_t = u_past L_x = ca.MX(ca.Sparsity.lower(Ny), var['L', Nt]) covar_x_t = L_to_cov_func(L_x) obj += self.__lf_func(var['mean', Nt], covar_x_t, P_s.reshape((Ny, Ny))) \ + np.full((1, num_slack),lam) @ var['eps', Nt] if lam_state is not None: obj += np.full( (1, num_state_slack), lam_state) @ var['eps_state', Nt] num_eq_con = ca.vertcat(*con_eq).size1() num_ineq_con = ca.vertcat(*con_ineq).size1() con_eq_lb = np.zeros((num_eq_con, )) con_eq_ub = np.zeros((num_eq_con, )) """ Terminal contraint """ if terminal_constraint is not None and not feedback: con_ineq.append(var['mean', Nt] - mean_ref_s) num_ineq_con += Ny con_ineq_lb.append(np.full((Ny, ), -terminal_constraint)) con_ineq_ub.append(np.full((Ny, ), terminal_constraint)) elif terminal_constraint is not None and feedback: con_ineq.append( self.__lf_func(var['mean', Nt], covar_x_t, P_s.reshape( (Ny, Ny)))) num_ineq_con += 1 con_ineq_lb.append(0) con_ineq_ub.append(terminal_constraint) con = ca.vertcat(*con_eq, *con_ineq) self.__conlb = ca.vertcat(con_eq_lb, *con_ineq_lb) self.__conub = ca.vertcat(con_eq_ub, *con_ineq_ub) """ Build solver object """ nlp = dict(x=var, f=obj, g=con, p=param_s) options = { 'ipopt.print_level': 0, 'ipopt.mu_init': 0.01, 'ipopt.tol': 1e-8, 'ipopt.warm_start_init_point': 'yes', 'ipopt.warm_start_bound_push': 1e-9, 'ipopt.warm_start_bound_frac': 1e-9, 'ipopt.warm_start_slack_bound_frac': 1e-9, 'ipopt.warm_start_slack_bound_push': 1e-9, 'ipopt.warm_start_mult_bound_push': 1e-9, 'ipopt.mu_strategy': 'adaptive', 'print_time': False, 'verbose': False, 'expand': True } if solver_opts is not None: options.update(solver_opts) self.__solver = ca.nlpsol('mpc_solver', 'ipopt', nlp, options) # First prediction used in the NLP, used in plot later self.__var_prediction = np.zeros((Nt + 1, Ny)) self.__mean_prediction = np.zeros((Nt + 1, Ny)) self.__mean = None build_solver_time += time.time() print('\n________________________________________') print('# Time to build mpc solver: %f sec' % build_solver_time) print('# Number of variables: %d' % self.__num_var) print('# Number of equality constraints: %d' % num_eq_con) print('# Number of inequality constraints: %d' % num_ineq_con) print('----------------------------------------')
cD = dae["cD"] + dae["cD_tether"] dae["L_over_D_with_tether"] = cL / cD ######## moment coefficients ####### # offset dae["momentCoeffs0"] = C.DMatrix([0, conf["cm0"], 0]) # with roll rates # non-dimensionalized angular velocity w_bn_b_hat = C.veccat([conf["bref"], conf["cref"], conf["bref"]]) * 0.5 / dae["airspeed"] * w_bn_b momentCoeffs_pqr = C.mul( C.blockcat( [ [conf["cl_p"], conf["cl_q"], conf["cl_r"]], [conf["cm_p"], conf["cm_q"], conf["cm_r"]], [conf["cn_p"], conf["cn_q"], conf["cn_r"]], ] ), w_bn_b_hat, ) dae["momentCoeffs_pqr"] = momentCoeffs_pqr # with alpha beta momentCoeffs_AB = C.mul( C.blockcat([[0, conf["cl_B"], conf["cl_AB"]], [conf["cm_A"], 0, 0], [0, conf["cn_B"], conf["cn_AB"]]]), C.vertcat([alpha, beta, alpha * beta]), ) dae["momentCoeffs_AB"] = momentCoeffs_AB # with control surfaces
x_end = X0 obj = [x_end - ydata_noise[0,:].T] for k in range(N): x_end = rk4(x0 = x_end, p = ca.vertcat([udata[k], EPS_U[k], P]))["xf"] obj.append(x_end - ydata_noise[k+1, :].T) r = ca.vertcat([ca.vertcat(obj), EPS_U]) Sigma_y_inv = ca.diag(ca.vec(wv)) Sigma_u_inv = ca.diag(weps_u) Z = ca.DMatrix(pl.zeros((Sigma_y_inv.shape[0], Sigma_u_inv.shape[1]))) Sigma = ca.blockcat(Sigma_y_inv, Z, Z.T, Sigma_u_inv) nlp = ca.MXFunction("nlp", ca.nlpIn(x = V), \ ca.nlpOut(f = 0.5 * ca.mul([r.T, Sigma, r]))) nlpsolver = ca.NlpSolver("nlpsolver", "ipopt", nlp) V0 = ca.vertcat([ pl.ones(3), \ pl.zeros(N), \ ydata[0,:].T ]) sol = nlpsolver(x0 = V0)
def compute_covariance_matrix(self): r''' This function computes the covariance matrix of the estimated parameters from the inverse of the KKT matrix for the parameter estimation problem. This allows then for statements on the quality of the values of the estimated parameters. For efficiency, only the inverse of the relevant part of the matrix is computed using the Schur complement. A more detailed description of this function will follow in future versions. ''' intro.pecas_intro() print('\n' + 20 * '-' + \ ' PECas covariance matrix computation ' + 21 * '-') print(''' Computing the covariance matrix for the estimated parameters, this might take some time ... ''') self.tstart_cov_computation = time.time() try: N1 = ca.MX(self.Vars.shape[0] - self.w.shape[0], \ self.w.shape[0]) N2 = ca.MX(self.Vars.shape[0] - self.w.shape[0], \ self.Vars.shape[0] - self.w.shape[0]) hess = ca.blockcat([[N2, N1], [N1.T, ca.diag(self.w)],]) # hess = hess + 1e-10 * ca.diag(self.Vars) # J2 can be re-used from parameter estimation, right? J2 = ca.jacobian(self.g, self.Vars) kkt = ca.blockcat( \ [[hess, \ J2.T], \ [J2, \ ca.MX(self.g.size1(), self.g.size1())]] \ ) B1 = kkt[:self.pesetup.np, :self.pesetup.np] E = kkt[self.pesetup.np:, :self.pesetup.np] D = kkt[self.pesetup.np:, self.pesetup.np:] Dinv = ca.solve(D, E, "csparse") F11 = B1 - ca.mul([E.T, Dinv]) self.fbeta = ca.MXFunction("fbeta", [self.Vars], [ca.mul([self.R.T, self.R]) / \ (self.yN.size + self.g.size1() - self.Vars.size())]) [self.beta] = self.fbeta([self.Varshat]) self.fcovp = ca.MXFunction("fcovp", [self.Vars], \ [self.beta * ca.solve(F11, ca.MX.eye(F11.size1()))]) [self.Covp] = self.fcovp([self.Varshat]) print( \ '''Covariance matrix computation finished, run show_results() to visualize.''') except AttributeError as err: errmsg = ''' You must execute run_parameter_estimation() first before the covariance matrix for the estimated parameters can be computed. ''' raise AttributeError(errmsg) finally: self.tend_cov_computation = time.time() self.duration_cov_computation = self.tend_cov_computation - \ self.tstart_cov_computation
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 tr(x, y, z): return c.blockcat([[1, 0, 0, x], [0, 1, 0, y], [0, 0, 1, z], [0, 0, 0, 1]])