diff --git a/doc/book/kalman_filter.Rmd b/doc/book/kalman_filter.Rmd index 08746ab06..1f8e202d7 100644 --- a/doc/book/kalman_filter.Rmd +++ b/doc/book/kalman_filter.Rmd @@ -13,7 +13,7 @@ predicts the next state of the object but it also provides how wrong we could be in our prediction. How wrong we could be is quantified in terms of covariance matrix? -So when we predict the next state of the object, we actually predict a Gaussian -distribution. +So when we predict the next state of the object, we actually predict a +**Gaussian distribution** at each time step. Let's start reminding ourselves the equations. diff --git a/doc/book/kalman_filter/theory.Rmd b/doc/book/kalman_filter/theory.Rmd index bc4b924a1..ae82efe24 100644 --- a/doc/book/kalman_filter/theory.Rmd +++ b/doc/book/kalman_filter/theory.Rmd @@ -62,9 +62,8 @@ where \begin{equation} \mathbf{H}_k = \begin{bmatrix} - \mathbf{I}_4 & \mathbf{0}_{4 \times 8} \\ - \mathbf{0}_{8 \times 4} & \mathbf{0}_{8 \times 8} \\ -\end{bmatrix} + \mathbf{I}_4 & \mathbf{0}_{4 \times 8} + \end{bmatrix} \end{equation} is called the observation model in the Kalman filter @@ -102,6 +101,18 @@ obtain is from the temporal integration as in classical Newtonian mechanics: \frac{1}{2} \ddot{\mathbf{b}}_k (t_{k+1} - t_k)^2 \end{equation} +And so the matrix $\mathbf{F}_k$ is + +\begin{equation} +\mathbf{F}_k = \begin{bmatrix} + \mathbf{I}_4 & \Delta_k \mathbf{I}_4 & \frac{\Delta_k^2}{2} \mathbf{I}_4 \\ + \mathbf{0}_4 & \mathbf{I}_4 & \Delta_k \mathbf{I}_4 \\ + \mathbf{0}_4 & \mathbf{0}_4 & \mathbf{I}_4 + \end{bmatrix}, +\end{equation} +where $\Delta_k = t_{k+1} - t_k$. + + Unless we are driving a car autonomously trying to keep a minimum distance with every pedestrian in a busy street, we can ignore $\mathbf{B}_k$ by setting it to $\mathbf{0}$.