In [1]:
%matplotlib inline
%load_ext autoreload
%autoreload 2
import numpy as np
from matplotlib import pylab as plt
from mpl_toolkits import mplot3d
from calc_traj import calc_traj
from range_doppler import *
from kalman_filter import KalmanFilter
from util import *
np.set_printoptions(precision=3)
The following section documents the development of a model for golf ball flight dynamics, it is largely based off EMSS's own investigations.
Ignoring all external influences, Figure 1 depicts the principle forces acting a golf ball in flight. These forces are assumed to be those with the largest influence on the golf ball's trajectory. The forces are:
$\mathbf{F}_D$: The drag force is opposed to the velocity $\mathbf{v}$, its magnitude for a given component is determined by $$F_{D} = \frac{1}{2} C_{D} \rho A |v|^{2}$$ $\rho$ is the density of the air and $A$ golf ball's cross sectional area. The drag coefficient, $C_{D}$, is itself a function of both the ball's linear and angular speed.
$\mathbf{F}_{L}$: The lift force is perpendicular to linear and rotational velocity, thus determined by a cross product. Its magnitude for a given component is determined by $$F_{L} = \frac{1}{2} C_{L} \rho A |v|^{2}$$ The lift coefficient, $C_{L}$, is itself a function of both the ball's linear and angular speed.
$m\mathbf{g}$: Gravity, it goes downwards.
Applying Newton's second law, $$\mathbf{a} = \frac{1}{m} \sum \mathbf{F}$$ and simplifying results in the following system of ODES:
$$ \begin{eqnarray} \frac{d^2x}{dt^2} &=& -\frac{\rho A |v|}{2m} (C_{D}v_{x} - C_{L}[\frac{\omega_y}{|\omega|}v_z - \frac{\omega_y}{|\omega|}v_y]) \\ \frac{d^2y}{dt^2} &=& -g -\frac{\rho A |v|}{2m} (C_{D}v_{y} - C_{L}[\frac{\omega_z}{|\omega|}v_x - \frac{\omega_x}{|\omega|}v_z]) \\ \frac{d^2z}{dt^2} &=& -\frac{\rho A |v|}{2m} (C_{D}v_{z} - C_{L}[\frac{\omega_x}{|\omega|}v_y - \frac{\omega_y}{|\omega|}v_x]) \end{eqnarray}$$This system assumes a linear rate of decay for the spin, which is described the following system of equations:
$$ \begin{eqnarray} \frac{d\omega_x}{dt} &=& -SRD \frac{\omega_x |v|}{R} \\ \frac{d\omega_y}{dt} &=& -SRD \frac{\omega_y |v|}{R} \\ \frac{d\omega_z}{dt} &=& -SRD \frac{\omega_z |v|}{R} \end{eqnarray} $$$R$ is the radius of the golf ball.
Remark : It is convention set the $y$-axis as elevation allowing the spin rate to be positive for back spin . This leads to a constant interchange of $y$ and $z$ axes for launch and direction angle calculations; however; this can be abstracted away.
Here SRD, $C_{D}$ and $C_{L}$ are given by their empirical equations:
$$ \begin{eqnarray} C_{D} &=& C_{D1} + C_{D2} + C_{D3} \sin(\pi [R_{e} - A_{1}]/A_{2}) \\ R_{e} &=& \frac{\rho |v| L}{\mu} \\ C_{L} &=& C_{L1} S^{0.45} \\ SRD &=& R_{1} S \\ S &=& \frac{ |\omega| R}{|v|} \end{eqnarray} $$It can be seen the drag coefficient depends on the object's shape and Reynolds number, $R_{e}$, and the lift coefficient on the spin rate factor, $S$. The values of the constants $C_{D1}$, $C_{D2}$, $C_{L1}$, $\mu$, $R$, $R_{1}$, $A_{1}$ and $A_{2}$ can be found in the calc_traj.py module.
Remark : EMSS's documentation shows the model is reasonably accurate when compared to the PGA tour measurements; however it requires the subtle tweaking of several parameters which aren't mentioned in the documentation. This model will not be used as the Kalman filter's system model but it will be used to generate the golf ball's simualted trajectory. This trajectory will then be used to simulate range-Doppler measurements which via multilateration will be converted back to its cartesian coordinates.
The system of ODES can be solved using the function calc_traj in the calc_traj.py module. The initial conditions for the various clubs can be found in the clubs.csv file or the table below. The table and plot below document the simulation's results.
In [2]:
names, p0, v0, w0 = load_clubs('clubs.csv')
N = len(names)
t = np.arange(0, 12, 0.001)
| Club | Launch angle ($^{\circ}$) | Speed (m/s) | Spin (RPM) | Maximum height (m) | Simulated height (m) | Maximum carry (m) | Simulated carry (m) |
|---|---|---|---|---|---|---|---|
| Driver | 10.9 | 74.2 | 2686 | 29.2 | 31.37 | 251.46 | 240.79 |
| 3-Wood | 9.2 | 70.2 | 3655 | 27.43 | 28.81 | 222.20 | 230.44 |
| 5-Wood | 9.4 | 67.5 | 4350 | 28.35 | 29.77 | 210.31 | 224.44 |
| Hybrid | 10.2 | 63.1 | 4360 | 24.69 | 28.22 | 193.85 | 210.82 |
| 3 Iron | 10.2 | 63.1 | 4360 | 24.69 | 28.22 | 193.85 | 210.82 |
| 4 Iron | 11.0 | 60.9 | 4836 | 25.60 | 27.71 | 185.62 | 203.88 |
| 5 Iron | 12.1 | 58.7 | 5361 | 28.35 | 32.94 | 177.39 | 198.87 |
| 6 Iron | 14.1 | 56.4 | 6231 | 27.43 | 36.34 | 167.33 | 190.14 |
| 7 Iron | 16.3 | 53.3 | 7097 | 29.26 | 33.33 | 157.28 | 169.26 |
| 8 Iron | 18.1 | 51.1 | 7998 | 28.35 | 33.33 | 146.30 | 156.38 |
| 9 Iron | 20.4 | 48.4 | 8647 | 27.43 | 34.84 | 135.33 | 140.73 |
| PW | 24.2 | 45.3 | 9304 | 26.52 | 35.33 | 124.36 | 138.71 |
In [3]:
fig = plt.figure(figsize=(25, 25))
ax = plt.axes(projection='3d')
for i in np.arange(0, N):
_, f, _ = calc_traj(p0[i, :], v0[i, :], w0[i, :], t)
ax.plot(f[:, 0], f[:, 1], f[:, 2])
ax.set_xlabel('x (m)', fontsize = '20')
ax.set_ylabel('y (m)', fontsize = '20')
ax.set_zlabel('z (m)', fontsize = '20')
ax.set_title('Trajectories for various club types', fontsize = '20')
ax.legend(names)
plt.show()
Each of EMSS's sensors will output a range-Doppler map for a given time instance. Rather than simulate this in full, the Doppler information will be approximated by the ball's radial velocity seen by the sensor.
Using the simulation data, the position and velocity, $\mathbf{p}$ and $\mathbf{v}$, of the golf ball are known in full. Therefore the range seen by each sensor for each time step, $r_{k, i}$, is given by:
$$ \begin{eqnarray} r_{k,i} = |\mathbf{p} - \mathbf{s}_k| \end{eqnarray}$$Where $\mathbf{s}_k$ is the known vector position of a given sensor. The radial velocity, $v_{k, i}$ seen by each sensor is determined as follows: $$ \begin{eqnarray} \mathbf{u}_{k,i} &=& \frac{\mathbf{p} - \mathbf{s}_k}{r_{k,i}} \\ v_{k, i} &=& \mathbf{u}_{k,i} \cdot \mathbf{v}_i \end{eqnarray}$$
$\mathbf{u}_{k,i}$ is the unit vecotr from a sensor $k$ to the ball's current position.
The function range_doppler in range_doppler.py determines the range and radial velocity of the ball seen by each sensor. The range-Doppler plot below does not reflect an actual map as it plots the entire trajectory.
Remark : Here I treated CPI (Coherent Processing Interval) as sensor sampling period, it seems to be what EMSS did. The sensor locations were also borrowed from EMSS's documentation, so I assume they reflect reality is some small way. Note the radial velocity is unneeded for the multilateration process, as it only requires range measurements, but was included for completeness.
In [4]:
sensor_locations = np.array([[-10, 28.5, 1], [-15, 30.3, 3],
[200, 30, 1.5], [220, -31, 2],
[-30, 0, 0.5], [150, 10, 0.6]])
cpi = 40e-3
t_sim = np.arange(0, 12, cpi)
t, p, v = calc_traj(p0[0, :], v0[0, :], w0[0, :], t_sim)
In [5]:
r_d = range_doppler(sensor_locations, p, v)
plt.figure(figsize= (10, 10))
plt.plot(r_d[0, :, 0], r_d[0, :, 1])
plt.xlabel('v (m/s)')
plt.ylabel('Range (m)')
plt.grid(True)
plt.title('range-Doppler plot for Sensor 1')
Out[5]:
There are two main techniques used for range based localization in sensor networks; trilateration and multilateration. Multilateration is derived from trilateration, but is capable of accurate localization when plagued by measurement noise. The following mathematical derivation is hoisted from [1] .</i>
Trilateration is the process of finding a node in space, given certain known anchor points. Assume the positions of the anchor points are defined by vectors $\mathbf{n}_0, \mathbf{n}_1, \mathbf{n}_1 \in \mathscr{R}^2$. An unkown point, $\mathbf{p} \in \mathscr{R}^2$, is at a known distance $d_{i}$ from each anchor point, therefore:
$$ \begin{eqnarray} d_{0}^{2} &=& |\mathbf{p} - \mathbf{n}_{0}| &=& |\mathbf{p}|^{2} - 2 \mathbf{p} \cdot \mathbf{n}_{0} + |\mathbf{n}_{0}|^{2} \\ d_{1}^{2} &=& |\mathbf{p} - \mathbf{n}_{1}| &=& |\mathbf{p}|^{2} - 2 \mathbf{p} \cdot \mathbf{n}_{1} + |\mathbf{n}_{1}|^{2} \\ d_{2}^{2} &=& |\mathbf{p} - \mathbf{n}_{2}| &=& |\mathbf{p}|^{2} - 2 \mathbf{p} \cdot \mathbf{n}_{2} + |\mathbf{n}_{2}|^{2} \\ \end{eqnarray} $$Subtracting the second and third equations from the first, yields: $$ \begin{eqnarray} 2(\mathbf{n}_{1} - \mathbf{n}_{0}) \cdot \mathbf{p} = d_{0}^{2} - d_{1}^{2} - |\mathbf{n}_{0}|^{2} + |\mathbf{n}_{1}|^{2} \\ 2(\mathbf{n}_{2} - \mathbf{n}_{0}) \cdot \mathbf{p} = d_{0}^{2} - d_{2}^{2} - |\mathbf{n}_{0}|^{2} + |\mathbf{n}_{2}|^{2} \\ \end{eqnarray} $$
Which can be rewritten in matrix form:
$$ \begin{eqnarray} \mathbf{A} &=& \begin{bmatrix} \mathbf{n}_{1} - \mathbf{n}_{0} \\ \mathbf{n}_{2} - \mathbf{n}_{0} \end{bmatrix}_{2x2} \\ \mathbf{b} &=& \begin{bmatrix} d_{0}^{2} - d_{1}^{2} - |\mathbf{n}_{0}|^{2} + |\mathbf{n}_{1}|^{2} \\ d_{0}^{2} - d_{2}^{2} - |\mathbf{n}_{0}|^{2} + |\mathbf{n}_{2}|^{2} \end{bmatrix}_{2x1} \\ 2 \mathbf{A} \cdot \mathbf{p} &=& \mathbf{b} \end{eqnarray} $$In general, to determine $\mathbf{p} \in \mathscr{R}^{N}$, $N+1$ anchor points are required $\mathbf{n}_{0}, ..., \mathbf{n}_{N} \in \mathscr{R}^{N}$. The geometric constraints are still satisfied with $|\mathbf{p} - \mathbf{n}_{i}|^2 = d_{i}^{2}$, $i = {0, 1, .. , N}$ and by a similiar process to that presented here will result in $N$ linear equations.
Trialateration asssumes perfect measurements, whereas multilateration can account for error by making use of $M+1$ anchor points in $\mathscr{R}^{N}$, $M > N$. Using the similiar constraints to trilateration, $|\mathbf{p} - \mathbf{n}_{i}|^2= d_{i}^{2}$, $i = {0, 1, .. , M}$ a system of $M$ linear equations are derived:
$$ \begin{eqnarray} \mathbf{A} &=& \begin{bmatrix} \mathbf{n}_{1} - \mathbf{n}_{0} \\ \dots \\ \mathbf{n}_{M} - \mathbf{n}_{0} \end{bmatrix}_{MxN} \\ \mathbf{b} &=& \begin{bmatrix} d_{0}^{2} - d_{1}^{2} - |\mathbf{n}_{0}|^{2} + |\mathbf{n}_{1}|^{2} \\ \dots \\ d_{0}^{2} - d_{M}^{2} - |\mathbf{n}_{0}|^{2} + |\mathbf{n}_{M}|^{2} \end{bmatrix}_{Mx1}\\ 2 \mathbf{A} \cdot \mathbf{p} &=& \mathbf{b} \end{eqnarray} $$As defined $M > N$, therfore this system is overdetermined. Luckily a least squares solution provides an accurate approximate for $\mathbf{p}$, whereas trilateration will produce significant error. The best estimate of $\mathbf{p}$ can be provided by first using QR-decompostion:
$$ \begin{eqnarray} 2 \mathbf{A} \cdot \mathbf{p} &=& \mathbf{b} \\ 2 \mathbf{QR} \cdot \mathbf{p} &=& \mathbf{b} \\ 2 \mathbf{R} \cdot \mathbf{p} &=& \mathbf{Q}^{T} \cdot \mathbf{b} \end{eqnarray}$$Here $\mathbf{Q}$ is orthonormal, $\mathbf{Q}^{T} \mathbf{Q} = \mathbf{I}$. This manipulation minimizes the error, $|2\mathbf{A} \cdot \mathbf{p} - \mathbf{b}|$, providing the best estimate for $\mathbf{p}$.
For any general, consistent system of equations:
$$\mathbf{A}_{m \times n} \mathbf{x}_{n \times 1} = \mathbf{b}_{m \times 1}$$If there is a solution, $\mathbf{b}$ is then simply a linear combination of the columns of $\mathbf{A}$. If $\mathbf{A} = \begin{bmatrix} \mathbf{c_{1}} & \cdots & \mathbf{c_{n}} \end{bmatrix}$, $\mathbf{c}_{i} \in \mathscr{R}^{m}$, then $\mathbf{b} = \sum^{n}_{i=1}x_{i} \mathbf{c}_{i}$. $\mathbf{b}$ is therefore an element in the vector space spanned by the columns of $\mathbf{A}$. This column space, $C(\mathbf{A})$, is a plane through the origin in $\mathscr{R}^{m}$ with the linearly dependent columns $\mathbf{A}$ serving as it basis, therefore its dimension is the rank, $r$, of $\mathbf{A}$. If $\mathbf{b}$ is in $C(\mathbf{A})$ there must be a solution for $\mathbf{x}$ as $\mathbf{x}$ describes the combination of the columns required to produce $\mathbf{b}$.
Remark : In general, $\mathbf{x} = \mathbf{x}_p + \mathbf{x}_n$, where $\mathbf{x}_n$ are some combination of the vectors in the nullspace, $N(\mathbf{A})$. However in the case of multilateration the rank of $\mathbf{A}$ is $n$ and therefore only produces a trivial nullspace (only the zero vector) and $\mathbf{x} = \mathbf{x}_p$.
The idea of $\mathbf{b}$ fitting perfectly into $C(\mathbf{A})$ is all well and good in theory, but what about practice? If $\mathbf{b}$ is measured data, there will multiple measurements $m>n$ and they will be unique such that $r = n$. $\mathbf{b}$ will almost always be corrupted by noise and be independent from the columns of $\mathbf{A}$, it will lie outside the column space's plane, leading to an inconsistent system of equations with no solution. So how to proceed? Solve the closest problem.
As mentioned before only vectors within the column space will solve this system, therefore necessary find the best approximate, $\mathbf{p}$, of $\mathbf{b}$ within $C(\mathbf{A})$. The system is now approximated by:
$$ \begin{eqnarray} \mathbf{A} \mathbf{\hat{x}} &=& \mathbf{p} \\ \mathbf{p} &=& \mathbf{P} \mathbf{b} \end{eqnarray}$$$\mathbf{\hat{x}}$ is our approximate solution and $\mathbf{p}$ is the projection of $\mathbf{b}$ onto the plane through the projection matrix $\mathbf{P}$. It is now our job to find the best possible $\mathbf{P}$, which minimizes the error between $\mathbf{p}$ and $\mathbf{b}$,
$$ \begin{eqnarray} \mathbf{e} &=& \mathbf{b} - \mathbf{p} \\ &=& \mathbf{b} - \mathbf{A} \mathbf{\hat{x}} \end{eqnarray}$$Mimimizing the total error,
$$\begin{eqnarray} ||\mathbf{e}|| &=& (\mathbf{b} - \mathbf{A} \mathbf{\hat{x}})^{T}(\mathbf{b} - \mathbf{A} \mathbf{\hat{x}}) \\ &=& \mathbf{b} \mathbf{b}^{T} - \mathbf{b}^{T} \mathbf{A} \mathbf{\hat{x}} - \mathbf{\hat{x}}^{T} \mathbf{A}^{T} \mathbf{b} - \mathbf{\hat{x}}^{T} \mathbf{A} \mathbf{A}^{T} \mathbf{\hat{x}}\\ \frac{\partial ||\mathbf{e}||}{\partial \mathbf{\hat{x}}} &=& - 2 \mathbf{b}^{T} \mathbf{A} + 2 \mathbf{\hat{x}}^{T} \mathbf{A}^{T} \mathbf{A} = \mathbf{0} \\ \mathbf{0} &=& -2 ( \mathbf{b}^{T} - \mathbf{\hat{x}}^{T} \mathbf{A}^{T} ) \mathbf{A} \\ \mathbf{0} &=& -2 \mathbf{e}^{T} \mathbf{A} \\ \mathbf{0}^{T} &=& ( \mathbf{e}^{T} \mathbf{A} )^{T} \\ \mathbf{0} &=& \mathbf{A}^{T} \mathbf{e} \\ \end{eqnarray}$$Remark: This is like minimizing energy, I assume there is no maximum error. This isn't particularly important, but it is nice to see a perdendicular projection come out so naturally.
The energy will be minimized when $\mathbf{e}$ is perdendicular to every column of $\mathbf{A}$, this means $\mathbf{e}$ should be perpendicular to the plane spanned by $C(\mathbf{A})$ (and thus it is in the left nullspace of $\mathbf{A}$, $N(\mathbf{A}^{T})$). This gives:
$$ \begin{eqnarray} \mathbf{A}^{T} \mathbf{e} &=& \mathbf{0} \\ \mathbf{c}_{1}^{T} \mathbf{e} &=& 0 \\ \cdots \\ \mathbf{c}_{n}^{T} \mathbf{e} &=& 0 \\ \end{eqnarray} $$From this,
$$ \begin{eqnarray} \mathbf{A}^{T} ( \mathbf{b} - \mathbf{A} \mathbf{\hat{x}} ) &=& \mathbf{0} \\ \mathbf{A}^{T} \mathbf{A} \mathbf{\hat{x}} &=& \mathbf{A}^{T} \mathbf{b} \\ \mathbf{\hat{x}} &=& (\mathbf{A}^{T} \mathbf{A})^{-1} \mathbf{A}^{T} \mathbf{b} \\ \mathbf{A} \mathbf{\hat{x}} &=& \mathbf{A} (\mathbf{A}^{T} \mathbf{A})^{-1} \mathbf{A}^{T} \mathbf{b} \\ \mathbf{p} &=& \mathbf{A} (\mathbf{A}^{T} \mathbf{A})^{-1} \mathbf{A}^{T} \mathbf{b} \end{eqnarray} $$We have now found our projection matrix $\mathbf{P} = \mathbf{A} (\mathbf{A}^{T} \mathbf{A})^{-1} \mathbf{A}^{T} \mathbf{b}$.
Remark : If have $\mathbf{A}$ has linearly dependent columns as we have assumed $\mathbf{A}^{T}\mathbf{A}$ will be invertible. In fact it will be positive definite, one of the best possible types matrix.
After projecting $\mathbf{b}$ onto the $C(\mathbf{A})$ plane we are left with the following system:
$$ \begin{eqnarray} \mathbf{A} \mathbf{\hat{x}} &=& \mathbf{p} \\ \mathbf{A} \mathbf{\hat{x}} &=& \mathbf{A} (\mathbf{A}^{T} \mathbf{A})^{-1} \mathbf{A}^{T} \mathbf{b} \\ \mathbf{A}^{T} \mathbf{A} \mathbf{\hat{x}} &=& \mathbf{A}^{T} \mathbf{A} (\mathbf{A}^{T} \mathbf{A})^{-1} \mathbf{A}^{T} \mathbf{b} \\ \mathbf{A}^{T} \mathbf{A} \mathbf{\hat{x}} &=& \mathbf{A}^{T} \mathbf{b} \end{eqnarray}$$As mentioned before $\mathbf{A}$ has $n$ independent columns and therefore $\mathbf{A}^{T} \mathbf{A}$ is positive definite and the system has a unique solution.
Remark : You may notice this final result was already reached in steps leading up to the projection matrix. I prefer projecting $\mathbf{b}$ then simpliying the resulting system, it just makes more sense to me.
You have notice in the formal explanation of multilateration I multiply by some orthonormal matrix, $\mathbf{Q}^{T}$, instead of by $\mathbf{A}^{T}$. This is done by choosing an orthonormal basis for the column space as it greatly simplifies the projection matrix:
$$\begin{eqnarray} \mathbf{P} &=& \mathbf{Q} (\mathbf{Q}^T \mathbf{Q})^{-1} \mathbf{Q}^{T} \mathbf{b} \\ &=& \mathbf{Q} \mathbf{Q}^{T} \mathbf{b} \\ \end{eqnarray}$$Exploiting $\mathbf{QR}$ decomposition we can solve a much simpler system:
$$\begin{eqnarray} \mathbf{A} \mathbf{\hat{x}} &=& \mathbf{Q} \mathbf{Q}^{T} \mathbf{b} \\ \mathbf{Q} \mathbf{R} \mathbf{\hat{x}} &=& \mathbf{Q} \mathbf{Q}^{T} \mathbf{b} \\ \mathbf{Q}^{T} \mathbf{Q} \mathbf{R} \mathbf{\hat{x}} &=& \mathbf{Q}^{T} \mathbf{Q} \mathbf{Q}^{T} \mathbf{b} \\ \mathbf{R} \mathbf{\hat{x}} &=& \mathbf{Q}^{T} \mathbf{b} \\ \end{eqnarray} $$Whether doing $\mathbf{QR}$ decomposition is more efficient than simply mutliplying by the transpose of a known matrix is mystery to me, but whatever.
Remark : The term orthonormal is usually reserved for square matrices, $\mathbf{Q}$ in this case is rectangular with orthormal columns, but we only have so much time. $\mathbf{Q}^{T}$ is $\mathbf{Q}$'s left inverse.
Remark: At this point you should be thoroughly convinced my derivations aren't worth reading. They are done more so to reinforce my understanding of the linear algebra, which is flaky at best.
The function multilateration in range_doppler determines the golf ball's coordinates in $\mathscr{R}^{3}$. The result for a single club is plotted below.
Remark : As of yet no measurement noise has been introduced and this result should be equivalent to trilateration. As it can be seen from the derivation the clocks on each sensor must be synchronized, hopefully EMSS's system accounts for this.
In [6]:
p_m = multilateration(sensor_locations, r_d[:, :, 1])
In [7]:
fig = plt.figure(figsize=(25, 25))
ax = plt.axes(projection='3d')
ax.plot(p_m[:, 0], p_m[:, 1], p_m[:, 2], linewidth=3.0)
ax.plot(p[:, 0], p[:, 1], p[:, 2], 'or')
ax.set_xlabel('x (m)', fontsize = '20')
ax.set_ylabel('y (m)', fontsize = '20')
ax.set_zlabel('z (m)', fontsize = '20')
ax.set_title('Multilateration', fontsize = '20')
ax.set_ylim([-1, 1])
ax.legend(['Simualted trajectory', 'Multilaterated trajectory'])
plt.show()
In [8]:
v_m = determine_velocity(t, p_m, r_d[:, :, 0])
A Kalman Filter object was created which implements the basic linear Kalman filter algorithm, as given in Probabilistic Robotics [2], [3].
Remark: This documentation was done quickly and is terrible. I've tried to remain consistent with Probablistic Robotics' terminology and notation, but it hasn't always worked out. </i>
The state transistion probability, $p(\mathbf{x}_{t} \left.|\right. \mathbf{u}_{t}, \mathbf{x}_{t-1})$, is linear function in its arguments with added Gaussian noise:
$$\mathbf{x}_{t} = \mathbf{A}_t \mathbf{x}_{t-1} + \mathbf{B}_{t} \mathbf{u}_{t} + \mathbf{\epsilon}_t$$Where,
As we are only tracking the golf ball, both the state transistion and control matrices will be used to model the system. For a first attempt a very basic model will be used to describe the system, ommiting all areodynamical contrivances. The state transition will be described the following system:
$$\begin{eqnarray} x_{t} &=& x_{t-1} + \dot{x}_{t-1} t \\ \dot{x}_{t} &=& \dot{x}_{t-1} \\ y_{t} &=& y_{t-1} + \dot{y}_{t-1} t \\ \dot{y}_{t} &=& \dot{y}_{t-1} \\ z_{t} &=& z_{t-1} + \dot{z}_{t-1} t - \frac{1}{2} g t^{2}\\ \dot{z}_{t} &=& \dot{z}_{t-1} - g t\\ \end{eqnarray}$$This can be written in matrix form, as:
\begin{eqnarray} \begin{bmatrix} x_{t} \\ \dot{x}_{t} \\ y_{t} \\ \dot{y}_{t} \\ z_{t} \\ \dot{z}_{t} \\ \end{bmatrix} &=& \begin{bmatrix} 1 & 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 1 \\ 0 & 0 & 0 & 0 & 0 & 1 \\ \end{bmatrix} \begin{bmatrix} x_{t-1} \\ \dot{x}_{t-1} \\ y_{t-1} \\ \dot{y}_{t-1} \\ z_{t-1} \\ \dot{z}_{t-1} \\ \end{bmatrix} + \begin{bmatrix} 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 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 & 1 \\ \end{bmatrix} \begin{bmatrix} 0 \\ 0 \\ 0 \\ 0 \\ - \frac{1}{2} g t^{2} \\ - g t \\ \end{bmatrix} \\ \mathbf{x}_{t} &=& \mathbf{A}_t \mathbf{x}_{t-1} + \mathbf{B}_{t} \mathbf{u}_{t} \end{eqnarray}With the $\mathbf{x}_{t}$, $ \mathbf{u}_{t}$, $\mathbf{A}_t$ and $\mathbf{B}_{t}$ in place, all that remains is to define $\mathbf{\epsilon}_{t}$'s covariance matrix, $\mathbf{R}_t$:
$$\mathbf{R}_{t} = \begin{bmatrix} \sigma_{r}^{2} & 0 & 0 & 0 & 0 & 0 \\ 0 & \sigma_{r}^{2} & 0 & 0 & 0 & 0 \\ 0 & 0 & \sigma_{r}^{2} & 0 & 0 & 0 \\ 0 & 0 & 0 & \sigma_{r}^{2} & 0 & 0 \\ 0 & 0 & 0 & 0 & \sigma_{r}^{2} & 0 \\ 0 & 0 & 0 & 0 & 0 & \sigma_{r}^{2} \\ \end{bmatrix} $$Here $\sigma_{r}^{2}$ is some appropriate variance.
Remark : Defining the covaraince as such keeps the noise for each 'state variable' independent, but I am not sure if this is the correct approach. Sure is easy though.
The measurement probability, $p(\mathbf{z}_t | \mathbf{x}_t)$, is also linear in arguments, with added Gussian noise:
$$ \mathbf{z}_t = \mathbf{C}_t \mathbf{x}_t + \mathbf{\delta}_t $$Where,
As the state only contains information about the ball's position and velocity and these can be 'directly' measured, $\mathbf{C}_t$ is simply an identity matrix:
\begin{eqnarray} \begin{bmatrix} z_{x_{t}} \\ z_{\dot{x}_{t}} \\ z_{y_{t}} \\ z_{\dot{y}_{t}} \\ z_{z_{t}} \\ z_{\dot{z}_{t}} \\ \end{bmatrix} &=& \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \\ \end{bmatrix} \begin{bmatrix} x_{t} \\ \dot{x}_{t} \\ y_{t} \\ \dot{y}_{t} \\ z_{t} \\ \dot{z}_{t} \\ \end{bmatrix} \\ \mathbf{z}_{t} &=& \mathbf{C}_t \mathbf{x}_{t} \end{eqnarray}The covariance matrix $\mathbf{Q}_t$, through similar motivation to $\mathbf{R}_t$, is:
$$\mathbf{Q}_{t} = \begin{bmatrix} \sigma_{q}^{2} & 0 & 0 & 0 & 0 & 0 \\ 0 & \sigma_{q}^{2} & 0 & 0 & 0 & 0 \\ 0 & 0 & \sigma_{q}^{2} & 0 & 0 & 0 \\ 0 & 0 & 0 & \sigma_{q}^{2} & 0 & 0 \\ 0 & 0 & 0 & 0 & \sigma_{q}^{2} & 0 \\ 0 & 0 & 0 & 0 & 0 & \sigma_{q}^{2} \\ \end{bmatrix} $$The code below intialises the variables for the Driver club, the values of $\sigma_{r}^{2}$ and $\sigma_{q}^{2}$ vary according to mood.
In [9]:
N = 6
M, _ = p_m.shape
dt = cpi
g = 9.81
sigma_r = 2.5
sigma_q = 0.1
In [10]:
A_t = np.identity(N)
A_t[0, 1] = A_t[2, 3] = A_t[4, 5] = dt
B_t = np.zeros((N, N))
B_t[4, 4] = B_t[5, 5] = 1
R_t = np.identity(N)*sigma_r
C_t = np.identity(N)
Q_t = np.identity(N)*sigma_q
In [11]:
mu_0 = np.zeros((N, 1))
mu_0[[1, 3, 5], :] = v[0, :].reshape(3, 1)
sig_0 = np.identity(N)
In [12]:
u_t = np.zeros((6, 1))
u_t[4] = -0.5*g*(dt**2)
u_t[5] = -g*dt
In [13]:
z_t = np.empty((M, N))
z_t[:, [0, 2, 4]] = p_m
z_t[:, [1, 3, 5]] = v_m
The KalmanFilter object directly implements the algorithm given in Probabilistic Robotics Chapter 3. The algorithm is now restated:
Remark : Nothing clever is done here, the algorithm is implemented as written, inverses are computed directly and so on.
The Kalman filter's estimates are determined by stepping through the processed measurements, $\mathbf{p}_m$ and $\mathbf{v}_m$, and feeding in a constant control vector $\mathbf{u}_t$.
Remark : As defined above, $\mathbf{u}_t$, is a component of the system model and not a control vector. This choice was made to keep the KalmanFilter object general, or something.
In [14]:
kf = KalmanFilter(A_t, B_t, C_t, R_t, Q_t, mu_0, sig_0)
In [15]:
mu_t = np.empty((M, N))
for i in np.arange(0, M):
mu_t[i, :] = kf.step(u_t, z_t[i]).reshape((N, ))
p_e = mu_t[:, [0, 2, 4]].reshape((M, 3))
v_e = mu_t[:, [1, 3, 5]].reshape((M, 3))
In [16]:
fig = plt.figure(figsize=(25, 25))
ax = plt.axes(projection='3d')
ax.plot(p[:, 0], p[:, 1], p[:, 2], linewidth=3.0)
ax.plot(p_e[:, 0], p_e[:, 1], p_e[:, 2], 'or')
ax.set_xlabel('x (m)', fontsize = '20')
ax.set_ylabel('y (m)', fontsize = '20')
ax.set_zlabel('z (m)', fontsize = '20')
ax.set_title('Kalman Filtering', fontsize = '20')
ax.set_ylim([-1, 1])
ax.legend(['Actual Trajectory', 'Estimated trajectory'])
plt.show()