In [1]:
import numpy as np
import matplotlib.pyplot as plt
import sympy as sp
%matplotlib inline
sp.init_printing()
In [2]:
X = np.vstack([np.linspace(-10, 0, 20), np.ones(20)]).T
y = np.linspace(-1, 1, 20) + np.random.randn(20)*0.1
B = np.linalg.solve(np.dot(X.T, X), np.dot(X.T, y))

plt.plot(X[:, 0], y, '.')

plt.plot(X[:, 0], np.dot(X, B), '--')
Out[2]:
[<matplotlib.lines.Line2D at 0x10b102090>]
In [105]:
np.linalg.inv(np.dot(X.T, X))
Out[105]:
array([[ 0.00542857,  0.02714286],
       [ 0.02714286,  0.18571429]])
In [106]:
B
Out[106]:
array([ 0.20507151,  0.99950092])
In [95]:
np.dot(X, B).T - y
Out[95]:
array([ 0.27559026,  0.09567109,  0.43089431, -0.10237317,  0.04768405,
        0.03102591, -0.20429733,  0.03103837, -0.1550533 , -0.26963858,
       -0.05420966, -0.08228222, -0.20143815, -0.0459794 , -0.12005627,
       -0.07579049,  0.07817814,  0.00498967,  0.20671372,  0.10933305])
In [96]:
np.dot((np.dot(X, B) - y).T, np.dot(X, B) - y)
Out[96]:
0.55731594973926846
In [97]:
np.dot(B.T, np.dot(np.dot(X.T, X), B)) - 2*np.dot(np.dot(X, B).T, y) + np.dot(y.T, y)
Out[97]:
0.55731594973926679
In [98]:
np.linalg.cholesky(np.linalg.inv(np.dot(X.T, X))*Out[68])
Out[98]:
array([[ 0.00247118,  0.        ],
       [-0.08454053,  0.07769152]])
In [239]:
# formulation w/ neat latex / greek symbols

f, h, ye, psie, w, xk, v, k, dt = sp.symbols("f, h, y_e, \psi_e, \omega, x_k, v, \kappa, \delta_t")
Cv, Tv, Cs, Ts, u_steering, u_acceleration = sp.symbols("C_v, T_v, C_s, T_s, u_\delta, u_a")
mu_s, mu_g, mu_ax, mu_ay = sp.symbols("\mu_\delta \mu_g, \mu_{a_x}, \mu_{a_y}")
In [257]:
# source code friendly formulation

f, h, ye, psie, w, xk, v, k, dt = sp.symbols("f, h, ye, psie, w, xk, v, k, dt")
Cv, Tv, Cs, Ts, u_steering, u_acceleration = sp.symbols("Cv, Tv, Cs, Ts, u_steering, u_acceleration")
mu_s, mu_g, mu_ax, mu_ay = sp.symbols("mu_s mu_g mu_ax, mu_ay")
In [258]:
f, h, ye, psie, w, xk, v, k, dt
Out[258]:
$$\left ( f, \quad h, \quad ye, \quad psie, \quad w, \quad xk, \quad v, \quad k, \quad dt\right )$$
In [259]:
Cv, Tv, Cs, Ts, u_steering, u_acceleration
Out[259]:
$$\left ( Cv, \quad Tv, \quad Cs, \quad Ts, \quad u_{steering}, \quad u_{acceleration}\right )$$
In [260]:
mu_s, mu_g, mu_ax, mu_ay  # steering and gyroscope bias
Out[260]:
$$\left ( \mu_{s}, \quad \mu_{g}, \quad \mu_{ax}, \quad \mu_{ay}\right )$$
In [261]:
X = sp.Matrix([[ye, psie, w, v, k, Cv, Tv, Cs, Ts, mu_s, mu_g, mu_ax, mu_ay]])
X
Out[261]:
$$\left[\begin{array}{ccccccccccccc}ye & psie & w & v & k & Cv & Tv & Cs & Ts & \mu_{s} & \mu_{g} & \mu_{ax} & \mu_{ay}\end{array}\right]$$
In [262]:
# dynamic state variables f(x)
# the clipped traction limit is an argument for a UKF here unless we use tanh or something

eCv, eTv, eCs, eTs = sp.exp(Cv), sp.exp(Tv), sp.exp(Cs), sp.exp(Ts)
fx = sp.Matrix([
        [ye - dt*v * sp.sin(psie),
         psie + dt*(w + v * k * sp.cos(psie) / (1 - k * ye)),
         w + dt*(v*eCs*(u_steering - mu_s) - w) / eTs,  # fixme: this should be clipped at some traction limit
         v + dt*(eCv * u_acceleration - v) / eTv,
         k, Cv, Tv, Cs, Ts, mu_s, mu_g, mu_ax, mu_ay
        ]])
fx.T
Out[262]:
$$\left[\begin{matrix}- dt v \sin{\left (psie \right )} + ye\\dt \left(\frac{k v \cos{\left (psie \right )}}{- k ye + 1} + w\right) + psie\\dt \left(v \left(- \mu_{s} + u_{steering}\right) e^{Cs} - w\right) e^{- Ts} + w\\dt \left(u_{acceleration} e^{Cv} - v\right) e^{- Tv} + v\\k\\Cv\\Tv\\Cs\\Ts\\\mu_{s}\\\mu_{g}\\\mu_{ax}\\\mu_{ay}\end{matrix}\right]$$
In [271]:
Fx = sp.simplify(sp.Matrix([sp.diff(fx, Xj) for Xj in X]).T)
Fx
Out[271]:
$$\left[\begin{array}{ccccccccccccc}1 & - dt v \cos{\left (psie \right )} & 0 & - dt \sin{\left (psie \right )} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\frac{dt k^{2} v \cos{\left (psie \right )}}{\left(k ye - 1\right)^{2}} & \frac{1}{k ye - 1} \left(dt k v \sin{\left (psie \right )} + k ye - 1\right) & dt & - \frac{dt k \cos{\left (psie \right )}}{k ye - 1} & \frac{dt v \cos{\left (psie \right )}}{\left(k ye - 1\right)^{2}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & - dt e^{- Ts} + 1 & - dt \left(\mu_{s} - u_{steering}\right) e^{Cs - Ts} & 0 & 0 & 0 & - dt v \left(\mu_{s} - u_{steering}\right) e^{Cs - Ts} & dt \left(v \left(\mu_{s} - u_{steering}\right) e^{Cs} + w\right) e^{- Ts} & - dt v e^{Cs - Ts} & 0 & 0 & 0\\0 & 0 & 0 & - dt e^{- Tv} + 1 & 0 & dt u_{acceleration} e^{Cv - Tv} & dt \left(- u_{acceleration} e^{Cv} + v\right) e^{- Tv} & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1\end{array}\right]$$
In [264]:
sp.print_python(Fx)
dt = Symbol('dt')
v = Symbol('v')
psie = Symbol('psie')
k = Symbol('k')
ye = Symbol('ye')
Ts = Symbol('Ts')
mu_s = Symbol('mu_s')
u_steering = Symbol('u_steering')
Cs = Symbol('Cs')
w = Symbol('w')
Tv = Symbol('Tv')
u_acceleration = Symbol('u_acceleration')
Cv = Symbol('Cv')
e = ImmutableMatrix([[1, -dt*v*cos(psie), 0, -dt*sin(psie), 0, 0, 0, 0, 0, 0, 0, 0, 0], [dt*k**2*v*cos(psie)/(k*ye - 1)**2, (dt*k*v*sin(psie) + k*ye - 1)/(k*ye - 1), dt, -dt*k*cos(psie)/(k*ye - 1), dt*v*cos(psie)/(k*ye - 1)**2, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, -dt*exp(-Ts) + 1, -dt*(mu_s - u_steering)*exp(Cs - Ts), 0, 0, 0, -dt*v*(mu_s - u_steering)*exp(Cs - Ts), dt*(v*(mu_s - u_steering)*exp(Cs) + w)*exp(-Ts), -dt*v*exp(Cs - Ts), 0, 0, 0], [0, 0, 0, -dt*exp(-Tv) + 1, 0, dt*u_acceleration*exp(Cv - Tv), dt*(-u_acceleration*exp(Cv) + v)*exp(-Tv), 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1]])
In [265]:
# h
hx = sp.Matrix([[ye / sp.cos(psie), sp.tan(psie)]])  # measuring b, m
hx.T
Out[265]:
$$\left[\begin{matrix}\frac{ye}{\cos{\left (psie \right )}}\\\tan{\left (psie \right )}\end{matrix}\right]$$
In [266]:
# H
Hx = sp.simplify(sp.Matrix([sp.diff(hx, Xj) for Xj in X]).T)
sp.print_python(Hx.subs([(psie, "psie"), (ye, "ye")]))
Hx
psie = Symbol('psie')
ye = Symbol('ye')
e = ImmutableMatrix([[1/cos(psie), ye*sin(psie)/cos(psie)**2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, cos(psie)**(-2), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]])
Out[266]:
$$\left[\begin{array}{ccccccccccccc}\frac{1}{\cos{\left (psie \right )}} & \frac{ye \sin{\left (psie \right )}}{\cos^{2}{\left (psie \right )}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & \frac{1}{\cos^{2}{\left (psie \right )}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\end{array}\right]$$
In [ ]:
 
In [267]:
X
Out[267]:
$$\left[\begin{array}{ccccccccccccc}ye & psie & w & v & k & Cv & Tv & Cs & Ts & \mu_{s} & \mu_{g} & \mu_{ax} & \mu_{ay}\end{array}\right]$$
In [268]:
# measure gyro_z, accel_x, accel_y
h_imu = sp.Matrix([[
            w + mu_g,
            v*w + mu_ax,
            -(sp.exp(Cv) * u_acceleration - v) / sp.exp(Tv) + mu_ay,
        ]])
h_imu.T
Out[268]:
$$\left[\begin{matrix}\mu_{g} + w\\\mu_{ax} + v w\\\mu_{ay} + \left(- u_{acceleration} e^{Cv} + v\right) e^{- Tv}\end{matrix}\right]$$
In [269]:
H_imu = sp.simplify(sp.Matrix([sp.diff(h_imu, Xj) for Xj in X]).T)
H_imu
Out[269]:
$$\left[\begin{array}{ccccccccccccc}0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0\\0 & 0 & v & w & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0\\0 & 0 & 0 & e^{- Tv} & 0 & - u_{acceleration} e^{Cv - Tv} & \left(u_{acceleration} e^{Cv} - v\right) e^{- Tv} & 0 & 0 & 0 & 0 & 0 & 1\end{array}\right]$$
In [270]:
sp.print_python(H_imu)
v = Symbol('v')
w = Symbol('w')
Tv = Symbol('Tv')
u_acceleration = Symbol('u_acceleration')
Cv = Symbol('Cv')
e = ImmutableMatrix([[0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0], [0, 0, v, w, 0, 0, 0, 0, 0, 0, 0, 1, 0], [0, 0, 0, exp(-Tv), 0, -u_acceleration*exp(Cv - Tv), (u_acceleration*exp(Cv) - v)*exp(-Tv), 0, 0, 0, 0, 0, 1]])
In [ ]: