<jupyter_start><jupyter_text>Kalman filtering -  some case studiesIn this notebook we will look at a few simplified cases of Kalman filtering. Estimating the trajectory of a ballIn the following we will develop a script to estimate the state of a thrown ball (position and velocity). The ball is pulled downwards by gravity and observed using a position sensor (assume some kind of image-recognition algorithm) perturbed by random Gaussian noise.The state of the system evolves according to the following discrete-time linear equations:$$ \vec{x}(t+1) = \vec{x}(t) + \Delta t \vec{v}(t) $$$$ \vec{v}(t+1) = \vec{v}(t) + \Delta t \vec{g} $$where $\vec{x} = (x,y)$ is the position vector, $\vec{v} = (\dot{x}, \dot{y})$ is the velocity vector and $\vec{g} = (0,g)$ is the gravitational force. Appending the position and velocity vectors together we can write the system evolution equation as$$\begin{bmatrix}           x_k \\           y_k \\           \dot{x}_k \\           \dot{y}_k         \end{bmatrix} =          \begin{bmatrix}         1 & 0 & \Delta t & 0 \\         0 & 1 & 0 & \Delta t \\         0 & 0 & 1 & 0 \\         0 & 0 & 0 & 1         \end{bmatrix} \cdot \begin{bmatrix}           x_{k-1} \\           y_{k-1} \\           \dot{x}_{k-1} \\           \dot{y}_{k-1}         \end{bmatrix} + \begin{bmatrix}         1 & 0 & 0 & 0 \\         0 & 1 & 0 & 0 \\         0 & 0 & 1 & 0 \\         0 & 0 & 0 & \Delta t         \end{bmatrix} \cdot         \begin{bmatrix}           0 \\           0 \\           0 \\           g         \end{bmatrix} + \vec{w}_k         $$          or more compactly as $$\vec{s}_k = \boldsymbol{A}\vec{s}_{k-1} + \boldsymbol{B}\vec{u}_k + \vec{w}_k$$ where $\vec{s}_k = \begin{bmatrix} x_k & y_k & \dot{x}_k & \dot{y}_k\end{bmatrix}^T$ is the internal state vector. Since we are measuring the position we can also write the following observation equation:$$\vec{z}_k = \begin{bmatrix}1 & 0 & 0 & 0\\0 & 1 & 0 & 0\end{bmatrix} \cdot\begin{bmatrix}           x_k \\           y_k \\           \dot{x}_k \\           \dot{y}_k         \end{bmatrix} + \vec{v}_k$$or more compactly as $$\vec{z}_k = \boldsymbol{C}\vec{s}_k + \vec{v}_k$$The process and measurement noise, $\vec{w}_k$ and $\vec{v}_k$ are assumed to obey Gaussian distribution functions.  The real trajectoryFor the "real" trajectory we advance the system using a very small time-step. In code this looks something like follows:<jupyter_code># gravitational force
g = 9.81

# time steps
ts = 1./1000 # 1000 Hz
time = np.arange(0,3,ts)

# state matrix
A = np.diag(np.ones(4))+np.diag(ts*np.ones(2),2)

# input-to-state matrix
B = np.eye(4); B[-1,-1] = ts

# input
u = np.asarray([0,0,0,-g])

# state-to-output matrix
C = np.array([[1,0,0,0],[0,1,0,0]])

              
# state and measurement arrays
x = np.empty((4,len(time)))
y = np.empty((2,len(time)))

# inital state
v0 = 20 # initial velocity
alpha = np.pi/4. # angle at which we throw
x[:,0] = np.asarray([0,0,v0*np.cos(alpha),v0*np.sin(alpha)])
y[:,0] = x[0:2,0]

# simulation
for i in range(1,len(time)):
    x[:,i] = A.dot(x[:,i-1]) + B.dot(u)
    y[:,i] = C.dot(x[:,i])

real_trajectory = y
plt.plot(real_trajectory[0,:],real_trajectory[1,:],'-',label="real trajectory")
plt.xlabel(r"$x$-position",fontsize=16)
plt.ylabel(r"$y$-position",fontsize=16)
plt.title("Flight of a ball",fontsize=16)
plt.show()<jupyter_output><empty_output><jupyter_text>MeasurementsFor the measurement data we will take the "real" data and perturb it with some Gaussian noise. In reality the measurements would be obtained from our camera system and algorithms estimating the flight of the ball.<jupyter_code>observation_noise = 0.8
measurements = y[0,::10] + np.random.normal(0,observation_noise,y[0,::10].shape)

plt.plot(measurements[:],'o',label="measurements")
plt.xlabel(r"sample",fontsize=16)
plt.ylabel(r"$x$-position",fontsize=16)
plt.title("Flight of a ball",fontsize=16)
plt.show()<jupyter_output><empty_output><jupyter_text>The filtered trajectory For the filtered trajectory we choose a filter rate matching the frequency of measurements and once again setup the state and input-to-state matrices:<jupyter_code>ts = 1./100 # 100 Hz
ftime = np.arange(0,3,ts)

A = np.diag(np.ones(4))+np.diag(ts*np.ones(2),2)
B = np.eye(4); B[-1,-1] = ts
C = np.array([1,0,0,0])
print(A)
print(B)
print(u)<jupyter_output>[[1.   0.   0.01 0.  ]
 [0.   1.   0.   0.01]
 [0.   0.   1.   0.  ]
 [0.   0.   0.   1.  ]]
[[1.   0.   0.   0.  ]
 [0.   1.   0.   0.  ]
 [0.   0.   1.   0.  ]
 [0.   0.   0.   0.01]]
[ 0.    0.    0.   -9.81]<jupyter_text>Next we require an estimate of the initial position and its covariance:<jupyter_code>xf = np.empty((4,len(ftime)))
yf = np.empty((2,len(ftime)))

x_init = np.array([0.,10.,1.,1.])
xf[:,0] = x_init<jupyter_output><empty_output><jupyter_text>Additionally we need to initialize the covariance of the estimation error as well as the modeling and error noise covariance matrices:<jupyter_code>evolution_noise = 0.01
Q = evolution_noise*np.eye(4)
P = A.dot(np.eye(4).dot(A.T))
R = observation_noise*np.eye(1)<jupyter_output><empty_output><jupyter_text>Finally, we can step the filtered system forward in time:<jupyter_code>for i in range(1,len(ftime)):

    # Prediction step
    # ---------------------
    # Project state and error covariance
    xtemp = A.dot(xf[:,i-1]) + B.dot(u)
    P = A.dot(P).dot(A.T) + Q

    # Correction step
    # ---------------------
    # Compute Kalman gain
    K = P.dot(C).dot(np.linalg.pinv(C.dot(P).dot(C) + R))
    # Update estimate via measurement
    xf[:,i] = xtemp + K.dot(measurements[i] - C.dot(xtemp))
    # Update error covariance
    P = P - K.dot(C).dot(P)<jupyter_output><empty_output><jupyter_text>The only remaining step is now to plot the filtered and measured outputs for comparison:<jupyter_code>plt.plot(real_trajectory[0,:],real_trajectory[1,:],'-',label="real trajectory")
plt.plot(measurements[0,:],measurements[1,:],'.',label="measurements")
plt.plot(xf[0,:],xf[1,:],'-',label="filtered")

plt.title("Filtered flight of a ball")
plt.xlabel(r"$x$-position")
plt.ylabel(r"$y$-position")
plt.legend(loc=0)
plt.show()<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.17346938775510204, 'fraction_numerical': 0.03320375769355361, 'mean_word_length': 3.1166666666666667, 'pattern_counts': {'":': 0, '<': 28, '<?xml version=': 0, '>': 28, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 2, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '7', 'dir': 'jupyter-structured-clean-dedup', 'id': '552012', 'license': 'BSD-3-Clause', 'n_tokens_mistral': 2385, 'n_tokens_neox': 2234, 'n_words': 589, 'path': 'notebooks/support_content/02-Kalman_filtering.ipynb', 'repo_name': 'bgt-pat/ufib_workshop', 'size': '6174'}