Added a simple Python simulation of the model.

lateral_track_analytic.py

@@ -4,95 +4,131 @@
 
 # build the bicycle state space
 b = {}
-delta, deltaDot, phiDot, phi, psi, yP, yQ =\
-    symbols('delta, deltaDot, phiDot, phi, psi, yP, yQ')
-tPhi, tDelta, fB, tDeltaDot = symbols('tPhi, tDelta, fB, tDeltaDot')
-b['x'] = Matrix([[yP], [psi], [phi], [delta], [phiDot], [deltaDot]])
-b['u'] = Matrix([[tPhi], [tDelta], [fB]])
-b['y'] = Matrix([[psi], [phi], [delta], [phiDot], [yQ]])
-
-numStates = len(b['x'])
-numInputs = len(b['u'])
-numOutputs = len(b['y'])
+delta, deltadot, phidot, phi, psi, yP, yq = symbols(
+    'delta, deltadot, phidot, phi, psi, yP, yq')
+tphi, tdelta, fB, tdeltadot = symbols('tphi, tdelta, fB, tdeltadot')
+b['x'] = Matrix([
+    [phi],
+    [delta],
+    [phidot],
+    [deltadot],
+    [psi],
+    [yP],
+])
+b['u'] = Matrix([
+    [tphi],
+    [tdelta],
+    [fB],
+])
+b['y'] = Matrix([
+    [phi],
+    [delta],
+    [phidot],
+    [psi],
+    [yq],
+])
+
+num_states = len(b['x'])
+num_inputs = len(b['u'])
+num_outputs = len(b['y'])
 
 # Make the A and B matrices generic as they are just essentially submatrices in
 # the closed loop system, but the C matrix plays a role in controller equations
 # because we are tracking the lateral deviation of the front wheel contact. In
 # this case, set the appropriate entries that are ones and zeros so that the
 # final equatios will be in a reasonably understandable form.
-b['A'] = Matrix(numStates, numStates,
-        lambda i, j: Symbol('a' + str(i + 1) + str(j + 1)))
-
-b['B'] = Matrix(numStates, numInputs,
-        lambda i, j: Symbol('b' + str(i + 1) + str(j + 1)))
-
-#b['C'] = Matrix(numOutputs, numStates,
-        #lambda i, j: Symbol('c' + str(i + 1) + str(j + 1)))
-
-b['C'] = zeros((numOutputs, numStates))
-b['C'][0, 1] = 1 # psi
-b['C'][1, 2] = 1 # phi
-b['C'][2, 3] = 1 # delta
-b['C'][3, 4] = 1 # phiDot
-# yQ depends on the rear wheel location, the heading angle and the steer angle
-b['C'][4, 0] = 1
-c52, c54 = symbols('c52 c54')
-b['C'][4, 1] = c52
-b['C'][4, 3] = c54
-
-b['D'] = Matrix(numOutputs, numInputs, lambda i, j: 0.)
+b['A'] = zeros(num_states, num_states)
+b['A'][0, 2] = 1
+b['A'][1, 3] = 1
+b['A'][2, 0] = Symbol('a_phidd_phi')
+b['A'][2, 1] = Symbol('a_phidd_del')
+b['A'][2, 2] = Symbol('a_phidd_phid')
+b['A'][2, 3] = Symbol('a_phidd_deld')
+b['A'][3, 0] = Symbol('a_deldd_phi')
+b['A'][3, 1] = Symbol('a_deldd_del')
+b['A'][3, 2] = Symbol('a_deldd_phid')
+b['A'][3, 3] = Symbol('a_deldd_deld')
+b['A'][4, 1] = Symbol('a_psid_del')
+b['A'][4, 3] = Symbol('a_psid_deld')
+b['A'][5, 4] = Symbol('a_ypd_psi')
+
+b['B'] = zeros(num_states, num_inputs)
+b['B'][2, 1] = Symbol('b_phidd_Tdel')
+b['B'][2, 2] = Symbol('b_phidd_F')
+b['B'][3, 1] = Symbol('b_deldd_Tdel')
+b['B'][3, 2] = Symbol('b_deldd_F')
+
+b['C'] = zeros(num_outputs, num_states)
+b['C'][0, 0] = 1  # phi
+b['C'][1, 1] = 1  # delta
+b['C'][2, 2] = 1  # phidot
+b['C'][3, 4] = 1  # psi
+# yq depends on the rear wheel location, the heading angle and the steer angle
+b['C'][4, 1] = Symbol('c_yq_del')
+b['C'][4, 4] = Symbol('c_yq_psi')
+b['C'][4, 5] = 1
+
+b['D'] = Matrix(num_outputs, num_inputs, lambda i, j: 0.)
 
 # Write the controller output as a function of the gains and the commanded
 # lateral deviation.
-kDelta, kPhiDot, kPhi, kPsi, kYQ = symbols('kDelta, kPhiDot, kPhi, kPsi, kYQ')
-deltac, phiDotc, phic, psic, yc = symbols('deltac, phiDotc, phic, psic, yc')
-psic = kYQ * (yc - yQ)
-phic = kPsi * (psic - psi)
-phiDotc = kPhi * (phic - phi)
-deltac = kPhiDot * (phiDotc - phiDot)
-Unm = kDelta * (deltac - delta)
+kdelta, kphidot, kphi, kpsi, kyq = symbols('kdelta, kphidot, kphi, kpsi, kyq')
+deltac, phidotc, phic, psic, yc = symbols('deltac, phidotc, phic, psic, yc')
+psic = kyq*(yc - yq)
+phic = kpsi*(psic - psi)
+phidotc = kphi*(phic - phi)
+deltac = kphidot*(phidotc - phidot)
+Unm = kdelta*(deltac - delta)
 Unm = Unm.expand()
-controller = Matrix([Unm.coeff(psi), Unm.coeff(phi), Unm.coeff(delta),
-    Unm.coeff(phiDot), Unm.coeff(yQ), Unm.coeff(yc)]).T
+controller = Matrix([
+    Unm.coeff(phi),
+    Unm.coeff(delta),
+    Unm.coeff(phidot),
+    Unm.coeff(psi),
+    Unm.coeff(yq),
+    Unm.coeff(yc),
+]).T
 
 # neuromuscular state space
-omega = Symbol('w')
+omega = Symbol('omega')
 zeta = Symbol('zeta')
 n = {}
 n['A'] = Matrix([[0, 1],
-                 [-omega**2, -2 * zeta * omega]])
+                 [-omega**2, -2*zeta*omega]])
 n['B'] = Matrix([[0],
                  [omega**2]])
 n['C'] = Matrix([1, 0]).T
 n['D'] = Matrix([0])
-n['x'] = Matrix([[tDelta],
-                 [tDeltaDot]])
+n['x'] = Matrix([[tdelta],
+                 [tdeltadot]])
 n['u'] = Matrix([Unm])
-n['y'] = Matrix([tDelta])
+n['y'] = Matrix([tdelta])
 
 # Compute the differential equations for the closed loop system.
-xDot = b['A'] * b['x'] + b['B'] * b['u']
-xDot = xDot.col_join(n['A'] * n['x'] + n['B'] * controller * (b['C'] *
-        b['x']).col_join(Matrix([yc])))
+xdot = b['A']*b['x'] + b['B']*b['u']
+xdot = xdot.col_join(
+    n['A']*n['x'] + n['B']*controller*(b['C']*b['x']).col_join(
+        Matrix([yc])))
+y = (b['C']*b['x']).col_join(n['C']*n['x'])
 
 # Build the closed loop state space matrices.
-systemStates = [yP, psi, phi, delta, phiDot, deltaDot, tDelta, tDeltaDot]
-systemInputs = [tPhi, fB, yc]
-systemOutputs = [psi, phi, delta, phiDot, yQ, tDelta]
+system_states = [phi, delta, phidot, deltadot, psi, yP, tdelta, tdeltadot]
+system_inputs = [fB, yc]
+system_outputs = [phi, delta, phidot, psi, yq, tdelta]
+
 
 def mat_coeff(equations, variables, i, j):
     c = equations[i].expand().coeff(variables[j])
     if c is None:
         c = 0
     return c
 
-A = Matrix(len(systemStates), len(systemStates), lambda i, j: mat_coeff(xDot,
-    systemStates, i, j))
 
-B = Matrix(len(systemStates), len(systemInputs), lambda i, j: mat_coeff(xDot,
-    systemInputs, i, j))
+A = Matrix(len(system_states), len(system_states),
+           lambda i, j: mat_coeff(xdot, system_states, i, j))
 
-y = (b['C'] * b['x']).col_join(n['C'] * n['x'])
+B = Matrix(len(system_states), len(system_inputs),
+           lambda i, j: mat_coeff(xdot, system_inputs, i, j))
 
-C = Matrix(len(systemOutputs), len(systemStates), lambda i, j: mat_coeff(y,
-    systemStates, i, j))
+C = Matrix(len(system_outputs), len(system_states),
+           lambda i, j: mat_coeff(y, system_states, i, j))

simulate_controlled_system.py

@@ -0,0 +1,77 @@
+import sympy as sm
+import numpy as np
+from scipy.signal import lsim, lti
+import matplotlib.pyplot as plt
+
+from lateral_track_analytic import (A, B, C, system_outputs)
+
+args = list(sm.ordered(list(A.free_symbols | B.free_symbols | C.free_symbols)))
+
+eval_sys = sm.lambdify(args, (A, B, C))
+
+# q'' = Minv(-(g*K0 + v**2*K2)*q - v*C1*q' + T + H*F))
+# Bicycle 1 from Hess, Moore, Hubbard 2012
+M = np.array([[106.0, 1.55], [1.55, 0.25]])
+K0 = np.array([[-93.2, -1.76], [-1.76, -0.68]])
+K2 = np.array([[0.0, 77.3], [0.0, 1.58]])
+C1 = np.array([[0.0, 29.9], [-0.45, 1.08]])
+v, g = 5.0, 9.81
+c, w, lam = 0.068, 1.12, 0.4
+# H from Jason in Moore 2012 (should be Bicycle 1 values to be perfectly
+# correct)
+H = np.array([[0.943], [0.011]])
+# Bicycle plant state space
+Minv = np.linalg.inv(M)
+# x = phi, del, phid, deld
+Ab = np.block([
+    [np.zeros_like(M), np.eye(2)],
+    [-Minv@(g*K0 + v**2*K2), -Minv*v@C1]
+])
+# u = Tphi, Tdel, F
+Bb = np.block([
+    [np.zeros_like(M), np.zeros((2, 1))],
+    [Minv, Minv@H]
+])
+
+vals = np.array([
+    Ab[3, 1],  # a_deldd_del,
+    Ab[3, 3],  # a_deldd_deld,
+    Ab[3, 0],  # a_deldd_phi,
+    Ab[3, 2],  # a_deldd_phid,
+    Ab[2, 1],  # a_phidd_del,
+    Ab[2, 3],  # a_phidd_deld,
+    Ab[2, 0],  # a_phidd_phi,
+    Ab[2, 2],  # a_phidd_phid,
+    v/w*np.cos(lam),  # a_psid_del,
+    c/w*np.cos(lam),  # a_psid_deld,
+    v,  # a_ypd_psi,
+    Bb[3, 2],  # b_deldd_F,
+    Bb[3, 1],  # b_deldd_Tdel,
+    Bb[2, 2],  # b_phidd_F,
+    Bb[2, 1],  # b_phidd_Tdel,
+    -c*np.cos(lam),  # c_yq_del,
+    w,  # c_yq_psi,
+    48.0,  # kDelta,
+    9.03,  # kPhi,
+    -0.08,  # kPhiDot,
+    0.161,  # kPsi,
+    0.097,  # kYQ,
+    30.0,  # omega
+    0.707,  # zeta
+])
+
+A_cl, B_cl, C_cl = eval_sys(*vals)
+
+sys = lti(A_cl, B_cl, C_cl, np.zeros((C_cl.shape[0], B_cl.shape[1])))
+t = np.linspace(0.0, 5.0, num=100)
+u = np.zeros((len(t), B_cl.shape[1]))
+u[len(t)//4:len(t)//3, 0] = 20.0  # F
+u[:, 1] = 0.2  # yc
+
+t, y, x = lsim(sys, u, t)
+
+fig, axes = plt.subplots(nrows=y.shape[1])
+for yi, ax, lab in zip(y.T, axes, system_outputs):
+    ax.plot(t, yi, label=lab)
+    ax.legend()
+plt.show()