Before we tackle the multidimensional Kalman Filter, we'll need to review some essential math topics:
You can jump to the next chapter if you are familiar with these topics.
All you need to know is basic terms and operations such as:
There are numerous Linear Algebra textbooks and web tutorials that cover these topics.
You can find a good tutorial on this topic on the visiondummy site:
https://www.visiondummy.com/2014/04/geometric-interpretation-covariance-matrix/
I extensively use the expectation algebra rules for Kalman Filter equations derivations. If you are interested in understanding the derivations, you need to master expectation algebra.
You already know what a random variable is and what an expected value (or expectation) is. If not, please read the previous background break page.
The expectation is denoted by the capital letter \( E \).
The expectation of the random variable \( E(X) \) equals the mean of the random variable:
Here are some basic expectation rules:
| Rule | Notes | |
|---|---|---|
| 1 | \( E(X) = \mu_{X} = \Sigma xp(x) \) | \( p(x) \) is the probability of \( x \) (discrete case) |
| 2 | \( E(a) = a \) | \( a \) is constant |
| 3 | \( E(aX) = aE(X) \) | \( a \) is constant |
| 4 | \( E(a \pm X) = a \pm E(X) \) | \( a \) is constant |
| 5 | \( E(a \pm bX) = a \pm bE(X) \) | \( a \) and \( b \) are constant |
| 6 | \( E(X \pm Y) = E(X) \pm E(Y) \) | \( Y \) is another random variable |
| 7 | \( E(XY) = E(X)E(Y) \) | If \( X \) and \( Y \) are independent |
The following table includes the variance and covariance expectation rules.
| Rule | Notes | |
|---|---|---|
| 8 | \( V(a) = 0 \) | \( V(a) \) is the variance of \( a \) \( a \) is constant |
| 9 | \( V(a \pm X) = V(X) \) | \( V(X) \) is the variance of \( X \) \( a \) is constant |
| 10 | \( V(X) = E(X^{2}) - \mu_{X}^{2} \) | \( V(X) \) is the variance of \( X \) |
| 11 | \( COV(X,Y) = E(XY) - \mu_{X}\mu_{Y} \) | \( COV(X,Y) \) is a covariance of \( X \) and \( Y \) |
| 12 | \( COV(X,Y) = 0 \) | if \( X \) and \( Y \) are independent |
| 13 | \( V(aX) = a^{2}V(X) \) | \( a \) is constant |
| 14 | \( V(X \pm Y) = V(X) + V(Y) \pm 2COV(X,Y) \) | |
| 15 | \( V(XY) \neq V(X)V(Y) \) |
The variance and covariance expectation rules are not straightforward. I prove some of them.
\[ V(a) = 0 \]
A constant does not vary, so the variance of a constant is 0.
\[ V(a \pm X) = V(X) \]
Adding a constant to the variable does not change its variance.
\[ V(X) = E(X^{2}) - \mu_{X}^{2} \]
The proof:
| Notes | |
|---|---|
| \( V(X) = \sigma_{X}^2 = E((X - \mu_{X})^2) = \) | |
| \( E(X^2 -2X\mu_{X} + \mu_{X}^2) = \) | |
| \( E(X^2) - E(2X\mu_{X}) + E(\mu_{X}^2) = \) | Applied rule number 5: \( E(a \pm bX) = a \pm bE(X) \) |
| \( E(X^2) - 2\mu_{X}E(X) + E(\mu_{X}^2) = \) | Applied rule number 3: \( E(aX) = aE(X) \) |
| \( E(X^2) - 2\mu_{X}E(X) + \mu_{X}^2 = \) | Applied rule number 2: \( E(a) = a \) |
| \( E(X^2) - 2\mu_{X}\mu_{X} + \mu_{X}^2 = \) | Applied rule number 1: \( E(X) = \mu_{X} \) |
| \( E(X^2) - \mu_{X}^2 \) |
\[ COV(X,Y) = E(XY) - \mu_{X}\mu_{Y} \]
The proof:
| Notes | |
|---|---|
| \( COV(X,Y) = E((X - \mu_{X})(Y - \mu_{Y}) \) = | |
| \( E(XY - X \mu_{Y} - Y \mu_{X} + \mu_{X}\mu_{Y}) = \) | |
| \( E(XY) - E(X \mu_{Y}) - E(Y \mu_{X}) + E(\mu_{X}\mu_{Y}) = \) | Applied rule number 6: \( E(X \pm Y) = E(X) \pm E(Y) \) |
| \( E(XY) - \mu_{Y} E(X) - \mu_{X} E(Y) + E(\mu_{X}\mu_{Y}) = \) | Applied rule number 3: \( E(aX) = aE(X) \) |
| \( E(XY) - \mu_{Y} E(X) - \mu_{X} E(Y) + \mu_{X}\mu_{Y} = \) | Applied rule number 2: \( E(a) = a \) |
| \( E(XY) - \mu_{Y} \mu_{X} - \mu_{X} \mu_{Y} + \mu_{X}\mu_{Y} = \) | Applied rule number 1: \( E(X) = \mu_{X} \) |
| \( E(XY) - \mu_{X}\mu_{Y} \) |
\[ V(aX) = a^{2}V(X) \]
The proof:
| Notes | |
|---|---|
| \( V(K) = \sigma_{K}^2 = E(K^{2}) - \mu_{K}^2 \) | |
| \( K = aX \) | |
| \( V(K) = V(aX) = E((aX)^{2} ) - (a \mu_{X})^{2} = \) | Substitute \( K \) by \( aX \) |
| \( E(a^{2}X^{2}) - a^{2} \mu_{X}^{2} = \) | |
| \( a^{2}E(X^{2}) - a^{2}\mu_{X}^{2} = \) | Applied rule number 3: \( E(aX) = aE(X) \) |
| \( a^{2}(E(X^{2}) - \mu_{X}^{2}) = \) | |
| \( a^{2}V(X) \) | Applied rule number 10: \( V(X) = E(X^{2}) - \mu_{X}^2 \) |
For constant velocity motion:
| \( x \) | is the displacement of the body |
| \( v \) | is the velocity of the body |
| \( \Delta t \) | is the time interval |
\[ V(X \pm Y) = V(X) + V(Y) \pm 2COV(X,Y) \]
The proof:
| Notes | |
|---|---|
| \( V(X \pm Y) = \) | |
| \( E((X \pm Y)^{2}) - (\mu_{X} \pm \mu_{Y})^{2} = \) | Applied rule number 10: \( V(X) = E(X^{2}) - \mu_{X}^2 \) |
| \( E(X^{2} \pm 2XY + Y^{2}) - (\mu_{X}^2 \pm 2\mu_{X}\mu_{Y} + \mu_{y}^2) = \) | |
| \( \color{red}{E(X^{2}) - \mu_{X}^2} + \color{blue}{E(Y^{2}) - \mu_{Y}^2} \pm 2(E(XY) - \mu_{X}\mu_{Y} ) = \) | Applied rule number 6: \( E(X \pm Y) = E(X) \pm E(Y) \) |
| \( \color{red}{V(X)} + \color{blue}{V(Y)} \pm 2(E(XY) - \mu_{X}\mu_{Y} ) = \) | Applied rule number 10: \( V(X) = E(X^{2}) - \mu_{X}^2 \) |
| \( V(X) + V(Y) \pm 2COV(X,Y) \) | Applied rule number 11: \( COV(X,Y) = E(XY) - \mu_{X}\mu_{Y} \) |
Assume vector \( \boldsymbol{x} \) with \( k \) elements:
\[ \boldsymbol{x} = \left[ \begin{matrix} x_{1}\\ x_{2}\\ \vdots \\ x_{k}\\ \end{matrix} \right] \]
The covariance matrix of the vector \( \boldsymbol{x} \) is given by:
The proof:
\[ COV(\boldsymbol{x}) = E \left( \left[ \begin{matrix} (x_{1} - \mu_{x_{1}})^{2} & (x_{1} - \mu_{x_{1}})(x_{2} - \mu_{x_{2}}) & \cdots & (x_{1} - \mu_{x_{1}})(x_{k} - \mu_{x_{k}}) \\ (x_{2} - \mu_{x_{2}})(x_{1} - \mu_{x_{1}}) & (x_{2} - \mu_{x_{2}})^{2} & \cdots & (x_{2} - \mu_{x_{2}})(x_{k} - \mu_{x_{k}}) \\ \vdots & \vdots & \ddots & \vdots \\ (x_{k} - \mu_{x_{k}})(x_{1} - \mu_{x_{1}}) & (x_{k} - \mu_{x_{k}})(x_{2} - \mu_{x_{2}}) & \cdots & (x_{k} - \mu_{x_{k}})^{2} \\ \end{matrix} \right] \right) = \]
\[ = E \left( \left[ \begin{matrix} (x_{1} - \mu_{x_{1}}) \\ (x_{2} - \mu_{x_{2}}) \\ \vdots \\ (x_{k} - \mu_{x_{k}}) \\ \end{matrix} \right] \left[ \begin{matrix} (x_{1} - \mu_{x_{1}}) & (x_{2} - \mu_{x_{2}}) & \cdots & (x_{k} - \mu_{x_{k}}) \end{matrix} \right] \right) = \]
\[ = E \left( \left( \boldsymbol{x} - \boldsymbol{\mu}_{x} \right) \left( \boldsymbol{x} - \boldsymbol{\mu}_{x} \right)^{T} \right) \]