In [2]:
import numpy as np
import matplotlib.pyplot as plt
import sympy as sp
%matplotlib inline
sp.init_printing()

In [348]:
# formulation w/ neat latex / greek symbols

f, h, s, ye, psie, w, xk, v, k, dt = sp.symbols("f, h, s, y_e, \psi_e, \omega, x_k, v, \kappa, \delta_t", real=True)
Cv, Tv, Cs, Ts, u_steering, u_acceleration = sp.symbols("C_v, T_v, C_s, T_s, u_\delta, u_a", real=True)
mu_s, mu_g, mu_ax, mu_ay = sp.symbols("\mu_\delta \mu_g, \mu_{a_x}, \mu_{a_y}", real=True)
c = sp.symbols("c", function=True)  # c(s) returns the curvature (1/radius) of the turn at track location s

In [3]:
# source code friendly formulation

f, h, s, ye, psie, w, xk, v, k, dt = sp.symbols("f, h, s, ye, psie, w, xk, v, k, dt", real=True)
Cv, Tv, Cs, Ts, u_steering, u_acceleration = sp.symbols("Cv, Tv, Cs, Ts, u_steering, u_acceleration", real=True)
mu_s, mu_g, mu_ax, mu_ay = sp.symbols("mu_s mu_g mu_ax, mu_ay", real=True)
c = sp.symbols("c", function=True)

In [4]:
f, h, ye, psie, w, xk, v, k, dt

Out[4]:
$$\left ( f, \quad h, \quad ye, \quad psie, \quad w, \quad xk, \quad v, \quad k, \quad dt\right )$$
In [5]:
Cv, Tv, Cs, Ts, u_steering, u_acceleration

Out[5]:
$$\left ( Cv, \quad Tv, \quad Cs, \quad Ts, \quad u_{steering}, \quad u_{acceleration}\right )$$
In [6]:
mu_s, mu_g, mu_ax, mu_ay  # steering and gyroscope bias

Out[6]:
$$\left ( \mu_{s}, \quad \mu_{g}, \quad \mu_{ax}, \quad \mu_{ay}\right )$$
In [10]:
# removed s from state
X = sp.Matrix([[ye, psie, w, v, k, Cv, Tv, Cs, Ts, mu_s, mu_g, mu_ax, mu_ay]])
X

Out[10]:
$$\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 [11]:
# dynamic state variables f(x)

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,
v + dt*(eCv * u_acceleration - v) / eTv,
k, Cv, Tv, Cs, Ts, mu_s, mu_g, mu_ax, mu_ay
]])
fx.T

Out[11]:
$$\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 [12]:
Fx = sp.simplify(sp.Matrix([sp.diff(fx, Xj) for Xj in X]).T)
Fx

Out[12]:
$$\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 [13]:
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 [25]:
# h
x_cg = sp.symbols("x_CG")
hx = sp.Matrix([
[(ye - x_cg*sp.tan(psie)) / sp.cos(psie),
sp.tan(psie),
-2*k
]])  # fitting x = a + by + cy^2
hx.T

Out[25]:
$$\left[\begin{matrix}\frac{- x_{CG} \tan{\left (psie \right )} + ye}{\cos{\left (psie \right )}}\\\tan{\left (psie \right )}\\- 2 k\end{matrix}\right]$$
In [26]:
# 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')
x_CG = Symbol('x_CG')
ye = Symbol('ye')
e = ImmutableMatrix([[1/cos(psie), (x_CG - 2*x_CG/cos(psie)**2 + ye*tan(psie))/cos(psie), 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], [0, 0, 0, 0, -2, 0, 0, 0, 0, 0, 0, 0, 0]])

Out[26]:
$$\left[\begin{array}{ccccccccccccc}\frac{1}{\cos{\left (psie \right )}} & \frac{1}{\cos{\left (psie \right )}} \left(x_{CG} - \frac{2 x_{CG}}{\cos^{2}{\left (psie \right )}} + ye \tan{\left (psie \right )}\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\\0 & 0 & 0 & 0 & -2 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\end{array}\right]$$
In [16]:
X

Out[16]:
$$\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 [17]:
# 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[17]:
$$\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 [18]:
H_imu = sp.simplify(sp.Matrix([sp.diff(h_imu, Xj) for Xj in X]).T)
H_imu

Out[18]:
$$\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 [19]:
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 [409]:
h_s = sp.Matrix([[s, k]])
H_s = sp.simplify(sp.Matrix([sp.diff(h_s, Xj) for Xj in X]).T)
sp.print_python(H_s)
H_s

e = ImmutableMatrix([[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0]])

Out[409]:
$$\left[\begin{array}{cccccccccccccc}1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\end{array}\right]$$
In [ ]:
# so S is just P[0,0] + variance(s)
# which means K is the left column of P (P H.T) divided by P[0,0] + variance(s)
# so as our estimate of s is refined, everything else is affected too by the covariance in P.

In [332]:
H_s * Fx.T * Fx * H_s.T

Out[332]:
$$\left[\begin{matrix}1 & \frac{dt v ye \cos{\left (psie \right )}}{\left(k ye - 1\right)^{2}}\\\frac{dt v ye \cos{\left (psie \right )}}{\left(k ye - 1\right)^{2}} & \frac{dt^{2} v^{2} ye^{2} \cos^{2}{\left (psie \right )}}{\left(k ye - 1\right)^{4}} + \frac{dt^{2} v^{2} \cos^{2}{\left (psie \right )}}{\left(k ye - 1\right)^{4}} + 1\end{matrix}\right]$$
In [377]:
# regression on curve given pixels
px, c = sp.symbols("p_x, c", complex=True)
p = (-ye*sp.exp(sp.I*psie) - sp.I/k * sp.exp(sp.I*psie) * (sp.exp(sp.I*k*s) - 1))
(p - px)**2

Out[377]:
$$\left(- p_{x} - y_{e} e^{i \psi_e} - \frac{i}{\kappa} \left(e^{i \kappa s} - 1\right) e^{i \psi_e}\right)^{2}$$
In [376]:
sp.simplify(sp.Matrix([sp.diff((p - px)**2, x) for x in [k, psie, ye, s]]))

Out[376]:
$$\left[\begin{matrix}- \frac{2}{\kappa^{3}} \left(\kappa \left(p_{x} + y_{e} e^{i \psi_e}\right) + i \left(e^{i \kappa s} - 1\right) e^{i \psi_e}\right) \left(\kappa s e^{i \left(\kappa s + \psi_e\right)} + i \left(e^{i \kappa s} - 1\right) e^{i \psi_e}\right)\\\frac{2}{\kappa^{2}} \left(\kappa \left(p_{x} + y_{e} e^{i \psi_e}\right) + i \left(e^{i \kappa s} - 1\right) e^{i \psi_e}\right) \left(i \kappa y_{e} - e^{i \kappa s} + 1\right) e^{i \psi_e}\\\frac{2}{\kappa} \left(\kappa \left(p_{x} + y_{e} e^{i \psi_e}\right) + i \left(e^{i \kappa s} - 1\right) e^{i \psi_e}\right) e^{i \psi_e}\\- \frac{2}{\kappa} \left(\kappa \left(p_{x} + y_{e} e^{i \psi_e}\right) + i \left(e^{i \kappa s} - 1\right) e^{i \psi_e}\right) e^{i \left(\kappa s + \psi_e\right)}\end{matrix}\right]$$
In [385]:
px, c = sp.symbols("p_x, c", complex=True)
thetax = sp.symbols('\\theta_x', real=True)

(px - c + sp.exp(sp.I * thetax) / k)**2

Out[385]:
$$\left(- c + p_{x} + \frac{e^{i \theta_x}}{\kappa}\right)^{2}$$
In [391]:
(sp.Matrix([sp.diff((px - c + sp.exp(sp.I * thetax) / k)**2, x) for x in [c, px, thetax, k]]))

Out[391]:
$$\left[\begin{matrix}2 c - 2 p_{x} - \frac{2}{\kappa} e^{i \theta_x}\\- 2 c + 2 p_{x} + \frac{2}{\kappa} e^{i \theta_x}\\\frac{2 i}{\kappa} \left(- c + p_{x} + \frac{e^{i \theta_x}}{\kappa}\right) e^{i \theta_x}\\- \frac{2}{\kappa^{2}} \left(- c + p_{x} + \frac{e^{i \theta_x}}{\kappa}\right) e^{i \theta_x}\end{matrix}\right]$$
In [421]:
sp.simplify(sp.diff(sp.sqrt(k**2 - px**2), px, px))

Out[421]:
$$- \frac{k^{2}}{\left(k^{2} - p_{x}^{2}\right)^{\frac{3}{2}}}$$
In [426]:
# -> -1/r. ok. confirmed.