# Kalman Filter
Until now, we dealed with ***ideal*** conditions, such as the ***complete
absence of noise***.

Let's now put ourselves in in this position:
$$
\begin{cases}
\dot{x}(t) = Ax(t) + Bu(t) + Gw(t) \\
      y(t) = C{x}(t) + Du(t) + v(t)
\end{cases}
$$

- $w(t)$ : Process Noise
- $v(t)$ : Measurement Noise

## Analyzing the noise[^unimore]
Usually the Expectation of Gaussian Nose is $0$, but if that's not the case, 
we just need to shift coordinates.

> [!NOTE]
> $w_k$ and $v_k$ are $w(t)$ and $v(t)$ with $t = k$
>

> [!TIP]
> if $E[X] = 0$ and $E[Y] = 0$, since $Cov(X,Y) = E[(X - E[X])(Y - E[Y])]$, 
> then $Cov(X,Y) = E[XY]$

- $E[w_k] = E[v_k] = 0 \rightarrow$ If this is not true, we can shift 
coordinates
- $E[w_k w_q^T] = \begin{cases}R_w\; \text{if} \;q = k \\0\; \text{if} \;q \neq k\end{cases}$
- $E[v_k v_q^T] = \begin{cases}R_v\; \text{if} \;q = k \\0\; \text{if} \;q \neq k\end{cases}$
- $E[w_k v_q^T] = 0 \;\forall k,q$

In particular, $v_k$ and $w_k$ are incorrelated between 
themselves, their states in time and the initial state 
$x(0)$

Since our state is affected by noise, we make an estimation of it, and it will be affected by an error:
$$
\hat{x}_k = x_k + e_k \leftrightarrow e_k = \hat{x}_k - x_k
$$

The error is measured as $e_k^T e_k = \sum_i e_i^2 \in \R$

Now we define some extra errors. The `a priori` happens 
before measuring the output, the `a posteriori` happens 
just right after:
$$
\begin{align*}
e_k^+ &= \hat{x}_k^+ - x_k = e_k\rightarrow 
\text{error a posteriori} \\


e_k^- &= \hat{x}_k^- - x_k = \epsilon_k \rightarrow 
\text{error a priori} \\


\hat{x}_k^+ &= \hat{x}_k^- - L_k(C\hat{x}_k^- - y_k) 
\rightarrow \text{Observed value} \\

\end{align*}

$$

Now, let's expand the formula:

$$
\begin{align*}
e_k^+ &= e_k = \hat{x}_k^+ - x_k = \\


&= \hat{x}_k^- - L_k(C\hat{x}_k^- - y_k) - x_k = \\


&= \hat{x}_k^- - x_k - L_k(C\hat{x}_k^- - y_k)  = \\


&= \epsilon_k - L_k(C\hat{x}_k^- - y_k)  = \\


&= \epsilon_k - L_k(C\hat{x}_k^- - Cx_k -v_k)  = \\


&= \epsilon_k - L_k(C(\epsilon_k) -v_k)  = \\


&= \epsilon_k - L_k C \epsilon_k -Lv_k  = \\


&= \epsilon_k - L_k C \epsilon_k -Lv_k  = \\


&= (I - L_k C\textcolor{red}{_k} )\epsilon_k -L
\textcolor{red}{_k}v_k 
\end{align*} \\

\text{Now let's compute } Cov(e_k^+)\\

\begin{align*}
Cov(e_k^+) &= E[e_k e_k^T] = \\


&= E \{[(I - L_k C_k )\epsilon_k -L_kv_k] [(I - L_k C_k )
\epsilon_k -L_kv_k ]^T\} = \\


&= E \{[(I - L_k C_k )\epsilon_k -L_kv_k] 
[\epsilon_k^T (I - L_k C_k )^T -v_k^T L_k^T ]\} = \\


&= E [
    (I - L_k C_k )\epsilon_k \epsilon_k^T (I - L_k C_k )^T -
    (I - L_k C_k )\epsilon_k v_k^T L_k^T -
    L_k v_k \epsilon_k^T (I - L_k C_k )^T +
    L_k v_k v_k^T L_k^T
]\rightarrow \\


&\text{Noises and errors are not correlated } \\


&E[
    (I - L_k C_k )\epsilon_k \epsilon_k^T (I - L_k C_k )^T 
] = (I - L_kC_k) P_k^- (I - L_kC_k)^T\\
&-E[
(I - L_k C_k )\epsilon_k v_k^T L_k^T 
] = 0 \rightarrow 
\text{
Remember that only $\epsilon_k$ and $v_k$ are random variables
}\\
&-E[
    L_k v_k \epsilon_k^T (I - L_k C_k )^T
] = 0 \rightarrow 
\text{
Remember that only $\epsilon_k$ and $v_k$ are random variables
}\\
&E[
    L_k v_k v_k^T L_k^T
] = L_k R_v L_k^T \rightarrow \\

P_k^+ &= (I - L_kC_k) P_k^- (I - L_kC_k)^T + L_k R_v L_k^T
\end{align*}
$$

We can see that our cost is 
$E[e_k^T e_k] = \sum_i e_i^2 = trace(P_k^+)$.
To minimize this we derive over $L_k$ and equal to $0$, as this will be our 
`minimum point` (assuming `Hessian` defined ***semi-positive***)

$$
\begin{align*}
\frac{d\,tr(P_k^+)}{dL_k} &= -2(I - L_kC_k)P_k^- C_k + 2L_kR_v 
= 0 \rightarrow\\

&\rightarrow  (-I +L_kC_k)P_k^- C_k^T + L_kR_v =0 \rightarrow\\


&\rightarrow  -P_k^- C_k^T +L_kC_k P_k^- C_k^T + L_kR_v =0 \rightarrow\\


&\rightarrow  L_kC_k P_k^- C_k^T + L_kR_v = + P_k^- C_k^T\rightarrow\\


&\rightarrow  L_k (C_k P_k^- C_k^T + R_v) =  P_k^- C_k^T\rightarrow\\


&\rightarrow  L_k  =  P_k^- C_k^T 
(\underbrace{C_k P_k^- C_k^T + R_v}_{S_k})^{-1}

\end{align*}
$$

Substitute for this minimum value:
> [!WARNING]
> You just need to expand this equationto get to the final solution
$$
\begin{align*}
P_k^+ &= (I - L_kC_k) P_k^- (I - L_kC_k)^T + L_k R_v L_k^T = \\


&= (I - C_kP_k^- C_k^T S_k^{-1}) P_k^- (I - P_k^- C_k^T S_k^{-1}C_k)^T + P_k^- 
C_k^T S_k^{-1}
R_v (P_k^- C_k^T S_k^{-1})^T = \\


&= (P_k^- - C_kP_k^- C_k^T S_k^{-1}P_k^-) (I - P_k^- C_k^T S_k^{-1}C_k)^T + P_k^- 
C_k^T S_k^{-1}
R_v (P_k^- C_k^T S_k^{-1})^T= \\


&= \dots = (I - L_kC_K)P_k^- \rightarrow \\


\rightarrow L_kC_kP_k^- &= P_k^- - P_k^+ \rightarrow \\


\rightarrow 
L_kC_kP_k^-C_k^T &= (P_k^- - P_k^+)C_k^T =- L_kR_v + P_k^-C_k^T 
\rightarrow \\

\rightarrow P_k^+C_k^T &= L_kR_v \rightarrow \\
\rightarrow L_k &=  P_k^+C_k^TR_v^{-1}\\

&\dots \\

P_K^- &= AP_{k-1}^+A^T + GR_wG^T 

\end{align*}
$$

### The actual Algorithm
0. $P_0^- = P_0$

Loop:
1. $P_k^+ = P_k^- - P_k^-C^T_k(C_kP^-_kC_k^T + R_v)^{-1}C_kP_k^-$
2. $\hat{x}_k^+ = \hat{x}_k^- + P_k^+C^T_kR_v^{-1}(y_k - C_k\hat{x}_k^-)$
3. $\hat{x}_{k+1}^- = A\hat{x}_k^+ + Bu_k$
4. $P_{k+1}^- = AP_k^+A^T + GR_wG^T$



[^unimore]: [Unimore | 14 January 2025](http://www.dii.unimore.it/~lbiagiotti/MaterialeTSC1516/TSC-03-KalmanFilter.pdf)