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 test_inv(self):
self.message("Matrix inverse")
a = DM([[1,2],[1,3]])
self.checkarray(mtimes(c.inv(a),a),eye(2),"DM inverse")
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
