Mathematical Steps to Obtain the KF Algorithm

0. Basic Knowledge and Notation:

1. PREDICTION STEP: Define the problem We could also write as our objective function the LMSE (Least Mean Square Error), but it will bring the exact same result. For simplicity we will define: And since we are searching for the LMSE estimator (or similar, the result will still be the LSME estimator): We substitute $x(t+1)$ We use the previous definition of $\tilde{x}$: We remove the uncorrelated part: So only the products of (1)$\times$(1)${}^T$ and (2)$\times$(2)${}^T$ remains: The variance is always positive, so by choosing $\hat{x}(t|t)$ such that the first component is $0$ is the best possible result, after all the second component only depends on $w(t)$ which I cannot change. Now let’s see how $P(t+1|t)$ changes, starting from its definition as the covariance matrix of the error on the prediction $\hat{x}(t+1|t)$: Knowing that since $w(t)$ is a white process: Then: We can rewrite it as we have already seen as the Discrete Lyapunov Equation:

2. CORRECTION STEP: Define the problem: Where:

• $y_1$ is the measurement vector obtained at time $t=1$
• $y_2$ is the measurement vector obtained at time $t=2$

We already know the solution of this problem: Now we need to use all the information we got: We know that $Y$ is divided in 2 parts $y_1$ and $y_2$, so: We now make a strong assumption, we say that $y_1$ and $y_2$ are uncorrelated, unfortunately in most cases this is not the case: The inverse of a diagonal matrix is: We solve the matrix product: Here we can say somenting: But unfortunately, this works only if $y_1$ and $y_2$ are uncorrelated:

Let’s now define the so called Innovation Process: The idea behind it is that we want to correct using only the new information contained in $y_2$, so ignore from $y_2$ the old information, already seen in $y_1$.

We can see the innovation process as the error of what the sensor measured $y(t)$ (so the “almost” ground truth, whit an added noise $v(t)$) and what our model predicted: $C\hat{x}(t+1|t)$

Doing some simple substitutions we have that the innovation process is equal to:

It also has some nice properties: We say that $\hat{x}(t+1|t)$ is a linear combination of $Y^t$, because we know that the mathematical model $f$ is linear and as we can see from $\hat{x}(t+1|t)$ it is a prediction of $x(t+1)$ that depends on the measurement $y(t)$ up to time $t$, ergo it’s a linear combination of $Y^t$.

Now we can define the “update” process as:

Since $e(t+1)$ is uncorrelated with $Y^t$:

Where: By the definition of covariance: Like we did previously: We study how the double product interact with each other: And we have that: We can “delete” the last two terms: And we have that $R_{x(t+1)e(t+1)}$ is equal to:

We calculate also the variance of $e(t+1)$: (Like before we can simplify because $v(t)$ is a White Process)

So we can say that

$$\hat{x}(t+1|t+1)=\hat{x}(t+1|t+1)+R_{x(t+1)e(t+1)}\cdot R_{e(t+1)}\cdot \left(e(t+1)-E\left[e(t+1)\right]\right)$$

is equivalent to:

We define the Kalman Gain:

Such that:

We have seen how $\hat{x}(t+1|t+1)$ evolves, now let’s see how $P(t+1|t+1)$ evolves, starting as always by its definition: We substitute the just found $\hat{x}(t+1|t+1)$: Then we substitute $x-\hat{x}=\tilde{x}$, and $y=Cx+v$ : Simple multiplication, and substitution for $x-\hat{x}=\tilde{x}$ We group $\tilde{x}$: Like before:

• The covariance of something (different from $v(t)$) multiplied by $v(t)$ is zero since $v$ is a White Process.
• The variance of $v(t)$ is $R(t)$, as defined in the formulation of the problem.
• The covariance of $\tilde{x}(a,|b)$ is $P(a,|b)$, from the definition of $P$
• Also note that $I$, $K(t+1)$ and $C$ are not stochastic.

We can simplify the formula, first we develop the matrix multiplication: Then we simplify:

Finally we obtain the last formula for the KF algorithm: Can also be written as: 3. Iterate