def ext_belief_dynamics(ext_belief, control, F, dt, A_fcn, C_fcn, Q, obs_cov_fcn): ext_belief_next = cat.struct_SX(ext_belief) # Predict the mean [mu_bar] = F([ ext_belief['m'], control, dt ]) # Compute linearization [A,_] = A_fcn([ ext_belief['m'], control, dt ]) [C,_] = C_fcn([ mu_bar ]) # Predict the covariance S_bar = ca.mul([ A, ext_belief['S'], A.T ]) + Q # Get the observations noise covariance [R] = obs_cov_fcn([ mu_bar ]) # Compute the inverse P = ca.mul([ C, S_bar, C.T ]) + R P_inv = ca.inv(P) # Kalman gain K = ca.mul([ S_bar, C.T, P_inv ]) # Update equations nx = ext_belief['m'].size() ext_belief_next['m'] = mu_bar ext_belief_next['S'] = ca.mul(ca.DMatrix.eye(nx) - ca.mul(K,C), S_bar) ext_belief_next['L'] = ca.mul([ A, ext_belief['L'], A.T ]) + \ ca.mul([ K, C, S_bar ]) # (ext_belief, control) -> next_ext_belief return ca.SXFunction('Extended-belief dynamics', [ext_belief,control],[ext_belief_next])
def _create_EBF(self): """Extended belief dynamics""" eb_next = cat.struct_SX(self.eb) # Compute the mean [mu_bar] = self.F([self.eb['m'], self.u]) # Compute linearization [A, _] = self.Fj_x([self.eb['m'], self.u]) [C, _] = self.hj_x([mu_bar]) # Get system and observation noises, as if the state was mu_bar [M] = self.M([self.eb['m'], self.u]) [N] = self.N([mu_bar]) # Predict the covariance S_bar = ca.mul([A, self.eb['S'], A.T]) + M # Compute the inverse P = ca.mul([C, S_bar, C.T]) + N P_inv = ca.inv(P) # Kalman gain K = ca.mul([S_bar, C.T, P_inv]) # Update equations eb_next['m'] = mu_bar eb_next['S'] = ca.mul(ca.DMatrix.eye(self.nx) - ca.mul(K, C), S_bar) eb_next['L'] = ca.mul([A, self.eb['L'], A.T]) + ca.mul([K, C, S_bar]) # (eb, u) -> eb_next op = {'input_scheme': ['eb', 'u'], 'output_scheme': ['eb_next']} return ca.SXFunction('Extended belief dynamics', [self.eb, self.u], [eb_next], op)
def belief_dynamics(belief, control, # current (b, u) F, dt, A_fcn, C_fcn, # dynamics Q, obs_cov_fcn): # covariances Q and R(x) belief_next = cat.struct_SX(belief) # Predict the mean [mu_bar] = F([ belief['m'], control, dt ]) # Compute linearization [A,_] = A_fcn([ belief['m'], control, dt ]) [C,_] = C_fcn([ mu_bar ]) # Predict the covariance S_bar = ca.mul([ A, belief['S'], A.T ]) + Q # Get the observations noise covariance [R] = obs_cov_fcn([ mu_bar ]) # Compute the inverse P = ca.mul([ C, S_bar, C.T ]) + R P_inv = ca.inv(P) # Kalman gain K = ca.mul([ S_bar, C.T, P_inv ]) # Update equations nx = belief['m'].size() belief_next['m'] = mu_bar belief_next['S'] = ca.mul(ca.DMatrix.eye(nx) - ca.mul(K,C), S_bar) # (belief, control) -> next_belief return ca.SXFunction('Belief dynamics',[belief,control],[belief_next])
def test_SX(self): self.message("SX unary operations") x=SX.sym("x",3,2) x0=array([[0.738,0.2],[ 0.1,0.39 ],[0.99,0.999999]]) self.numpyEvaluationCheckPool(self.pool,[x],x0,name="SX") x=SX.sym("x",3,3) x0=array([[0.738,0.2,0.3],[ 0.1,0.39,-6 ],[0.99,0.999999,-12]]) #self.numpyEvaluationCheck(lambda x: c.det(x[0]), lambda x: linalg.det(x),[x],x0,name="det(SX)") self.numpyEvaluationCheck(lambda x: SX(c.det(x[0])), lambda x: linalg.det(x),[x],x0,name="det(SX)") self.numpyEvaluationCheck(lambda x: c.inv(x[0]), lambda x: linalg.inv(x),[x],x0,name="inv(SX)")
def _create_EKF(self): """Extended Kalman filter""" b_next = cat.struct_SX(self.b) # Compute the mean [mu_bar] = self.F([self.b['m'], self.u]) # Compute linearization [A, _] = self.Fj_x([self.b['m'], self.u]) [C, _] = self.hj_x([mu_bar]) # Get system and observation noises [M] = self.M([self.b['m'], self.u]) [N] = self.N([mu_bar]) # Predict the covariance S_bar = ca.mul([A, self.b['S'], A.T]) + M # Compute the inverse P = ca.mul([C, S_bar, C.T]) + N P_inv = ca.inv(P) # Kalman gain K = ca.mul([S_bar, C.T, P_inv]) # Predict observation [z_bar] = self.h([mu_bar]) # Update equations b_next['m'] = mu_bar + ca.mul([K, self.z - z_bar]) b_next['S'] = ca.mul(ca.DMatrix.eye(self.nx) - ca.mul(K, C), S_bar) # (b, u, z) -> b_next op = {'input_scheme': ['b', 'u', 'z'], 'output_scheme': ['b_next']} return ca.SXFunction('Extended Kalman filter', [self.b, self.u, self.z], [b_next], op)
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 test_inv(self): self.message("Matrix inverse") a = DM([[1,2],[1,3]]) self.checkarray(mtimes(c.inv(a),a),eye(2),"DM inverse")
# print 'Fu:' # print Fu # print 'Vxx:' # print Vxx # Check if Quu is positive definite try: np.linalg.cholesky(Quu) except np.linalg.LinAlgError as e: mu_flag = True print 'Quu:' print Quu break # Save gains iQuu = ca.inv(Quu) k_all[i] = -ca.mul(iQuu, Qu) K_all[i] = -ca.mul(iQuu, Qux) # Compute expansion of V-function # Vx = Qx - ca.mul([ Qux.T, iQuu, Qu ]) # Vxx = Qxx - ca.mul([ Qux.T, iQuu, Qux ]) Vx = Qx + ca.mul([ K_all[i].T, Quu, k_all[i] ]) + \ ca.mul(K_all[i].T, Qu) + ca.mul(Qux.T, k_all[i]) Vxx = Qxx + ca.mul([ K_all[i].T, Quu, K_all[i] ]) + \ ca.mul(K_all[i].T, Qux) + ca.mul(Qux.T, K_all[i]) # Manage regularization if mu_flag: # Increase mu d_mu = max(d_mu0, d_mu*d_mu0)
def rocket_equations(jit=True): x = ca.SX.sym('x', 14) u = ca.SX.sym('u', 4) p = ca.SX.sym('p', 16) t = ca.SX.sym('t') dt = ca.SX.sym('dt') # State: x # body frame: Forward, Right, Down omega_b = x[0:3] # inertial angular velocity expressed in body frame r_nb = x[3:7] # modified rodrigues parameters v_b = x[7:10] # inertial velocity expressed in body components p_n = x[10:13] # positon in nav frame m_fuel = x[13] # mass # Input: u m_dot = ca.if_else(m_fuel > 0, u[0], 0) fin = u_to_fin(u) # Parameters: p g = p[0] # gravity Jx = p[1] # moment of inertia Jy = p[2] Jz = p[3] Jxz = p[4] ve = p[5] l_fin = p[6] w_fin = p[7] CL_alpha = p[8] CL0 = p[9] CD0 = p[10] K = p[11] s_fin = p[12] rho = p[13] m_empty = p[14] l_motor = p[15] # Calculations m = m_empty + m_fuel J_b = ca.SX.zeros(3, 3) J_b[0, 0] = Jx + m_fuel * l_motor**2 J_b[1, 1] = Jy + m_fuel * l_motor**2 J_b[2, 2] = Jz J_b[0, 2] = J_b[2, 0] = Jxz C_nb = so3.Dcm.from_mrp(r_nb) g_n = ca.vertcat(0, 0, g) v_n = ca.mtimes(C_nb, v_b) # aerodynamics VT = ca.norm_2(v_b) q = 0.5 * rho * ca.dot(v_b, v_b) fins = { 'top': { 'fwd': [1, 0, 0], 'up': [0, 1, 0], 'angle': fin[0] }, 'left': { 'fwd': [1, 0, 0], 'up': [0, 0, -1], 'angle': fin[1] }, 'down': { 'fwd': [1, 0, 0], 'up': [0, -1, 0], 'angle': fin[2] }, 'right': { 'fwd': [1, 0, 0], 'up': [0, 0, 1], 'angle': fin[3] }, } rel_wind_dir = v_b / VT # build fin lift/drag forces vel_tol = 1e-3 FA_b = ca.vertcat(0, 0, 0) MA_b = ca.vertcat(0, 0, 0) for key, data in fins.items(): fwd = data['fwd'] up = data['up'] angle = data['angle'] U = ca.dot(fwd, v_b) W = ca.dot(up, v_b) side = ca.cross(fwd, up) alpha = ca.if_else(ca.fabs(U) > vel_tol, -ca.atan(W / U), 0) perp_wind_dir = ca.cross(side, rel_wind_dir) norm_perp = ca.norm_2(perp_wind_dir) perp_wind_dir = ca.if_else( ca.fabs(norm_perp) > vel_tol, perp_wind_dir / norm_perp, up) CL = CL0 + CL_alpha * (alpha + angle) CD = CD0 + K * (CL - CL0)**2 # model stall as no lift if above 23 deg. L = ca.if_else(ca.fabs(alpha) < 0.4, CL * q * s_fin, 0) D = CD * q * s_fin FAi_b = L * perp_wind_dir - D * rel_wind_dir FA_b += FAi_b MA_b += ca.cross(-l_fin * fwd - w_fin * side, FAi_b) FA_b = ca.if_else(ca.fabs(VT) > vel_tol, FA_b, ca.SX.zeros(3)) MA_b = ca.if_else(ca.fabs(VT) > vel_tol, MA_b, ca.SX.zeros(3)) # propulsion FP_b = ca.vertcat(m_dot * ve, 0, 0) # force and momental total F_b = FA_b + FP_b + ca.mtimes(C_nb.T, m * g_n) M_b = MA_b force_moment = ca.Function('force_moment', [x, u, p], [F_b, M_b], ['x', 'u', 'p'], ['F_b', 'M_b']) # right hand side rhs = ca.Function('rhs', [x, u, p], [ ca.vertcat( ca.mtimes(ca.inv(J_b), M_b - ca.cross(omega_b, ca.mtimes(J_b, omega_b))), so3.Mrp.kinematics(r_nb, omega_b), F_b / m - ca.cross(omega_b, v_b), ca.mtimes(C_nb, v_b), -m_dot) ], ['x', 'u', 'p'], ['rhs'], {'jit': jit}) # prediction t0 = ca.SX.sym('t0') h = ca.SX.sym('h') x0 = ca.SX.sym('x', 14) x1 = rk4(lambda t, x: rhs(x, u, p), t0, x0, h) x1[3:7] = so3.Mrp.shadow_if_necessary(x1[3:7]) predict = ca.Function('predict', [x0, u, p, t0, h], [x1], {'jit': jit}) def schedule(t, start, ty_pairs): val = start for ti, yi in ty_pairs: val = ca.if_else(t > ti, yi, val) return val # reference trajectory pitch_d = 1.0 euler = so3.Euler.from_mrp(r_nb) # roll, pitch, yaw pitch = euler[1] # control u_control = ca.SX.zeros(4) # these controls are just test controls to make sure the fins are working u_control[0] = 0.1 # mass flow rate u_control[1] = 0 u_control[2] = (pitch - 1) u_control[3] = 0 control = ca.Function('control', [x, p, t, dt], [u_control], ['x', 'p', 't', 'dt'], ['u']) # initialize pitch_deg = ca.SX.sym('pitch_deg') omega0_b = ca.vertcat(0, 0, 0) r0_nb = so3.Mrp.from_euler(ca.vertcat(0, pitch_deg * ca.pi / 180, 0)) v0_b = ca.vertcat(0, 0, 0) p0_n = ca.vertcat(0, 0, 0) m0_fuel = 0.8 # x: omega_b, r_nb, v_b, p_n, m_fuel x0 = ca.vertcat(omega0_b, r0_nb, v0_b, p0_n, m0_fuel) # g, Jx, Jy, Jz, Jxz, ve, l_fin, w_fin, CL_alpha, CL0, CD0, K, s, rho, m_emptpy, l_motor p0 = [ 9.8, 0.05, 1.0, 1.0, 0.0, 350, 1.0, 0.05, 2 * np.pi, 0, 0.01, 0.01, 0.05, 1.225, 0.2, 1.0 ] initialize = ca.Function('initialize', [pitch_deg], [x0, p0]) return { 'rhs': rhs, 'predict': predict, 'control': control, 'initialize': initialize, 'force_moment': force_moment, 'x': x, 'u': u, 'p': p } return rhs, x, u, p