Date: January 22, 2024

Topic: Kalman Filters

Recall

We use the state transition matrix $F$ to estimate the new position and speed of the object.

For example, since the position is dependent on the previous position and speed, the new $x$ position is $x'= x + \dot x \Delta t$. The same can be seen for $y$.

Notes

Predicting the Next Position

With $x$,$y$ being the location and $\dot{x},\dot{y}$ being the velocity (hidden) - assuming a constant velocity

$$ \bar{x} = \begin{bmatrix} x \\ y \\ \dot{x} \\ \dot{y} \end{bmatrix},

F = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} $$

When a prediction is performed, uncertainty increases. This is represented by $P$ which is the uncertainty encoded in the covariance matrix

Uncertainty

When we do a prediction, there will be an increase in uncertainty ($t_k$ to $t_{k+1}$)

$Q$ is used to include errors inherent in the model (e.g., due to truncation, etc)

After movement and measurement, we get our new position but it includes some uncertainty $R$

By taking the new observation into our calculation, we reduce the uncertainty of the estimate. 2 Gaussians that multiply will have a results in reduced uncertainty

Measurements and Predictions

Measurement Step (Observations)

• Where based on our initial position and some measurements, we get our measured position with some uncertainty.
• $\bar z$ is the new location and $R$ is the uncertainty regarding $x$ and $y$

$$ \bar z = \begin{bmatrix} x \\ y \end{bmatrix}, R = \begin{bmatrix} \sigma_x^2 & 0 \\ 0 & \sigma_y^2 \end{bmatrix} $$

Correcting Prediction (Observation Update)

Red is predicted, blue is actual and z is where we move the prediction after the observation

• $\bar x$: state vector of dimension (n), e.g., $[x,y,\dot x, \dot y]^T$ where $n=4$, $\hat{\bar{x}}$ is the predicted state
• $\bar z$: observation vector of dimension (m), e.g., $[x_m, y_m]^T$ where $m=2$.
• A hat indicates the predicted state of a quantity
• A bar is the observation state
• $K$ is the Kalman gain, which is the ratio of the uncertainty of the previous measurement to the total uncertainty in the measurement + observation ($K=\frac{\hat\sigma_{x}^2}{\hat\sigma^2_x + \sigma^2_m}$)
• Use $K$ to to compute the weighing of the difference of the measurement $\bar z_{k+1}$ and what we expect the measurement to be $H\bar x_{k+1}$ to move the measurement direction
• Then, the covariance $P$ is updated with $P_{k+1} = (I-KH)\hat P_{k+1}$
• In a properly tuned Kalman filter, the covariance should always drop when there is a measurement update

<aside> 📌 SUMMARY: Explanation of the terms used in the update process of Kalman filters

</aside>